Publishes pose. We were missing the CLOCK from gazebo!

traclabs · Nov 8, 2024 · 058c193 · 058c193
1 parent cac6d08
commit 058c193
Show file tree

Hide file tree

Showing 31 changed files with 588 additions and 466 deletions.
diff --git a/astrobee/launch/astrobee.launch.py b/astrobee/launch/astrobee.launch.py
@@ -113,7 +113,7 @@ def generate_launch_description():
                   "output" : LaunchConfiguration("output"), 
                   "gtloc"  : LaunchConfiguration("gtloc"),    # Use Ground Truth Localizer
                 }.items(),
-                condition=LaunchConfigurationNotEquals("llp", "disabled"),
+                condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("llp"), "disabled")),
               )                      
             ]
         ),
@@ -137,7 +137,7 @@ def generate_launch_description():
                 "output" : LaunchConfiguration("output"), 
                 "gtloc"  : LaunchConfiguration("gtloc"),   # Use Ground Truth Localizer
               }.items(),
-              condition=LaunchConfigurationNotEquals("llp", "disabled"),
+              condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("mlp"), "disabled")),
               )
             ]
         )

diff --git a/astrobee/launch/controller/descriptions.launch.py b/astrobee/launch/controller/descriptions.launch.py
@@ -40,7 +40,7 @@ def generate_launch_description():
             name="astrobee_state_publisher",
             parameters=[{'robot_description': ParameterValue(granite_robot_description) }],
             arguments=[granite_urdf],
-            condition=LaunchConfigurationEquals("world", "granite")
+            condition=IfCondition(EqualsSubstitution(LaunchConfiguration("world"), "granite"))
         ),
         # Granite robot description
         Node(
@@ -50,7 +50,7 @@ def generate_launch_description():
             name="astrobee_state_publisher",
             parameters=[{'robot_description': ParameterValue(iss_robot_description) }],
             arguments=[iss_urdf],
-            condition=LaunchConfigurationEquals("world", "iss")
+            condition=IfCondition(EqualsSubstitution(LaunchConfiguration("world"), "iss"))
         ),
         # Granite robot description
         Node(

diff --git a/astrobee/launch/controller/sim_start.launch.py b/astrobee/launch/controller/sim_start.launch.py
@@ -22,7 +22,7 @@ def generate_launch_description():
 
     return LaunchDescription([
         DeclareLaunchArgument("world", default_value="iss"),     # Robot namespace
-        DeclareLaunchArgument("sviz",    default_value="true"),    # Robot pose
+        DeclareLaunchArgument("sviz",    default_value="false"),    # Robot pose
         DeclareLaunchArgument("vmware",  default_value="true"),    # Robot description
         DeclareLaunchArgument("speed",   default_value="1"),       # Robot description
         DeclareLaunchArgument("debug",   default_value="false"),   # Robot description

diff --git a/astrobee/launch/controller/synthetic.launch.py b/astrobee/launch/controller/synthetic.launch.py
@@ -25,25 +25,25 @@ def generate_launch_description():
     #     <!-- Connect and update environment variables if required -->
     #     <machine unless="$(eval arg('sim')=='local')" name="sim_server" default="true"
     #              address="$(arg sim)" user="astrobee" password="astrobee" timeout="10"/>
-    if LaunchConfigurationNotEquals("bag", ""):
+    if IfCondition(EqualsSubstitution(LaunchConfiguration("bag"), "")):
         return LaunchDescription([
             DeclareLaunchArgument("ns"),                    # Robot namespace
             DeclareLaunchArgument("pose"),                  # Robot pose
             DeclareLaunchArgument("robot_description"),     # Robot description
             #   <group if="$(eval arg('bag')=='')" >
             SetEnvironmentVariable(name="ASTROBEE_CONFIG_DIR",
                                    value="/home/astrobee/native/config",
-                                   condition=LaunchConfigurationNotEquals("sim", "local")),
+                                   condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))),
             SetEnvironmentVariable(name="ASTROBEE_RESOURCE_DIR",
                                    value=os.getenv("ASTROBEE_RESOURCE_DIR", get_path("/resources")),
-                                   condition=LaunchConfigurationNotEquals("sim", "local")),
+                                   condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))),
             SetEnvironmentVariable(name="ROSCONSOLE_CONFIG_FILE",
                                    value=os.getenv("ROSCONSOLE_CONFIG_FILE", get_path("/resources/logging.config")),
-                                   condition=LaunchConfigurationNotEquals("sim", "local")),
+                                   condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))),
             SetEnvironmentVariable(name="DISPLAY", value=":0",
-                                   condition=LaunchConfigurationNotEquals("sim", "local")),
+                                   condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))),
             SetEnvironmentVariable(name="ROS_IP", value=LaunchConfiguration("sim"),
-                                   condition=LaunchConfigurationNotEquals("sim", "local")),
+                                   condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))),
 
             IncludeLaunchDescription(
                 get_launch_file("launch/spawn_astrobee.launch.py", "astrobee_gazebo"),

diff --git a/astrobee/launch/robot/LLP.launch.py b/astrobee/launch/robot/LLP.launch.py
@@ -22,19 +22,19 @@
 def generate_launch_description():
 
     return LaunchDescription([
-        SetEnvironmentVariable(name="ASTROBEE_ROBOT",         condition=LaunchConfigurationNotEquals("llp", "local"),
+        SetEnvironmentVariable(name="ASTROBEE_ROBOT",         condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("llp"), "local")),
                                value=os.getenv("ASTROBEE_ROBOT", LaunchConfiguration("robot"))),
-        SetEnvironmentVariable(name="ASTROBEE_WORLD",         condition=LaunchConfigurationNotEquals("llp", "local"),
+        SetEnvironmentVariable(name="ASTROBEE_WORLD",         condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("llp"), "local")),
                                value=os.getenv("ASTROBEE_WORLD", LaunchConfiguration("world"))),
-        SetEnvironmentVariable(name="ASTROBEE_CONFIG_DIR",    condition=LaunchConfigurationNotEquals("llp", "local"),
+        SetEnvironmentVariable(name="ASTROBEE_CONFIG_DIR",    condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("llp"), "local")),
                                value=os.getenv("ASTROBEE_CONFIG_DIR", "/opt/astrobee/config")),
-        SetEnvironmentVariable(name="ASTROBEE_RESOURCE_DIR",  condition=LaunchConfigurationNotEquals("llp", "local"),
+        SetEnvironmentVariable(name="ASTROBEE_RESOURCE_DIR",  condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("llp"), "local")),
                                value=os.getenv("ASTROBEE_RESOURCE_DIR", "/res")),
-        SetEnvironmentVariable(name="ROSCONSOLE_CONFIG_FILE", condition=LaunchConfigurationNotEquals("llp", "local"),
+        SetEnvironmentVariable(name="ROSCONSOLE_CONFIG_FILE", condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("llp"), "local")),
                                value=os.getenv("ROSCONSOLE_CONFIG_FILE", "/res/logging.config")),
 
 
-        SetEnvironmentVariable(name="ROS_HOSTNAME", condition=LaunchConfigurationNotEquals("llp", "local"),
+        SetEnvironmentVariable(name="ROS_HOSTNAME", condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("llp"), "local")),
                                value=LaunchConfiguration("llp")),
 
 

diff --git a/astrobee/launch/robot/MLP.launch.py b/astrobee/launch/robot/MLP.launch.py
@@ -23,18 +23,18 @@ def generate_launch_description():
 
     return LaunchDescription([
         # Update the environment variables relating to absolute paths
-        SetEnvironmentVariable(name="ASTROBEE_ROBOT",         condition=LaunchConfigurationNotEquals("mlp", "local"),
+        SetEnvironmentVariable(name="ASTROBEE_ROBOT",         condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("mlp"), "local")),
                                value=os.getenv("ASTROBEE_ROBOT", LaunchConfiguration("robot"))),
-        SetEnvironmentVariable(name="ASTROBEE_WORLD",         condition=LaunchConfigurationNotEquals("mlp", "local"),
+        SetEnvironmentVariable(name="ASTROBEE_WORLD",         condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("mlp"), "local")),
                                value=os.getenv("ASTROBEE_WORLD", LaunchConfiguration("world"))),
-        SetEnvironmentVariable(name="ASTROBEE_CONFIG_DIR",    condition=LaunchConfigurationNotEquals("mlp", "local"),
+        SetEnvironmentVariable(name="ASTROBEE_CONFIG_DIR",    condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("mlp"), "local")),
                                value=os.getenv("ASTROBEE_CONFIG_DIR", "/opt/astrobee/config")),
-        SetEnvironmentVariable(name="ASTROBEE_RESOURCE_DIR",  condition=LaunchConfigurationNotEquals("mlp", "local"),
+        SetEnvironmentVariable(name="ASTROBEE_RESOURCE_DIR",  condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("mlp"), "local")),
                                value=os.getenv("ASTROBEE_RESOURCE_DIR", "/res")),
-        SetEnvironmentVariable(name="ROSCONSOLE_CONFIG_FILE", condition=LaunchConfigurationNotEquals("mlp", "local"),
+        SetEnvironmentVariable(name="ROSCONSOLE_CONFIG_FILE", condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("mlp"), "local")),
                                value=os.getenv("ROSCONSOLE_CONFIG_FILE", "/res/logging.config")),
 
-        SetEnvironmentVariable(name="ROS_HOSTNAME", condition=LaunchConfigurationNotEquals("mlp", "local"),
+        SetEnvironmentVariable(name="ROS_HOSTNAME", condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("mlp"), "local")),
                                value=LaunchConfiguration("mlp")),
 
 
@@ -74,7 +74,8 @@ def generate_launch_description():
             #     remappings=[('/image', '/burgerimage')],
             #     parameters=[{'history': 'keep_last'}],
             #     extra_arguments=[{'use_intra_process_comms': True}])
-            ]
+            ],
+            output=LaunchConfiguration("output")
         ),
         ComposableNodeContainer(
         name='mlp_localization',
@@ -96,7 +97,8 @@ def generate_launch_description():
             #     remappings=[('/image', '/burgerimage')],
             #     parameters=[{'history': 'keep_last'}],
             #     extra_arguments=[{'use_intra_process_comms': True}])
-            ]
+            ],
+        output=LaunchConfiguration("output")    
         ),
         ComposableNodeContainer(
         name='mlp_graph_localization',
@@ -231,7 +233,8 @@ def generate_launch_description():
                 name='mapper',
                 parameters=[{'use_sim_time': True}],
                 extra_arguments=[{'use_intra_process_comms': False}]),
-            ]
+            ],
+        output=LaunchConfiguration("output")    
         ),
         ComposableNodeContainer(
         name='mlp_management',

diff --git a/astrobee/launch/sim.launch.py b/astrobee/launch/sim.launch.py
@@ -61,9 +61,9 @@ def generate_launch_description():
                            condition=IfCondition(LaunchConfiguration("perch"))),
         # Default is using JPM Berth 1, for Berth 2 use: '9.817 -10.312 4.293 1 0 0 0'
         launch_arg("pose", default_value="9.816 -9.806 4.293 0 0 0",
-                           condition=LaunchConfigurationEquals("world", "iss")),
+                           condition=IfCondition(EqualsSubstitution(LaunchConfiguration("world"), "iss"))),
         launch_arg("pose", default_value="0 0 -0.7 0 0 0",
-                           condition=LaunchConfigurationEquals("world", "granite")),
+                           condition=IfCondition(EqualsSubstitution(LaunchConfiguration("world"), "granite"))),
         # Multi-robot simulation
         launch_arg("honey", default_value="false", description="Insert honey robot"),
         launch_arg("bumble", default_value="false", description="Insert bumble robot"),
@@ -125,7 +125,7 @@ def generate_launch_description():
         # Launch a recorder for this robot
         IncludeLaunchDescription(
             get_launch_file("launch/controller/bagrecord.launch.py"),
-            condition=LaunchConfigurationNotEquals("rec", ""),
+            condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("rec"), "")),
             launch_arguments={"bag": LaunchConfiguration("rec")}.items(),
         ),
         #   <!-- Allow the simulator to be optionally launched remotely-->
@@ -135,18 +135,18 @@ def generate_launch_description():
         # Update the environment variables relating to absolute paths
         SetEnvironmentVariable(name="ASTROBEE_CONFIG_DIR",
                                value=os.getenv("ASTROBEE_CONFIG_DIR", "/home/astrobee/native/config"),
-                               condition=LaunchConfigurationNotEquals("sim", "local")),
+                               condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))),
         SetEnvironmentVariable(name="ASTROBEE_RESOURCE_DIR",
                                value=os.getenv("ASTROBEE_RESOURCE_DIR", "/home/astrobee/native/resources"),
-                               condition=LaunchConfigurationNotEquals("sim", "local")),
+                               condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))),
         SetEnvironmentVariable(name="ROSCONSOLE_CONFIG_FILE",
                                value=os.getenv("ROSCONSOLE_CONFIG_FILE", "/home/astrobee/native/resources/logging.config"),
-                               condition=LaunchConfigurationNotEquals("sim", "local")),
+                               condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))),
 
         SetEnvironmentVariable(name="DISPLAY", value=":0",
-                               condition=LaunchConfigurationNotEquals("sim", "local")),
+                               condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))),
         SetEnvironmentVariable(name="ROS_IP", value=LaunchConfiguration("sim"),
-                               condition=LaunchConfigurationNotEquals("sim", "local")),
+                               condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))),
         # Start the simulator
         IncludeLaunchDescription(
             get_launch_file("launch/controller/sim_start.launch.py"),

diff --git a/astrobee/launch/spawn.launch.py b/astrobee/launch/spawn.launch.py
@@ -25,9 +25,9 @@ def generate_launch_description():
         DeclareLaunchArgument("robot", default_value=os.getenv("ASTROBEE_ROBOT", "sim")),
         DeclareLaunchArgument("world", default_value=os.getenv("ASTROBEE_WORLD", "iss")),
         DeclareLaunchArgument("pose",  default_value="9.92 -9.54 4.50 0 0 0",
-                                       condition=LaunchConfigurationEquals("world", "iss")),
+                                       condition=IfCondition(EqualsSubstitution(LaunchConfiguration("world"), "iss"))),
         DeclareLaunchArgument("pose",  default_value="9.92 -9.54 4.50 0 0 0",
-                                       condition=LaunchConfigurationEquals("world", "granite")),
+                                       condition=IfCondition(EqualsSubstitution(LaunchConfiguration("world"), "granite"))),
 
         # Make sure all environment variables are set for controller
         # Override the robot and world environment variables all the time. The

diff --git a/astrobee/utilities/utilities.py b/astrobee/utilities/utilities.py
@@ -29,8 +29,6 @@
 )
 from launch.conditions import (
     IfCondition,
-    LaunchConfigurationEquals,
-    LaunchConfigurationNotEquals,
     UnlessCondition,
 )
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -41,6 +39,8 @@
     LaunchConfiguration,
     PythonExpression,
     TextSubstitution,
+    EqualsSubstitution,
+    NotEqualsSubstitution
 )
 from launch_ros.actions import ComposableNodeContainer, Node
 from launch_ros.descriptions import ComposableNode, ParameterValue

diff --git a/description/description/urdf/macro_perching_arm.urdf.xacro b/description/description/urdf/macro_perching_arm.urdf.xacro
@@ -58,7 +58,7 @@
       </collision>
     </link>
     <joint
-      name="${bay}"
+      name="${bay}_fixed_joint"
       type="fixed">
       <origin
         xyz="${xyz}"
@@ -381,26 +381,30 @@
         velocity="0.12" />
     </joint>
     <xacro:if value="${bay == 'top_aft'}">
-      <gazebo>
-        <plugin name="top_aft_perching_arm_ros" filename="libgazebo_model_plugin_perching_arm.so">
+      <!--gazebo>
+        <plugin 
+          name="astrobee_gazebo::GazeboModelPluginPerchingArm" 
+          filename="gazebo_model_plugin_perching_arm">
           <rate>10.0</rate>
           <bay>top_aft</bay>
           <proximal>1.57079632679</proximal>
           <distal>0</distal>
           <gripper>0</gripper>
         </plugin>
-      </gazebo>
+      </gazebo>-->
     </xacro:if>
     <xacro:if value="${bay == 'bot_aft'}">
-      <gazebo>
-        <plugin name="bot_aft_perching_arm_ros" filename="libgazebo_model_plugin_perching_arm.so">
+      <!--<gazebo>
+        <plugin 
+          name="astrobee_gazebo::GazeboModelPluginPerchingArm" 
+          filename="gazebo_model_plugin_perching_arm">
           <rate>10.0</rate>
           <bay>bot_aft</bay>
           <proximal>1.57079632679</proximal>
           <distal>0</distal>
           <gripper>0</gripper>
         </plugin>
-      </gazebo>
+      </gazebo>-->
     </xacro:if>
   </xacro:macro>
 </robot>
diff --git a/description/description/urdf/model.urdf.xacro b/description/description/urdf/model.urdf.xacro
@@ -114,23 +114,23 @@
 
   <!-- MODEL -->
   <xacro:include filename="$(find description)/urdf/model_drag.urdf.xacro" />
-  <xacro:include filename="$(find description)/urdf/model_pmc.urdf.xacro" />
+<!--  <xacro:include filename="$(find description)/urdf/model_pmc.urdf.xacro" />-->
   <xacro:include filename="$(find description)/urdf/model_eps.urdf.xacro" />
   <xacro:include filename="$(find description)/urdf/model_truth.urdf.xacro" />
-  <xacro:include filename="$(find description)/urdf/model_laser.urdf.xacro" />
+<!--  <xacro:include filename="$(find description)/urdf/model_laser.urdf.xacro" />
   <xacro:include filename="$(find description)/urdf/model_flashlights.urdf.xacro" />
   <xacro:include filename="$(find description)/urdf/model_speed_cam.urdf.xacro" />
-  <xacro:include filename="$(find description)/urdf/model_signal_lights.urdf.xacro" />
+  <xacro:include filename="$(find description)/urdf/model_signal_lights.urdf.xacro" />-->
 
   <!-- Monitors -->
   <xacro:include filename="$(find description)/urdf/model_monitors.urdf.xacro" />
 
   <!-- SENSORS -->
-  <xacro:include filename="$(find description)/urdf/sensor_imu.urdf.xacro" />
+  <!--<xacro:include filename="$(find description)/urdf/sensor_imu.urdf.xacro" />
   <xacro:include filename="$(find description)/urdf/sensor_nav_cam.urdf.xacro" />
   <xacro:include filename="$(find description)/urdf/sensor_dock_cam.urdf.xacro" />
   <xacro:include filename="$(find description)/urdf/sensor_haz_cam.urdf.xacro" />
-  <xacro:include filename="$(find description)/urdf/sensor_perch_cam.urdf.xacro" />
+  <xacro:include filename="$(find description)/urdf/sensor_perch_cam.urdf.xacro" />-->
 
   <!-- PERCHING ARM -->
   <xacro:include filename="$(find description)/urdf/macro_perching_arm.urdf.xacro" />

diff --git a/description/description/urdf/model_drag.urdf.xacro b/description/description/urdf/model_drag.urdf.xacro
@@ -19,10 +19,13 @@
 
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
   <gazebo>
-    <plugin name="drag_ros" filename="libgazebo_model_plugin_drag.so">>
+    <plugin 
+      name="astrobee_gazebo::GazeboModelPluginDrag" 
+      filename="gazebo_model_plugin_drag">
+      <plugin_name>drag_plugin</plugin_name>
       <coefficient>1.05</coefficient>  <!-- Drag coefficient -->
       <area>0.092903</area>            <!-- Surface area     -->
       <density>1.225</density>         <!-- Air density      -->
     </plugin>
   </gazebo>
-</robot>
+</robot>
diff --git a/description/description/urdf/model_eps.urdf.xacro b/description/description/urdf/model_eps.urdf.xacro
@@ -19,7 +19,10 @@
 
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
   <gazebo>
-    <plugin name="eps_ros" filename="libgazebo_model_plugin_eps.so">
+    <plugin 
+      name="astrobee_gazebo::GazeboModelPluginEps" 
+      filename="gazebo_model_plugin_eps">
+      <plugin_name>eps_plugin</plugin_name>
       <rate>10.0</rate>                    <!-- Rate at which dock state checked  -->
       <distance_near>0.04</distance_near>  <!-- Threshold distance for magnetism  -->
       <distance_far>0.05</distance_far>    <!-- Threshold distance for magnetism  -->
@@ -29,5 +32,14 @@
       <battery_bottom_left>false</battery_bottom_left>
       <battery_bottom_right>false</battery_bottom_right>
     </plugin>
+
+    <plugin 
+        filename="gz-sim-detachable-joint-system"
+        name="gz::sim::systems::DetachableJoint">
+        <parent_link>body</parent_link>
+        <child_model>dock</child_model>
+        <child_link>body</child_link>
+    </plugin>
+
   </gazebo>
 </robot>