From 0515af6f3b08eac1a7b2f29707eb183368a903db Mon Sep 17 00:00:00 2001 From: "Ana C. Huaman Quispe" Date: Wed, 5 Jun 2024 21:55:46 -0500 Subject: [PATCH 1/3] Added initial changes for ns. Still failing in the spawn --- astrobee/launch/astrobee.launch.py | 73 +++++++++++++--------- simulation/launch/spawn_astrobee.launch.py | 10 +-- 2 files changed, 48 insertions(+), 35 deletions(-) diff --git a/astrobee/launch/astrobee.launch.py b/astrobee/launch/astrobee.launch.py index fd07c61a47..a79fca6230 100644 --- a/astrobee/launch/astrobee.launch.py +++ b/astrobee/launch/astrobee.launch.py @@ -17,7 +17,8 @@ from utilities.utilities import * - +from launch.actions import GroupAction +from launch_ros.actions import PushRosNamespace def generate_launch_description(): robot_description = Command(['xacro ', get_path('urdf/model.urdf.xacro', 'description'), @@ -63,7 +64,7 @@ def generate_launch_description(): # Set the TF prefix, create a robot description and joint state publisher Node( package="robot_state_publisher", - namespace="", + namespace=LaunchConfiguration("ns"), executable="robot_state_publisher", name="robot_state_publisher", parameters=[ @@ -88,34 +89,41 @@ def generate_launch_description(): ), # LLP -# -# -# - IncludeLaunchDescription( - get_launch_file("launch/robot/LLP.launch.py"), - launch_arguments={ - "drivers": LaunchConfiguration("drivers"), # Don't start driver nodes - "spurn" : LaunchConfiguration("spurn"), # Prevent node - "nodes" : LaunchConfiguration("nodes"), # Launch node group - "extra" : LaunchConfiguration("extra"), # Inject extra nodes - "debug" : LaunchConfiguration("debug"), # Debug a node set - "output" : LaunchConfiguration("output"), - "gtloc" : LaunchConfiguration("gtloc"), # Use Ground Truth Localizer - }.items(), - condition=LaunchConfigurationNotEquals("llp", "disabled"), + # + # + # + GroupAction( + actions=[ + PushRosNamespace(LaunchConfiguration('ns')), + IncludeLaunchDescription( + get_launch_file("launch/robot/LLP.launch.py"), + launch_arguments={ + "drivers": LaunchConfiguration("drivers"), # Don't start driver nodes + "spurn" : LaunchConfiguration("spurn"), # Prevent node + "nodes" : LaunchConfiguration("nodes"), # Launch node group + "extra" : LaunchConfiguration("extra"), # Inject extra nodes + "debug" : LaunchConfiguration("debug"), # Debug a node set + "output" : LaunchConfiguration("output"), + "gtloc" : LaunchConfiguration("gtloc"), # Use Ground Truth Localizer + }.items(), + condition=LaunchConfigurationNotEquals("llp", "disabled"), + ) + ] ), - # MLP -# -# -# - IncludeLaunchDescription( - get_launch_file("launch/robot/MLP.launch.py"), - launch_arguments={ + # + # + # + GroupAction( + actions=[ + PushRosNamespace(LaunchConfiguration('ns')), + IncludeLaunchDescription( + get_launch_file("launch/robot/MLP.launch.py"), + launch_arguments={ "drivers": LaunchConfiguration("drivers"), # Don't start driver nodes "spurn" : LaunchConfiguration("spurn"), # Prevent node "nodes" : LaunchConfiguration("nodes"), # Launch node group @@ -123,9 +131,12 @@ def generate_launch_description(): "debug" : LaunchConfiguration("debug"), # Debug a node set "output" : LaunchConfiguration("output"), "gtloc" : LaunchConfiguration("gtloc"), # Use Ground Truth Localizer - }.items(), - condition=LaunchConfigurationNotEquals("llp", "disabled"), - ), + }.items(), + condition=LaunchConfigurationNotEquals("llp", "disabled"), + ) + ] + ) + ]) diff --git a/simulation/launch/spawn_astrobee.launch.py b/simulation/launch/spawn_astrobee.launch.py index 91c253e8ca..012fdc23d5 100644 --- a/simulation/launch/spawn_astrobee.launch.py +++ b/simulation/launch/spawn_astrobee.launch.py @@ -44,9 +44,10 @@ def generate_launch_description(): executable='spawn_entity.py', name='spawn_astrobee', output='screen', - arguments=["-topic", "/robot_description", "-entity", "bsharp", "-timeout", "30.0", + arguments=["-entity", "bsharp", "-timeout", "30.0", "-x", LaunchConfiguration("x"), "-y", LaunchConfiguration("y"), "-z", LaunchConfiguration("z"), - "-R", LaunchConfiguration("R"), "-P", LaunchConfiguration("P"), "-Y", LaunchConfiguration("Y")], + "-R", LaunchConfiguration("R"), "-P", LaunchConfiguration("P"), "-Y", LaunchConfiguration("Y"), + "-stdin", LaunchConfiguration("robot_description")], condition=LaunchConfigurationEquals("ns", "") ), Node( @@ -54,9 +55,10 @@ def generate_launch_description(): executable='spawn_entity.py', name='spawn_astrobee', output='screen', - arguments=["-topic", "/robot_description", "-entity", LaunchConfiguration("ns"), "-timeout", "30.0", + arguments=["-entity", LaunchConfiguration("ns"), "-timeout", "30.0", "-x", LaunchConfiguration("x"), "-y", LaunchConfiguration("y"), "-z", LaunchConfiguration("z"), - "-R", LaunchConfiguration("R"), "-P", LaunchConfiguration("P"), "-Y", LaunchConfiguration("Y")], + "-R", LaunchConfiguration("R"), "-P", LaunchConfiguration("P"), "-Y", LaunchConfiguration("Y"), + "-stdin", LaunchConfiguration("robot_description")], condition=LaunchConfigurationNotEquals("ns", "") ) From 7cbb0fafde7dfc96e8aa49f20a04c0eef85e80de Mon Sep 17 00:00:00 2001 From: Ana Huaman Date: Thu, 6 Jun 2024 01:37:24 -0500 Subject: [PATCH 2/3] Added spawn for ns and no-ns --- simulation/launch/spawn_astrobee.launch.py | 43 +++++++++++----------- 1 file changed, 22 insertions(+), 21 deletions(-) diff --git a/simulation/launch/spawn_astrobee.launch.py b/simulation/launch/spawn_astrobee.launch.py index 012fdc23d5..2637ef2192 100644 --- a/simulation/launch/spawn_astrobee.launch.py +++ b/simulation/launch/spawn_astrobee.launch.py @@ -32,34 +32,35 @@ def read_pose(context, *args, **kwargs): DeclareLaunchArgument('Y', default_value=pose[5]), ] -def generate_launch_description(): - return LaunchDescription([ - DeclareLaunchArgument("ns", default_value=""), # Robot namespace - DeclareLaunchArgument("pose"), # Robot pose - OpaqueFunction(function=read_pose), - DeclareLaunchArgument("robot_description"), # Robot description +def launch_setup(context, *args, **kwargs): + ns = str( (LaunchConfiguration('ns').perform(context)) ) + + topic = "/robot_description" + entity = "bsharp" - Node( + if ns: + topic = "/" + ns + topic + entity = ns + + spawn_entity = Node( package='gazebo_ros', executable='spawn_entity.py', name='spawn_astrobee', output='screen', - arguments=["-entity", "bsharp", "-timeout", "30.0", + arguments=["-topic", topic, "-entity", entity, "-timeout", "30.0", "-x", LaunchConfiguration("x"), "-y", LaunchConfiguration("y"), "-z", LaunchConfiguration("z"), - "-R", LaunchConfiguration("R"), "-P", LaunchConfiguration("P"), "-Y", LaunchConfiguration("Y"), - "-stdin", LaunchConfiguration("robot_description")], + "-R", LaunchConfiguration("R"), "-P", LaunchConfiguration("P"), "-Y", LaunchConfiguration("Y")], condition=LaunchConfigurationEquals("ns", "") - ), - Node( - package='gazebo_ros', - executable='spawn_entity.py', - name='spawn_astrobee', - output='screen', - arguments=["-entity", LaunchConfiguration("ns"), "-timeout", "30.0", - "-x", LaunchConfiguration("x"), "-y", LaunchConfiguration("y"), "-z", LaunchConfiguration("z"), - "-R", LaunchConfiguration("R"), "-P", LaunchConfiguration("P"), "-Y", LaunchConfiguration("Y"), - "-stdin", LaunchConfiguration("robot_description")], - condition=LaunchConfigurationNotEquals("ns", "") ) + + return [spawn_entity] +def generate_launch_description(): + + return LaunchDescription([ + DeclareLaunchArgument("ns", default_value=""), # Robot namespace + DeclareLaunchArgument("pose"), # Robot pose + OpaqueFunction(function=read_pose), + DeclareLaunchArgument("robot_description"), # Robot description + OpaqueFunction(function=launch_setup) ]) From 4a35aa88f9c91e12f66781888f7b36b5965ee5ce Mon Sep 17 00:00:00 2001 From: "Ana C. Huaman Quispe" Date: Thu, 6 Jun 2024 10:22:09 -0500 Subject: [PATCH 3/3] Works for namespaced bumble and non-ns robot --- astrobee/resources/rviz/iss_ros2.rviz | 156 +++++++++++---------- behaviors/dock/src/dock_component.cc | 3 +- management/executive/src/executive.cc | 3 +- simulation/launch/spawn_astrobee.launch.py | 6 +- 4 files changed, 89 insertions(+), 79 deletions(-) diff --git a/astrobee/resources/rviz/iss_ros2.rviz b/astrobee/resources/rviz/iss_ros2.rviz index 5d01a1d146..ea1d4f419b 100644 --- a/astrobee/resources/rviz/iss_ros2.rviz +++ b/astrobee/resources/rviz/iss_ros2.rviz @@ -7,8 +7,9 @@ Panels: - /Global Options1 - /Status1 - /Debug1 + - /Dock Cam1 Splitter Ratio: 0.5 - Tree Height: 415 + Tree Height: 433 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -35,81 +36,81 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true - ar_tag: + bumble/ar_tag: Value: true - body: + bumble/body: Value: true - dock/berth1: + bumble/dock_cam: Value: true - dock/berth1/approach: + bumble/flashlight_aft: Value: true - dock/berth1/complete: + bumble/flashlight_front: Value: true - dock/berth2: + bumble/handrail/approach: Value: true - dock/berth2/approach: + bumble/handrail/body: Value: true - dock/berth2/complete: + bumble/handrail/complete: Value: true - dock/body: + bumble/haz_cam: Value: true - dock_cam: + bumble/imu: Value: true - flashlight_aft: + bumble/inertial_link: Value: true - flashlight_front: + bumble/laser: Value: true - granite/body: + bumble/nav_cam: Value: true - handrail/approach: + bumble/payload/bottom_aft: Value: true - handrail/body: + bumble/payload/bottom_front: Value: true - handrail/complete: + bumble/payload/top_aft: Value: true - haz_cam: + bumble/payload/top_front: Value: true - imu: + bumble/perch_cam: Value: true - inertial_link: + bumble/sci_cam: Value: true - iss/body: + bumble/top_aft: Value: true - laser: + bumble/top_aft_arm_distal_link: Value: true - nav_cam: + bumble/top_aft_arm_proximal_link: Value: true - payload/bottom_aft: + bumble/top_aft_gripper_left_distal_link: Value: true - payload/bottom_front: + bumble/top_aft_gripper_left_proximal_link: Value: true - payload/top_aft: + bumble/top_aft_gripper_right_distal_link: Value: true - payload/top_front: + bumble/top_aft_gripper_right_proximal_link: Value: true - perch/body: + bumble/truth: Value: true - perch_cam: + dock/berth1: Value: true - rviz: + dock/berth1/approach: Value: true - sci_cam: + dock/berth1/complete: Value: true - top_aft: + dock/berth2: Value: true - top_aft_arm_distal_link: + dock/berth2/approach: Value: true - top_aft_arm_proximal_link: + dock/berth2/complete: Value: true - top_aft_gripper_left_distal_link: + dock/body: Value: true - top_aft_gripper_left_proximal_link: + granite/body: Value: true - top_aft_gripper_right_distal_link: + iss/body: Value: true - top_aft_gripper_right_proximal_link: + perch/body: Value: true - truth: + rviz: Value: true world: Value: true @@ -120,39 +121,48 @@ Visualization Manager: Show Names: false Tree: world: - body: - ar_tag: + bumble/body: + bumble/ar_tag: {} - dock_cam: + bumble/dock_cam: {} - flashlight_aft: + bumble/flashlight_aft: {} - flashlight_front: + bumble/flashlight_front: {} - haz_cam: + bumble/haz_cam: {} - imu: + bumble/imu: {} - inertial_link: + bumble/inertial_link: {} - laser: + bumble/laser: {} - nav_cam: + bumble/nav_cam: {} - payload/bottom_aft: + bumble/payload/bottom_aft: {} - payload/bottom_front: + bumble/payload/bottom_front: {} - payload/top_aft: + bumble/payload/top_aft: {} - payload/top_front: + bumble/payload/top_front: {} - perch_cam: + bumble/perch_cam: {} - sci_cam: - {} - top_aft: + bumble/sci_cam: {} + bumble/top_aft: + bumble/top_aft_arm_proximal_link: + bumble/top_aft_arm_distal_link: + bumble/top_aft_gripper_left_proximal_link: + bumble/top_aft_gripper_left_distal_link: + {} + bumble/top_aft_gripper_right_proximal_link: + bumble/top_aft_gripper_right_distal_link: + {} + bumble/truth: + {} dock/body: dock/berth1: dock/berth1/approach: @@ -172,8 +182,6 @@ Visualization Manager: {} rviz: {} - truth: - {} Update Interval: 0 Value: true - Class: rviz_common/Group @@ -339,7 +347,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /robot_description + Value: /honey/robot_description Enabled: false Links: All Links Enabled: true @@ -351,7 +359,7 @@ Visualization Manager: Inertia: false Mass: false Name: Honey - TF Prefix: honey + TF Prefix: "" Update Interval: 0 Value: false Visual Enabled: true @@ -365,7 +373,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /robot_description + Value: /bumble/robot_description Enabled: false Links: All Links Enabled: true @@ -377,7 +385,7 @@ Visualization Manager: Inertia: false Mass: false Name: Bumble - TF Prefix: bumble + TF Prefix: "" Update Interval: 0 Value: false Visual Enabled: true @@ -391,7 +399,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /robot_description + Value: /queen/robot_description Enabled: false Links: All Links Enabled: true @@ -403,7 +411,7 @@ Visualization Manager: Inertia: false Mass: false Name: Queen - TF Prefix: queen + TF Prefix: "" Update Interval: 0 Value: false Visual Enabled: true @@ -527,16 +535,16 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 2.0104706287384033 + Distance: 2.5802009105682373 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 9.056347846984863 - Y: 10.248156547546387 - Z: -2.718514919281006 + X: 8.70544147491455 + Y: 8.735550880432129 + Z: -2.141327142715454 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -552,12 +560,12 @@ Window Geometry: collapsed: false Dock Cam: collapsed: false - Height: 1016 + Height: 1043 Hide Left Dock: false Hide Right Dock: false Nav Cam: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000025a0000035efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000228000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000100044006f0063006b002000430061006d0100000269000000850000002800fffffffb0000000e004e00610076002000430061006d01000002f4000000a50000002800ffffff000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000003c30000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016700000379fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000023a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000100044006f0063006b002000430061006d010000027b000000890000002800fffffffb0000000e004e00610076002000430061006d010000030a000000aa0000002800ffffff000000010000010f00000379fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000379000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004fe0000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -566,6 +574,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1848 - X: 72 - Y: 27 + Width: 1920 + X: 0 + Y: 0 diff --git a/behaviors/dock/src/dock_component.cc b/behaviors/dock/src/dock_component.cc index fe7dd0ac02..c6db7ee98c 100644 --- a/behaviors/dock/src/dock_component.cc +++ b/behaviors/dock/src/dock_component.cc @@ -780,7 +780,8 @@ class DockComponent : public ff_util::FreeFlyerComponent { msg.header.stamp = GetTimeNow(); // ConfigClient blocks when Setting parameters. Creating a new node prevent this - std::shared_ptr temp_node = std::make_shared("temporal_dock_node", "", rclcpp::NodeOptions().use_global_arguments(false)); + std::string platform = this->GetPlatform(); + std::shared_ptr temp_node = std::make_shared("temporal_dock_node", platform, rclcpp::NodeOptions().use_global_arguments(false)); ff_util::ConfigClient cfg(temp_node, NODE_CHOREOGRAPHER); // Set parameters for the choreographer diff --git a/management/executive/src/executive.cc b/management/executive/src/executive.cc index b17182fe41..25d2d0eb0a 100644 --- a/management/executive/src/executive.cc +++ b/management/executive/src/executive.cc @@ -1196,7 +1196,8 @@ bool Executive::ConfigureLed( bool Executive::ConfigureMobility(bool move_to_start, std::string& err_msg) { // Initialize choreographer config client if it hasn't been initialized if (!choreographer_cfg_) { - cfg_node_ = std::make_shared("executive_cfg_node", "", rclcpp::NodeOptions().use_global_arguments(false)); + std::string platform = this->GetPlatform(); + cfg_node_ = std::make_shared("executive_cfg_node", platform, rclcpp::NodeOptions().use_global_arguments(false)); choreographer_cfg_ = std::make_shared(cfg_node_, NODE_CHOREOGRAPHER); } diff --git a/simulation/launch/spawn_astrobee.launch.py b/simulation/launch/spawn_astrobee.launch.py index 2637ef2192..26991aa270 100644 --- a/simulation/launch/spawn_astrobee.launch.py +++ b/simulation/launch/spawn_astrobee.launch.py @@ -33,7 +33,7 @@ def read_pose(context, *args, **kwargs): ] def launch_setup(context, *args, **kwargs): - ns = str( (LaunchConfiguration('ns').perform(context)) ) + ns = str( (LaunchConfiguration("ns").perform(context)) ) topic = "/robot_description" entity = "bsharp" @@ -49,8 +49,8 @@ def launch_setup(context, *args, **kwargs): output='screen', arguments=["-topic", topic, "-entity", entity, "-timeout", "30.0", "-x", LaunchConfiguration("x"), "-y", LaunchConfiguration("y"), "-z", LaunchConfiguration("z"), - "-R", LaunchConfiguration("R"), "-P", LaunchConfiguration("P"), "-Y", LaunchConfiguration("Y")], - condition=LaunchConfigurationEquals("ns", "") + "-R", LaunchConfiguration("R"), "-P", LaunchConfiguration("P"), "-Y", LaunchConfiguration("Y"), + "-robot_namespace", LaunchConfiguration("ns")] ) return [spawn_entity]