diff --git a/spot_driver/launch/spot_driver.launch.py b/spot_driver/launch/spot_driver.launch.py index e1562a504..8b07fd7fd 100644 --- a/spot_driver/launch/spot_driver.launch.py +++ b/spot_driver/launch/spot_driver.launch.py @@ -11,7 +11,7 @@ from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, TextSubstitution from launch_ros.substitutions import FindPackageShare -from spot_driver.launch.spot_launch_helpers import spot_has_arm +from spot_driver.launch.spot_launch_helpers import IMAGE_PUBLISHER_ARGS, declare_image_publisher_args, spot_has_arm THIS_PACKAGE = "spot_driver" @@ -137,16 +137,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: spot_image_publishers = IncludeLaunchDescription( PythonLaunchDescriptionSource([FindPackageShare(THIS_PACKAGE), "/launch", "/spot_image_publishers.launch.py"]), launch_arguments={ - key: LaunchConfiguration(key) - for key in [ - "config_file", - "depth_registered_mode", - "publish_point_clouds", - "uncompress_images", - "publish_compressed_images", - "stitch_front_images", - "spot_name", - ] + key: LaunchConfiguration(key) for key in ["config_file", "spot_name"] + IMAGE_PUBLISHER_ARGS }.items(), condition=IfCondition(LaunchConfiguration("launch_image_publishers")), ) @@ -193,57 +184,7 @@ def generate_launch_description() -> launch.LaunchDescription: description="Choose whether to launch the image publishing nodes from Spot.", ) ) - launch_args.append( - DeclareLaunchArgument( - "depth_registered_mode", - default_value="from_nodelets", - choices=["disable", "from_spot", "from_nodelets"], - description=( - "If `disable` is set, do not publish registered depth images." - " If `from_spot` is set, request registered depth images from Spot through its SDK." - " If `from_nodelets` is set, use depth_image_proc::RegisterNode component nodes running on the host" - " computer to create registered depth images (this reduces the computational load on Spot's internal" - " systems)." - ), - ) - ) - launch_args.append( - DeclareLaunchArgument( - "publish_point_clouds", - default_value="False", - choices=["True", "true", "False", "false"], - description=( - "If true, create and publish point clouds for each depth registered and RGB camera pair. Requires that" - " the depth_register_mode launch argument is set to a value that is not `disable`." - ), - ) - ) - launch_args.append( - DeclareLaunchArgument( - "uncompress_images", - default_value="True", - choices=["True", "true", "False", "false"], - description="Choose whether to publish uncompressed images from Spot.", - ) - ) - launch_args.append( - DeclareLaunchArgument( - "publish_compressed_images", - default_value="False", - choices=["True", "true", "False", "false"], - description="Choose whether to publish compressed images from Spot.", - ) - ) - launch_args.append( - DeclareLaunchArgument( - "stitch_front_images", - default_value="False", - choices=["True", "true", "False", "false"], - description=( - "Choose whether to publish a stitched image constructed from Spot's front left and right cameras." - ), - ) - ) + launch_args += declare_image_publisher_args() launch_args.append(DeclareLaunchArgument("spot_name", default_value="", description="Name of Spot")) ld = launch.LaunchDescription(launch_args) diff --git a/spot_driver/launch/spot_image_publishers.launch.py b/spot_driver/launch/spot_image_publishers.launch.py index 35adc3ec6..46b822d7f 100644 --- a/spot_driver/launch/spot_image_publishers.launch.py +++ b/spot_driver/launch/spot_image_publishers.launch.py @@ -1,7 +1,6 @@ # Copyright (c) 2023-2024 Boston Dynamics AI Institute LLC. All rights reserved. import os -from enum import Enum from typing import List import launch @@ -11,16 +10,12 @@ from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from spot_driver.launch.spot_launch_helpers import get_camera_sources, spot_has_arm - - -class DepthRegisteredMode(Enum): - DISABLE = "disable" - FROM_SPOT = "from_spot" - FROM_NODELETS = "from_nodelets" - - def __repr__(self) -> str: - return self.value +from spot_driver.launch.spot_launch_helpers import ( + DepthRegisteredMode, + declare_image_publisher_args, + get_camera_sources, + spot_has_arm, +) def create_depth_registration_nodelets( @@ -199,58 +194,8 @@ def generate_launch_description() -> launch.LaunchDescription: description="Path to configuration file for the driver.", ) ) - launch_args.append( - DeclareLaunchArgument( - "depth_registered_mode", - default_value="from_nodelets", - choices=["disable", "from_spot", "from_nodelets"], - description=( - "If `disable` is set, do not publish registered depth images." - " If `from_spot` is set, request registered depth images from Spot through its SDK." - " If `from_nodelets` is set, use depth_image_proc::RegisterNode component nodes running on the host" - " computer to create registered depth images (this reduces the computational load on Spot's internal" - " systems)." - ), - ) - ) - launch_args.append( - DeclareLaunchArgument( - "publish_point_clouds", - default_value="False", - choices=["True", "true", "False", "false"], - description=( - "If true, create and publish point clouds for each depth registered and RGB camera pair. Requires that" - " the depth_register_mode launch argument is set to a value that is not `disable`." - ), - ) - ) - launch_args.append( - DeclareLaunchArgument( - "uncompress_images", - default_value="True", - choices=["True", "true", "False", "false"], - description="Choose whether to publish uncompressed images from Spot.", - ) - ) - launch_args.append( - DeclareLaunchArgument( - "publish_compressed_images", - default_value="False", - choices=["True", "true", "False", "false"], - description="Choose whether to publish compressed images from Spot.", - ) - ) - launch_args.append( - DeclareLaunchArgument( - "stitch_front_images", - default_value="False", - choices=["True", "true", "False", "false"], - description=( - "Choose whether to publish a stitched image constructed from Spot's front left and right cameras." - ), - ) - ) launch_args.append(DeclareLaunchArgument("spot_name", default_value="", description="Name of Spot")) + launch_args += declare_image_publisher_args() ld = launch.LaunchDescription(launch_args) diff --git a/spot_driver/spot_driver/launch/spot_launch_helpers.py b/spot_driver/spot_driver/launch/spot_launch_helpers.py index a220b3f1f..50914467b 100644 --- a/spot_driver/spot_driver/launch/spot_launch_helpers.py +++ b/spot_driver/spot_driver/launch/spot_launch_helpers.py @@ -2,15 +2,99 @@ import logging import os +from enum import Enum from typing import Any, Dict, List, Optional, Tuple import yaml +from launch.actions import DeclareLaunchArgument from spot_wrapper.wrapper import SpotWrapper COLOR_END = "\33[0m" COLOR_YELLOW = "\33[33m" +IMAGE_PUBLISHER_ARGS = [ + "depth_registered_mode", + "publish_point_clouds", + "uncompress_images", + "publish_compressed_images", + "stitch_front_images", +] + + +class DepthRegisteredMode(Enum): + """Options for obtaining depth registered images images from Spot. We can request them from Spot's SDK, generate + them using `depth_image_proc`'s nodelets, or just not publish them at all.""" + + DISABLE = "disable" + FROM_SPOT = "from_spot" + FROM_NODELETS = "from_nodelets" + + def __repr__(self) -> str: + return self.value + + +def declare_image_publisher_args() -> List[DeclareLaunchArgument]: + """Generates launch arguments for each element in IMAGE_PUBLISHER_ARGS. This is useful to avoid copying and pasting + the same launch arguments multiple times launchfiles that call the spot_image_publishers launchfile. + + Returns: + List[DeclareLaunchArgument]: List of DeclareLaunchArguments useful for image publishing. + """ + launch_args = [] + launch_args.append( + DeclareLaunchArgument( + "depth_registered_mode", + default_value=DepthRegisteredMode.FROM_NODELETS.value, + choices=[e.value for e in DepthRegisteredMode], + description=( + f"If `{DepthRegisteredMode.DISABLE.value}` is set, do not publish registered depth images." + f" If `{DepthRegisteredMode.FROM_SPOT.value}` is set, request registered depth images from Spot through" + f" its SDK. If `{DepthRegisteredMode.FROM_NODELETS.value}` is set, use depth_image_proc::RegisterNode" + " component nodes running on the host computer to create registered depth images (this reduces the" + " computational load on Spot's internal systems)." + ), + ) + ) + launch_args.append( + DeclareLaunchArgument( + "publish_point_clouds", + default_value="False", + choices=["True", "true", "False", "false"], + description=( + "If true, create and publish point clouds for each depth registered and RGB camera pair. Requires that" + " the depth_register_mode launch argument is set to a value that is not `disable`." + ), + ) + ) + launch_args.append( + DeclareLaunchArgument( + "uncompress_images", + default_value="True", + choices=["True", "true", "False", "false"], + description="Choose whether to publish uncompressed images from Spot.", + ) + ) + launch_args.append( + DeclareLaunchArgument( + "publish_compressed_images", + default_value="False", + choices=["True", "true", "False", "false"], + description="Choose whether to publish compressed images from Spot.", + ) + ) + launch_args.append( + DeclareLaunchArgument( + "stitch_front_images", + default_value="False", + choices=["True", "true", "False", "false"], + description=( + "Choose whether to publish a stitched image constructed from Spot's front left and right cameras." + ), + ) + ) + return launch_args + def get_ros_param_dict(config_file_path: str) -> Dict[str, Any]: """Get a dictionary of parameter_name: parameter_value from a ROS config yaml file. diff --git a/spot_ros2_control/README.md b/spot_ros2_control/README.md index 456379163..438287db8 100644 --- a/spot_ros2_control/README.md +++ b/spot_ros2_control/README.md @@ -1,6 +1,6 @@ # spot_ros2_control -This is a ROS 2 package designed to communicate with Spot's low level API through ROS 2 control. It sets up a hardware interface that can be either `mock` or connected to the robot using the [Spot C++ SDK](https://github.com/boston-dynamics/spot-cpp-sdk) (currently in progress). By default, it loads a standard joint state broadcaster and forward position controller provided from `ros2_control`. +This is a ROS 2 package designed to communicate with Spot's low level API through ROS 2 control. It sets up a hardware interface that can be either `mock` or connected to the robot using the [Spot C++ SDK](https://github.com/boston-dynamics/spot-cpp-sdk). By default, it loads a standard joint state broadcaster and forward position controller provided from `ros2_control`. ## On-robot @@ -24,8 +24,17 @@ You can then run the launchfile with the following command: ros2 launch spot_ros2_control spot_ros2_control.launch.py hardware_interface:=robot config_file:=path/to/spot_ros.yaml ``` -This hardware interface can stream the joint angles of the robot using the low level API at ~333 Hz. -Both state streaming and command streaming are implemented. +If you wish to launch these nodes in a namespace, add the argument `spot_name:=`. + +This hardware interface will stream the joint angles of the robot using the low level API at 333 Hz onto the topic `//low_level/joint_states`. + +Commands can be sent on the topic `//forward_position_controller/commands`. This will forward position commands directly to the spot sdk through the hardware interface. + +> [!CAUTION] +> When using the forward position controller, there is no safety mechanism in place to ensure smooth motion. The ordering of the command must match the ordering of the joints specified in the controller configuration file ([here for robots with an arm](config/spot_default_controllers_with_arm.yaml) or [here for robots without an arm](config/spot_default_controllers_without_arm.yaml)), and the robot can move in unpredictable and dangerous ways if this is not set correctly. Make sure to keep a safe distance from the robot when working with this controller and ensure the e-stop can easily be pressed if needed. + +Additionally, the state publisher node, object synchronization node, and image publisher nodes from [`spot_driver`](../spot_driver/) will get launched by default when running on the robot to provide extra information such as odometry topics and camera feeds. +To turn off the image publishers (which can cause problems with bandwidth), add the launch argument `launch_image_publishers:=false`. ## Mock @@ -35,7 +44,7 @@ To use a mock hardware interface, run: ros2 launch spot_ros2_control spot_ros2_control.launch.py hardware_interface:=mock ``` -By default, this will load a robot with no arm. If you want your mock robot to have an arm, add the launch argument `mock_has_arm:=True`. +By default, this will load a robot with no arm. If you want your mock robot to have an arm, add the launch argument `mock_arm:=True`. ## Examples diff --git a/spot_ros2_control/config/examples.yaml b/spot_ros2_control/config/examples.yaml deleted file mode 100644 index 1dcbacf89..000000000 --- a/spot_ros2_control/config/examples.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - # Used by both noarm squat and wiggle arm examples. - command_rate: 50.0 # Rate in Hz at which to send commands - seconds_per_motion: 2.0 # Number of seconds per each motion of the robot. - - # Used by the noarm squat example. - # The following joint angles are in the order of FL_hip_x, FL_hip_y, FL_knee, FR..., RL..., RR... - stand_joint_angles: [0.12, 0.72, -1.45, -0.12, 0.72, -1.45, 0.12, 0.72, -1.45, -0.12, 0.72, -1.45] - squat_joint_angles: [0.15, 1.30, -2.25, -0.15, 1.30, -2.25, 0.15, 1.30, -2.25, -0.15, 1.30, -2.25] - - # Used by the wiggle arm example. - joints_to_wiggle: [16, 18 ] # Indices of the joints to wiggle. This corresponds to WR0 and F1X - # Full list of indices -> joint name: - # 0 1 2 3 4 5 6 7 8 9 10 11 - # FL_hip_x, FL_hip_y, FL_knee, FR_hip_x, FR_hip_y, FR_knee, RL_hip_x, RL_hip_y, RL_knee, RR_hip_x, RR_hip_y, RR_knee - # 12 13 14 15 16 17 18 - # SH0, SH1, EL0, EL1, WR0, WR1, F1X - wiggle_up_offsets: [ 1.0, -0.5] # Offset from starting position of each joints_to_wiggle for the first wiggle (up) - wiggle_down_offsets: [-1.0, -0.5] # Offset from starting position of each joints_to_wiggle for the second wiggle (down) diff --git a/spot_ros2_control/examples/src/joint_command_passthrough.cpp b/spot_ros2_control/examples/src/joint_command_passthrough.cpp index e74fa8942..8aad2dc58 100644 --- a/spot_ros2_control/examples/src/joint_command_passthrough.cpp +++ b/spot_ros2_control/examples/src/joint_command_passthrough.cpp @@ -20,7 +20,7 @@ class JointCommandPassthrough : public rclcpp::Node { JointCommandPassthrough() : Node("joint_passthrough") { std::string robot_namespace = declare_parameter("robot_namespace", "Spot"); robot_namespace = robot_namespace.empty() ? "" : robot_namespace + "/"; - std::string joint_state_topic = robot_namespace + declare_parameter("joint_state_topic", "joint_states"); + std::string joint_state_topic = robot_namespace + declare_parameter("joint_state_topic", "low_level/joint_states"); std::string joint_commands_topic = robot_namespace + declare_parameter("joint_commands_topic", "joint_commands"); std::string controller_commands_topic = robot_namespace + declare_parameter("controller_commands_topic", "forward_position_controller/commands"); diff --git a/spot_ros2_control/examples/src/noarm_squat.cpp b/spot_ros2_control/examples/src/noarm_squat.cpp index 6bc1545e5..b09246e5e 100644 --- a/spot_ros2_control/examples/src/noarm_squat.cpp +++ b/spot_ros2_control/examples/src/noarm_squat.cpp @@ -18,8 +18,13 @@ class NoarmSquat : public rclcpp::Node { public: NoarmSquat() : Node("noarm_squat"), squat_state_{SquatState::INITIALIZING}, initialized_{false}, count_{0}, njoints_{12} { - stand_joint_angles_ = declare_parameter("stand_joint_angles", std::vector{}); - squat_joint_angles_ = declare_parameter("squat_joint_angles", std::vector{}); + // The following joint angles are in the order of FL_hip_x, FL_hip_y, FL_knee, FR..., RL..., RR... + stand_joint_angles_ = declare_parameter( + "stand_joint_angles", + std::vector{0.12, 0.72, -1.45, -0.12, 0.72, -1.45, 0.12, 0.72, -1.45, -0.12, 0.72, -1.45}); + squat_joint_angles_ = declare_parameter( + "squat_joint_angles", + std::vector{0.15, 1.30, -2.25, -0.15, 1.30, -2.25, 0.15, 1.30, -2.25, -0.15, 1.30, -2.25}); const auto command_rate = declare_parameter("command_rate", 50.0); // how frequently to send commands in Hz const auto seconds_per_motion = declare_parameter("seconds_per_motion", 2.0); // how many seconds the squat and stand motions should take @@ -36,7 +41,7 @@ class NoarmSquat : public rclcpp::Node { command_pub_ = create_publisher("forward_position_controller/commands", 10); joint_states_sub_ = create_subscription( - "joint_states", 10, std::bind(&NoarmSquat::joint_states_callback, this, std::placeholders::_1)); + "low_level/joint_states", 10, std::bind(&NoarmSquat::joint_states_callback, this, std::placeholders::_1)); const auto timer_period = std::chrono::duration_cast(std::chrono::duration(1. / command_rate)); diff --git a/spot_ros2_control/examples/src/wiggle_arm.cpp b/spot_ros2_control/examples/src/wiggle_arm.cpp index ff21a4189..2ee25b401 100644 --- a/spot_ros2_control/examples/src/wiggle_arm.cpp +++ b/spot_ros2_control/examples/src/wiggle_arm.cpp @@ -19,9 +19,12 @@ enum class WiggleState { WIGGLE_DOWN, WIGGLE_MIDDLE, WIGGLE_UP, RESET }; class WiggleArm : public rclcpp::Node { public: WiggleArm() : Node("wiggle_arm"), wiggle_state_{WiggleState::WIGGLE_DOWN}, initialized_{false} { - joints_to_wiggle_ = declare_parameter("joints_to_wiggle", std::vector{}); - wiggle_up_offsets_ = declare_parameter("wiggle_up_offsets", std::vector{}); - wiggle_down_offsets_ = declare_parameter("wiggle_down_offsets", std::vector{}); + // Indices of the joints to wiggle. This corresponds to WR0 and F1X + joints_to_wiggle_ = declare_parameter("joints_to_wiggle", std::vector{16, 18}); + // Offset from starting position of each joints_to_wiggle for the first wiggle (up) + wiggle_up_offsets_ = declare_parameter("wiggle_up_offsets", std::vector{1.0, -0.5}); + // Offset from starting position of each joints_to_wiggle for the second wiggle (down) + wiggle_down_offsets_ = declare_parameter("wiggle_down_offsets", std::vector{-1.0, -0.5}); const auto command_rate = declare_parameter("command_rate", 50.0); // how frequently to send commands in Hz const auto seconds_per_motion = declare_parameter("seconds_per_motion", 2.0); // how many seconds each wiggle should take @@ -34,7 +37,7 @@ class WiggleArm : public rclcpp::Node { } joint_states_sub_ = create_subscription( - "joint_states", 10, std::bind(&WiggleArm::joint_states_callback, this, std::placeholders::_1)); + "low_level/joint_states", 10, std::bind(&WiggleArm::joint_states_callback, this, std::placeholders::_1)); command_pub_ = create_publisher("forward_position_controller/commands", 10); const auto timer_period = std::chrono::duration_cast(std::chrono::duration(1. / command_rate)); diff --git a/spot_ros2_control/hardware/include/spot_ros2_control/spot_joint_map.hpp b/spot_ros2_control/hardware/include/spot_ros2_control/spot_joint_map.hpp index 790e6f7d7..3fd25db4f 100644 --- a/spot_ros2_control/hardware/include/spot_ros2_control/spot_joint_map.hpp +++ b/spot_ros2_control/hardware/include/spot_ros2_control/spot_joint_map.hpp @@ -69,6 +69,7 @@ static const std::vector arm_kd = {5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5. /// @param ordered_joint_angles_ Joint positions from the joint state message following the correct order. /// @return boolean indicating if the joint angles got ordered successfully. bool order_joints(const sensor_msgs::msg::JointState& msg, std::vector& ordered_joint_angles) { + // this needs to be fixed so that it works if the joints are namespaced const auto njoints = msg.position.size(); ordered_joint_angles.resize(njoints); // Different joint index maps for arm-full and arm-less diff --git a/spot_ros2_control/launch/noarm_squat.launch.py b/spot_ros2_control/launch/noarm_squat.launch.py index 36bdff555..95786e672 100644 --- a/spot_ros2_control/launch/noarm_squat.launch.py +++ b/spot_ros2_control/launch/noarm_squat.launch.py @@ -1,11 +1,9 @@ # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare def generate_launch_description(): @@ -16,27 +14,11 @@ def generate_launch_description(): default_value="", description="Name of the Spot that will be used as a namespace.", ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - PathJoinSubstitution( - [FindPackageShare("spot_ros2_control"), "launch", "spot_ros2_control.launch.py"] - ) - ] - ), - launch_arguments={ - "mock_has_arm": "false", - "robot_controller": "forward_position_controller", - "hardware_interface": "mock", - "spot_name": LaunchConfiguration("spot_name"), - }.items(), - ), Node( package="spot_ros2_control", executable="noarm_squat", name="noarm_squat", output="screen", - parameters=[PathJoinSubstitution([FindPackageShare("spot_ros2_control"), "config", "examples.yaml"])], namespace=LaunchConfiguration("spot_name"), ), ] diff --git a/spot_ros2_control/launch/spot_ros2_control.launch.py b/spot_ros2_control/launch/spot_ros2_control.launch.py index abecbd79d..82aa9eb1a 100644 --- a/spot_ros2_control/launch/spot_ros2_control.launch.py +++ b/spot_ros2_control/launch/spot_ros2_control.launch.py @@ -5,8 +5,9 @@ import yaml from ament_index_python.packages import get_package_share_directory from launch import LaunchContext, LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( Command, FindExecutable, @@ -16,12 +17,17 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from spot_driver.launch.spot_launch_helpers import get_login_parameters, spot_has_arm +from spot_driver.launch.spot_launch_helpers import ( + IMAGE_PUBLISHER_ARGS, + declare_image_publisher_args, + get_login_parameters, + spot_has_arm, +) THIS_PACKAGE = "spot_ros2_control" -def create_controllers_config(spot_name: str, has_arm: bool) -> None: +def create_controllers_config(spot_name: str, has_arm: bool) -> str: """Writes a configuration file used to put the ros2 control nodes into a namespace. This is necessary as if your ros2 control nodes are launched in a namespace, the configuration yaml used must also reflect this same namespace when defining parameters of your controllers. @@ -95,10 +101,10 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: controllers_config: str = LaunchConfiguration("controllers_config").perform(context) mock_arm: bool = IfCondition(LaunchConfiguration("mock_arm")).evaluate(context) spot_name: str = LaunchConfiguration("spot_name").perform(context) + config_file: str = LaunchConfiguration("config_file").perform(context) # If connected to a physical robot, query if it has an arm. Otherwise, use the value in mock_arm. if hardware_interface == "robot": - config_file = LaunchConfiguration("config_file").perform(context) arm = spot_has_arm(config_file_path=config_file, spot_name="") username, password, hostname = get_login_parameters(config_file)[:3] login_params = f" hostname:={hostname} username:={username} password:={password}" @@ -128,7 +134,6 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: # If no controller config file is selected, use the appropriate default. Else, just use the yaml that is passed in. if controllers_config == "": # Generate spot_default_controllers.yaml depending on namespace and whether the robot has an arm. - create_controllers_config(spot_name, arm) controllers_config = create_controllers_config(spot_name, arm) # Add nodes @@ -139,6 +144,8 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: output="both", parameters=[robot_description, controllers_config], namespace=spot_name, + # Remap joint states so it doesn't collide with high level joint states. + remappings=[(f"/{tf_prefix}joint_states", f"/{tf_prefix}low_level/joint_states")], ) ) ld.add_action( @@ -148,6 +155,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: output="both", parameters=[robot_description], namespace=spot_name, + remappings=[(f"/{tf_prefix}joint_states", f"/{tf_prefix}low_level/joint_states")], ) ) ld.add_action( @@ -178,6 +186,44 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: namespace=spot_name, ) ) + # Finally, launch extra nodes for state and image publishing if we are running on a robot. + if hardware_interface == "robot": + # launch image publishers + ld.add_action( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [FindPackageShare("spot_driver"), "launch", "spot_image_publishers.launch.py"] + ) + ] + ), + launch_arguments={ + key: LaunchConfiguration(key) for key in ["config_file", "spot_name"] + IMAGE_PUBLISHER_ARGS + }.items(), + condition=IfCondition(LaunchConfiguration("launch_image_publishers")), + ) + ) + # launch state publisher node (useful for publishing odom & other statuses) + ld.add_action( + Node( + package="spot_driver", + executable="state_publisher_node", + output="screen", + parameters=[config_file, {"spot_name": spot_name}], + namespace=spot_name, + ) + ) + # launch object sync node (for fiducials) + ld.add_action( + Node( + package="spot_driver", + executable="object_synchronizer_node", + output="screen", + parameters=[config_file, {"spot_name": spot_name}], + namespace=spot_name, + ) + ) return @@ -213,8 +259,7 @@ def generate_launch_description(): DeclareLaunchArgument( "robot_controller", default_value="forward_position_controller", - choices=["forward_position_controller"], - description="Robot controller to start. Must match an entry in controller_config.", + description="Robot controller to start. Must match an entry in controllers_config.", ), DeclareLaunchArgument( "mock_arm", @@ -224,7 +269,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( "launch_rviz", - default_value="True", + default_value="true", choices=["True", "true", "False", "false"], description="Flag to enable rviz.", ), @@ -233,7 +278,14 @@ def generate_launch_description(): default_value="", description="Name of the Spot that will be used as a namespace.", ), + DeclareLaunchArgument( + "launch_image_publishers", + default_value="true", + choices=["True", "true", "False", "false"], + description="Choose whether to launch the image publishers.", + ), ] + + declare_image_publisher_args() ) # Add nodes to launch description ld.add_action(OpaqueFunction(function=launch_setup, args=[ld])) diff --git a/spot_ros2_control/launch/wiggle_arm.launch.py b/spot_ros2_control/launch/wiggle_arm.launch.py index 8ea42eab1..333fad52b 100644 --- a/spot_ros2_control/launch/wiggle_arm.launch.py +++ b/spot_ros2_control/launch/wiggle_arm.launch.py @@ -1,11 +1,9 @@ # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare def generate_launch_description(): @@ -16,27 +14,11 @@ def generate_launch_description(): default_value="", description="Name of the Spot that will be used as a namespace.", ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - PathJoinSubstitution( - [FindPackageShare("spot_ros2_control"), "launch", "spot_ros2_control.launch.py"] - ) - ] - ), - launch_arguments={ - "mock_has_arm": "true", - "robot_controller": "forward_position_controller", - "hardware_interface": "mock", - "spot_name": LaunchConfiguration("spot_name"), - }.items(), - ), Node( package="spot_ros2_control", executable="wiggle_arm", name="wiggle_arm", output="screen", - parameters=[PathJoinSubstitution([FindPackageShare("spot_ros2_control"), "config", "examples.yaml"])], namespace=LaunchConfiguration("spot_name"), ), ]