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] 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]