diff --git a/spot_driver/include/spot_driver/conversions/robot_state.hpp b/spot_driver/include/spot_driver/conversions/robot_state.hpp index 3997403cf..7aed87e2a 100644 --- a/spot_driver/include/spot_driver/conversions/robot_state.hpp +++ b/spot_driver/include/spot_driver/conversions/robot_state.hpp @@ -134,8 +134,9 @@ std::optional getTf(const ::bosdyn::api::FrameTreeSnap * @return If the robot state message contains the velocity of the Spot's body relative to the odometry frame in its * kinematic state, return a TwistWithCovarianceStamped containing this data. Otherwise, return nullopt. */ -std::optional getOdomTwist( - const ::bosdyn::api::RobotState& robot_state, const google::protobuf::Duration& clock_skew); +std::optional getOdomTwist(const ::bosdyn::api::RobotState& robot_state, + const google::protobuf::Duration& clock_skew, + const bool is_using_vision); /** * @brief Create an Odometry ROS message representing Spot's pose and velocity relative to a fixed world frame by @@ -152,7 +153,7 @@ std::optional getOdomTwist( */ std::optional getOdom(const ::bosdyn::api::RobotState& robot_state, const google::protobuf::Duration& clock_skew, const std::string& prefix, - bool is_using_vision); + const bool is_using_vision); /** * @brief Create a PowerState ROS message by parsing a RobotState message. diff --git a/spot_driver/launch/spot_driver.launch.py b/spot_driver/launch/spot_driver.launch.py index fab0ef70c..5580e32da 100644 --- a/spot_driver/launch/spot_driver.launch.py +++ b/spot_driver/launch/spot_driver.launch.py @@ -2,13 +2,12 @@ import os -import launch -import launch_ros from launch import LaunchContext, LaunchDescription 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, LaunchConfiguration, PathJoinSubstitution, TextSubstitution +from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from spot_driver.launch.spot_launch_helpers import IMAGE_PUBLISHER_ARGS, declare_image_publisher_args, spot_has_arm @@ -38,8 +37,6 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: robot_description_pkg_share = FindPackageShare(robot_description_package).find(robot_description_package) - # Since spot_image_publisher_node is responsible for retrieving and publishing images, disable all image publishing - # in spot_driver. spot_driver_params = { "spot_name": spot_name, "mock_enable": mock_enable, @@ -50,7 +47,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: # Merge the two dicts spot_driver_params = {**spot_driver_params, **mock_spot_driver_params} - spot_driver_node = launch_ros.actions.Node( + spot_driver_node = Node( package="spot_driver", executable="spot_ros2", name="spot_ros2", @@ -63,21 +60,22 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: if not tf_prefix and spot_name: tf_prefix = PathJoinSubstitution([spot_name, ""]) - kinematc_node_params = {"spot_name": spot_name} - kinematic_node = launch_ros.actions.Node( + spot_name_param = {"spot_name": spot_name} + + kinematic_node = Node( package="spot_driver", executable="spot_inverse_kinematics_node", output="screen", - parameters=[config_file, kinematc_node_params], + parameters=[config_file, spot_name_param], namespace=spot_name, ) ld.add_action(kinematic_node) - object_sync_node = launch_ros.actions.Node( + object_sync_node = Node( package="spot_driver", executable="object_synchronizer_node", output="screen", - parameters=[config_file, {"spot_name": spot_name}], + parameters=[config_file, spot_name_param], namespace=spot_name, ) ld.add_action(object_sync_node) @@ -97,7 +95,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: ] ) robot_description_params = {"robot_description": robot_description} - robot_state_publisher = launch_ros.actions.Node( + robot_state_publisher = Node( package="robot_state_publisher", executable="robot_state_publisher", output="screen", @@ -106,17 +104,16 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: ) ld.add_action(robot_state_publisher) - spot_robot_state_publisher_params = {"spot_name": spot_name, "preferred_odom_frame": "odom"} - spot_robot_state_publisher = launch_ros.actions.Node( + spot_robot_state_publisher = Node( package="spot_driver", executable="state_publisher_node", output="screen", - parameters=[config_file, spot_robot_state_publisher_params], + parameters=[config_file, spot_name_param], namespace=spot_name, ) ld.add_action(spot_robot_state_publisher) - spot_alert_node = launch_ros.actions.Node( + spot_alert_node = Node( package="spot_driver", executable="spot_alerts", name="spot_alerts", @@ -126,7 +123,9 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: ld.add_action(spot_alert_node) rviz = IncludeLaunchDescription( - PythonLaunchDescriptionSource([FindPackageShare(THIS_PACKAGE), "/launch", "/rviz.launch.py"]), + PythonLaunchDescriptionSource( + PathJoinSubstitution([FindPackageShare(THIS_PACKAGE), "launch", "rviz.launch.py"]) + ), launch_arguments={ "spot_name": spot_name, "rviz_config_file": rviz_config_file, @@ -136,7 +135,9 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: ld.add_action(rviz) spot_image_publishers = IncludeLaunchDescription( - PythonLaunchDescriptionSource([FindPackageShare(THIS_PACKAGE), "/launch", "/spot_image_publishers.launch.py"]), + PythonLaunchDescriptionSource( + PathJoinSubstitution([FindPackageShare(THIS_PACKAGE), "launch", "spot_image_publishers.launch.py"]) + ), launch_arguments={ key: LaunchConfiguration(key) for key in ["config_file", "spot_name"] + IMAGE_PUBLISHER_ARGS }.items(), @@ -145,7 +146,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: ld.add_action(spot_image_publishers) -def generate_launch_description() -> launch.LaunchDescription: +def generate_launch_description() -> LaunchDescription: launch_args = [] launch_args.append( @@ -195,7 +196,7 @@ def generate_launch_description() -> launch.LaunchDescription: launch_args += declare_image_publisher_args() launch_args.append(DeclareLaunchArgument("spot_name", default_value="", description="Name of Spot")) - ld = launch.LaunchDescription(launch_args) + ld = LaunchDescription(launch_args) ld.add_action(OpaqueFunction(function=launch_setup, args=[ld])) diff --git a/spot_driver/src/conversions/robot_state.cpp b/spot_driver/src/conversions/robot_state.cpp index 5b8658cb5..dfafc343b 100644 --- a/spot_driver/src/conversions/robot_state.cpp +++ b/spot_driver/src/conversions/robot_state.cpp @@ -163,30 +163,41 @@ std::optional getTf(const ::bosdyn::api::FrameTreeSnap return tf_msg; } -std::optional getOdomTwist( - const ::bosdyn::api::RobotState& robot_state, const google::protobuf::Duration& clock_skew) { - if (!robot_state.has_kinematic_state() || !robot_state.kinematic_state().has_velocity_of_body_in_odom()) { +std::optional getOdomTwist(const ::bosdyn::api::RobotState& robot_state, + const google::protobuf::Duration& clock_skew, + const bool is_using_vision) { + if (!robot_state.has_kinematic_state()) { + return std::nullopt; + } + + const auto& kinematic_state = robot_state.kinematic_state(); + if (is_using_vision && !kinematic_state.has_velocity_of_body_in_vision()) { + return std::nullopt; + } else if (!is_using_vision && !kinematic_state.has_velocity_of_body_in_odom()) { return std::nullopt; } geometry_msgs::msg::TwistWithCovarianceStamped odom_twist_msg; // TODO(schornakj): need to add the frame ID here? + + const bosdyn::api::SE3Velocity& velocity_of_body_in_world = + is_using_vision ? kinematic_state.velocity_of_body_in_vision() : kinematic_state.velocity_of_body_in_odom(); + odom_twist_msg.header.stamp = spot_ros2::robotTimeToLocalTime(robot_state.kinematic_state().acquisition_timestamp(), clock_skew); - convertToRos(robot_state.kinematic_state().velocity_of_body_in_odom(), odom_twist_msg.twist.twist); + convertToRos(velocity_of_body_in_world, odom_twist_msg.twist.twist); return odom_twist_msg; } std::optional getOdom(const ::bosdyn::api::RobotState& robot_state, const google::protobuf::Duration& clock_skew, const std::string& prefix, - bool is_using_vision) { + const bool is_using_vision) { if (!robot_state.has_kinematic_state() || !robot_state.kinematic_state().has_acquisition_timestamp() || - !robot_state.kinematic_state().has_transforms_snapshot() || - !robot_state.kinematic_state().has_velocity_of_body_in_odom()) { + !robot_state.kinematic_state().has_transforms_snapshot()) { return std::nullopt; } - const auto odom_twist = getOdomTwist(robot_state, clock_skew); + const auto odom_twist = getOdomTwist(robot_state, clock_skew, is_using_vision); if (!odom_twist) { return std::nullopt; } diff --git a/spot_driver/src/robot_state/state_publisher.cpp b/spot_driver/src/robot_state/state_publisher.cpp index 7e4d702f8..66d6bc57d 100644 --- a/spot_driver/src/robot_state/state_publisher.cpp +++ b/spot_driver/src/robot_state/state_publisher.cpp @@ -69,7 +69,7 @@ void StatePublisher::timerCallback() { getEstopStates(robot_state, clock_skew), getJointStates(robot_state, clock_skew, frame_prefix_), getTf(robot_state, clock_skew, frame_prefix_, full_tf_root_id_), - getOdomTwist(robot_state, clock_skew), + getOdomTwist(robot_state, clock_skew, is_using_vision_), getOdom(robot_state, clock_skew, frame_prefix_, is_using_vision_), getPowerState(robot_state, clock_skew), getSystemFaultState(robot_state, clock_skew), diff --git a/spot_driver/test/include/spot_driver/robot_state_test_tools.hpp b/spot_driver/test/include/spot_driver/robot_state_test_tools.hpp index 9ae03edb7..20006a48c 100644 --- a/spot_driver/test/include/spot_driver/robot_state_test_tools.hpp +++ b/spot_driver/test/include/spot_driver/robot_state_test_tools.hpp @@ -116,6 +116,18 @@ inline void addBodyVelocityOdom(::bosdyn::api::KinematicState* mutable_kinematic velocity_angular->set_z(rz); } +inline void addBodyVelocityVision(::bosdyn::api::KinematicState* mutable_kinematic_state, double x, double y, double z, + double rx, double ry, double rz) { + auto* velocity_linear = mutable_kinematic_state->mutable_velocity_of_body_in_vision()->mutable_linear(); + velocity_linear->set_x(x); + velocity_linear->set_y(y); + velocity_linear->set_z(z); + auto* velocity_angular = mutable_kinematic_state->mutable_velocity_of_body_in_vision()->mutable_angular(); + velocity_angular->set_x(rx); + velocity_angular->set_y(ry); + velocity_angular->set_z(rz); +} + inline void addAcquisitionTimestamp(::bosdyn::api::KinematicState* mutable_kinematic_state, const google::protobuf::Timestamp& timestamp) { mutable_kinematic_state->mutable_acquisition_timestamp()->CopyFrom(timestamp); diff --git a/spot_driver/test/src/conversions/test_robot_state.cpp b/spot_driver/test/src/conversions/test_robot_state.cpp index 9dc4bf657..0712a166b 100644 --- a/spot_driver/test/src/conversions/test_robot_state.cpp +++ b/spot_driver/test/src/conversions/test_robot_state.cpp @@ -343,21 +343,15 @@ TEST(RobotStateConversions, TestGetOdomTwist) { timestamp.set_nanos(0); addAcquisitionTimestamp(robot_state.mutable_kinematic_state(), timestamp); - auto* velocity_linear = robot_state.mutable_kinematic_state()->mutable_velocity_of_body_in_odom()->mutable_linear(); - velocity_linear->set_x(1.0); - velocity_linear->set_y(2.0); - velocity_linear->set_z(3.0); - auto* velocity_angular = robot_state.mutable_kinematic_state()->mutable_velocity_of_body_in_odom()->mutable_angular(); - velocity_angular->set_x(1.0); - velocity_angular->set_y(2.0); - velocity_angular->set_z(3.0); + addBodyVelocityOdom(robot_state.mutable_kinematic_state(), 1.0, 2.0, 3.0, 1.0, 2.0, 3.0); // GIVEN some nominal clock skew google::protobuf::Duration clock_skew; clock_skew.set_seconds(1); // WHEN we create a TwistWithCovarianceStamped ROS message - const auto out = getOdomTwist(robot_state, clock_skew); + const auto is_using_vision = false; + const auto out = getOdomTwist(robot_state, clock_skew, is_using_vision); // THEN this succeeds ASSERT_THAT(out.has_value(), IsTrue()); @@ -377,7 +371,8 @@ TEST(RobotStateConversions, TestGetOdomTwistNoBodyVelocityInRobotState) { clock_skew.set_seconds(1); // WHEN we attempt to create a TwistWithCovarianceStamped ROS message - const auto out = getOdomTwist(robot_state, clock_skew); + const auto is_using_vision = false; + const auto out = getOdomTwist(robot_state, clock_skew, is_using_vision); // THEN no message is output ASSERT_THAT(out.has_value(), IsFalse()); @@ -435,7 +430,7 @@ TEST(RobotStateConversions, TestGetOdomInVisionFrame) { timestamp.set_seconds(99); timestamp.set_nanos(0); addAcquisitionTimestamp(robot_state.mutable_kinematic_state(), timestamp); - addBodyVelocityOdom(robot_state.mutable_kinematic_state(), 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + addBodyVelocityVision(robot_state.mutable_kinematic_state(), 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); addRootFrame(robot_state.mutable_kinematic_state()->mutable_transforms_snapshot(), "vision"); addTransform(robot_state.mutable_kinematic_state()->mutable_transforms_snapshot(), "body", "vision", 1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0);