Added the props for each animal

uf-mil · Sep 22, 2024 · 75b181e · 75b181e
1 parent 751141c
commit 75b181e
Show file tree

Hide file tree

Showing 14 changed files with 729 additions and 10 deletions.
diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/2024_wildlife_mission.py b/NaviGator/mission_control/navigator_missions/navigator_missions/2024_wildlife_mission.py
@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+import numpy as np
+from enum import Enum
+from geometry_msgs.msg import Point
+from mil_misc_tools import ThrowingArgumentParser
+from mil_msgs.msg import ObjectsInImage
+from mil_msgs.srv import CameraToLidarTransform, CameraToLidarTransformRequest
+from mil_tools import rosmsg_to_numpy
+
+from std_srvs.srv import SetBoolRequest
+
+from .navigator import NaviGatorMission
+
+class MoveState(Enum):
+    NOT_STARTED = 1
+    RUNNING = 2
+    CANCELLED = 3
+    FINISHED = 4
+
+class Wildlife2024(NaviGatorMission):
+    animals_observed = {
+        "B_M" : False,    # Manatee => Counter clockwise
+        "G_I" : False,    # Iguana => Clockwise (by choice)
+        "R_P" : False     # Python => Clockwise
+    }
+
+    @classmethod
+    async def setup(cls):
+        cls.camsub = cls.nh.subscribe("/bbox_pub", ObjectsInImage)
+        await cls.camsub.setup()
+
+        cls.camera_lidar_tf = cls.nh.get_service_client(
+            "/wamv/sensors/camera/front_right_cam/image_raw",
+            CameraToLidarTransform,
+        )
+
+    @classmethod
+    async def shutdown(cls):
+        await cls.camsub.shutdown()
+
+    async def run(self, args):
+        # Check nearest objects
+
+        # Subscribe to the camera topic
+
+        # Gat access to PCODAR service
+        unknown_objects = await self.get_sorted_objects(name="UNKNOWN", n=3, throw=False)
+
+        # Associate Object to class
+
+            # Get the point cloud points associated with the object
+
+            # Map the points to the camera frame
+
+            # Classify by mode OR mean of frequent pixels 
+
+        # Go to buoy and circle accordingly
+        pass
diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/__init__.py b/NaviGator/mission_control/navigator_missions/navigator_missions/__init__.py
@@ -1,5 +1,8 @@
 import mil_missions_core
 
+# RobotX 2024 Missions
+from .2024_wildlife_mission import Wildlife2024
+
 from . import pose_editor
 from .back_and_forth import BackAndForth
 from .circle import Circle

diff --git a/NaviGator/simulation/navigator_gazebo/models/iguana_buoy/buoy.dae b/NaviGator/simulation/navigator_gazebo/models/iguana_buoy/buoy.dae
diff --git a/NaviGator/simulation/navigator_gazebo/models/iguana_buoy/buoy.sdf b/NaviGator/simulation/navigator_gazebo/models/iguana_buoy/buoy.sdf
@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<sdf version='1.0'>
+  <model name='iguana_green_buoy'>
+    <static>true</static>
+    <link name='base_link'>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <inertial>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <mass>1</mass>
+        <inertia>
+          <ixx>0.166667</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.166667</iyy>
+          <iyz>0</iyz>
+          <izz>0.166667</izz>
+        </inertia>
+      </inertial>
+      <collision name='base_link_collision'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>file://iguana_buoy/buoy.dae</uri>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name='base_link_visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>file://iguana_buoy/buoy.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>
diff --git a/NaviGator/simulation/navigator_gazebo/models/iguana_buoy/model.config b/NaviGator/simulation/navigator_gazebo/models/iguana_buoy/model.config
@@ -0,0 +1,13 @@
+<model>
+  <name>Python 2024 Buoy</name>
+  <version>1.0</version>
+  <sdf version='1.0'>buoy.sdf</sdf>
+
+  <author>
+    <name>Daniel Parra</name>
+  </author>
+
+  <description>
+    This is the model for Wildlife Task of RobotX 2024.
+  </description>
+</model>
diff --git a/NaviGator/simulation/navigator_gazebo/models/manatee_buoy/buoy.dae b/NaviGator/simulation/navigator_gazebo/models/manatee_buoy/buoy.dae
diff --git a/NaviGator/simulation/navigator_gazebo/models/manatee_buoy/buoy.sdf b/NaviGator/simulation/navigator_gazebo/models/manatee_buoy/buoy.sdf
@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<sdf version='1.0'>
+  <model name='manatee_blue_buoy'>
+    <static>true</static>
+    <link name='base_link'>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <inertial>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <mass>1</mass>
+        <inertia>
+          <ixx>0.166667</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.166667</iyy>
+          <iyz>0</iyz>
+          <izz>0.166667</izz>
+        </inertia>
+      </inertial>
+      <collision name='base_link_collision'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>file://manatee_buoy/buoy.dae</uri>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name='base_link_visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>file://manatee_buoy/buoy.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>
diff --git a/NaviGator/simulation/navigator_gazebo/models/manatee_buoy/model.config b/NaviGator/simulation/navigator_gazebo/models/manatee_buoy/model.config
@@ -0,0 +1,13 @@
+<model>
+  <name>Python 2024 Buoy</name>
+  <version>1.0</version>
+  <sdf version='1.0'>buoy.sdf</sdf>
+
+  <author>
+    <name>Daniel Parra</name>
+  </author>
+
+  <description>
+    This is the model for Wildlife Task of RobotX 2024.
+  </description>
+</model>
diff --git a/NaviGator/simulation/navigator_gazebo/models/python_buoy/buoy.dae b/NaviGator/simulation/navigator_gazebo/models/python_buoy/buoy.dae
diff --git a/NaviGator/simulation/navigator_gazebo/models/python_buoy/buoy.sdf b/NaviGator/simulation/navigator_gazebo/models/python_buoy/buoy.sdf
@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<sdf version='1.0'>
+  <model name='python_red_buoy'>
+    <static>true</static>
+    <link name='base_link'>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <inertial>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <mass>1</mass>
+        <inertia>
+          <ixx>0.166667</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.166667</iyy>
+          <iyz>0</iyz>
+          <izz>0.166667</izz>
+        </inertia>
+      </inertial>
+      <collision name='base_link_collision'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>file://python_buoy/buoy.dae</uri>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name='base_link_visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>file://python_buoy/buoy.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>
diff --git a/NaviGator/simulation/navigator_gazebo/models/python_buoy/model.config b/NaviGator/simulation/navigator_gazebo/models/python_buoy/model.config
@@ -0,0 +1,13 @@
+<model>
+  <name>Python 2024 Buoy</name>
+  <version>1.0</version>
+  <sdf version='1.0'>buoy.sdf</sdf>
+
+  <author>
+    <name>Daniel Parra</name>
+  </author>
+
+  <description>
+    This is the model for the python in  Wildlife Task of RobotX 2024.
+  </description>
+</model>
diff --git a/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_20m_triangle.world b/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_20m_triangle.world
@@ -181,19 +181,18 @@
       </ambient>
     </scene>
     <include>
-      <name>crocodile_buoy</name>
+      <uri>model://python_buoy</uri>
       <pose>-510 215 0 0 0 0</pose>
-      <uri>model://crocodile_buoy</uri>
     </include>
     <include>
-      <name>platypus_buoy</name>
+      <name>iguana_buoy</name>
       <pose>-490 215 0 0 0 0</pose>
-      <uri>model://platypus_buoy</uri>
+      <uri>model://iguana_buoy</uri>
     </include>
     <include>
-      <name>turtle_buoy</name>
+      <name>manatee_buoy</name>
       <pose>-500 233 0 0 0 0</pose>
-      <uri>model://turtle_buoy</uri>
+      <uri>model://manatee_buoy</uri>
     </include>
     <!-- The scoring plugin -->
     <plugin filename="libwildlife_scoring_plugin.so" name="wildlife_scoring_plugin">
@@ -215,17 +214,17 @@
       <!-- wildlife specific parameters -->
       <buoys>
         <buoy>
-          <model_name>crocodile_buoy</model_name>
+          <model_name>python_buoy</model_name>
           <link_name>link</link_name>
           <goal>avoid</goal>
         </buoy>
         <buoy>
-          <model_name>platypus_buoy</model_name>
+          <model_name>iguana_buoy</model_name>
           <link_name>link</link_name>
           <goal>circumnavigate_clockwise</goal>
         </buoy>
         <buoy>
-          <model_name>turtle_buoy</model_name>
+          <model_name>manatee_buoy</model_name>
           <link_name>link</link_name>
           <goal>circumnavigate_counterclockwise</goal>
         </buoy>

diff --git a/mil_common/axros b/mil_common/axros
diff --git a/ptk.txt b/ptk.txt
@@ -0,0 +1,2 @@
+35748
+41321
+2 −2		axros/src/axros/publisher.py
+2 −2		axros/src/axros/subscriber.py