Skip to content

Commit

Permalink
navigator_missions: Add TD feedback, color sequence derivation for wi…
Browse files Browse the repository at this point in the history
…ldlife
  • Loading branch information
cbrxyz committed Nov 9, 2024
1 parent df55de8 commit 5097dfc
Show file tree
Hide file tree
Showing 3 changed files with 41 additions and 5 deletions.
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
---
global_frame: enu
initial_pois:
circle_totems: [26.09821319580078, 59.91523361206055, 0.0]
wildlife: [26.09821319580078, 59.91523361206055, 0.0]
dock: [108.05, -1.07, 0.0]
entrance_gate: [51.28, -48.69, 0.0]
obstacle_course: [-32.803, -83.41, 0.0]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,10 +3,10 @@
from enum import Enum

import numpy as np
import rospy
from mil_msgs.msg import ObjectsInImage
from mil_msgs.srv import CameraToLidarTransform
from mil_tools import rosmsg_to_numpy
from navigator_msgs.srv import MessageWildlifeEncounter, MessageWildlifeEncounterRequest

from .navigator import NaviGatorMission

Expand Down Expand Up @@ -227,7 +227,7 @@ def get_indices_of_most_confident_animals(

async def find_wildlife(self):
robot_position = (await self.tx_pose())[0]
self.send_feedback("FINDING WILDLIFE")
self.send_feedback("[wildlife] FINDING WILDLIFE")

def filter_and_sort(objects, positions):
distances = np.linalg.norm(positions - robot_position, axis=1)
Expand Down Expand Up @@ -263,8 +263,26 @@ def is_done(objects, positions):
# Update explore dict
self.animals_observed[label] = True
if label == self.chosen_animal:
self.send_feedback(
f"[wildlife] starting circle of {self.chosen_animal}",
)
td_animal_labels = {
"red_python_buoy": "P",
"green_iguana_buoy": "I",
"blue_manatee_buoy": "M",
}
self.send_feedback("[wildlife] sending feedback")
await self.td_feedback(
MessageWildlifeEncounterRequest(
circling_wildlife=td_animal_labels.get(label, "P"),
clockwise=label == "blue_manatee_buoy",
number_of_circles=2 if label == "red_python_buoy" else 1,
),
)
self.send_feedback("[wildlife] sent feedback!")
await self.circle_animal(animal)
if self.chosen_animal == "red_python_buoy":
self.send_feedback("[wildlife] starting double circle of red")
await self.circle_animal(animal)

# # Check if all wildlife has been circled
Expand All @@ -283,7 +301,25 @@ def is_done(objects, positions):
async def run(self, args):
# Check nearest objects
self.objects_passed = set()
self.chosen_animal = rospy.get_param("chosen_animal", "red_python_buoy")
try:
self.color_sequence = await self.nh.get_param("color_sequence", str)
except TypeError:
print("invalid color sequence type")
self.color_sequence = None
if not self.color_sequence:
self.color_sequence = "RGB"
self.send_feedback("Invalid color sequence, defaulting to RGB")
animals = {
"R": "red_python_buoy",
"G": "green_iguana_buoy",
"B": "blue_manatee_buoy",
}
self.chosen_animal = animals.get(self.color_sequence[0], "red_python_buoy")
self.send_feedback(f"[wildlife] circling {self.chosen_animal}")
self.td_feedback = self.nh.get_service_client(
"/wildlife_encounter_message",
MessageWildlifeEncounter,
)
await self.change_wrench("autonomous")
# Wait a bit for PCDAR to get setup
# await self.set_classifier_enabled.wait_for_service()
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -158,7 +158,7 @@
<!-- Wind -->
<wind_direction>180</wind_direction>
<!-- in degrees -->
<wind_mean_velocity>8.0</wind_mean_velocity>
<wind_mean_velocity>4.0</wind_mean_velocity>
<var_wind_gain_constants>8.0</var_wind_gain_constants>
<var_wind_time_constants>20</var_wind_time_constants>
<random_seed>10</random_seed>
Expand Down

0 comments on commit 5097dfc

Please sign in to comment.