Skip to content

Commit

Permalink
[SW-1764] allow use of preferred_odom_frame parameter (#538)
Browse files Browse the repository at this point in the history 
## Change Overview

The `preferred_odom_frame` parameter is supposed to let you chose whether "odom" (default) or "vision" frame is used as the base frame in the `/odometry` topic. On main right now this is hardcoded to "odom" in the launchfile

This PR allows you to actually change this parameter. It also introduces some logic to deal with getting the velocity of the body in either the "odom" or "vision" frame, depending on which choice is selected. There is also some minor launchfile cleanup here. 

related to: #29, and takes a lot of work from #416

## Testing Done

- [x] default launch: nothing changes, and "SpotName/odom" is the parent frame in `/SpotName/odometry` topic
- [x] with `preferred_odom_frame: "vision"` parameter set via config file: "vision" is the parent frame in `/SpotName/odometry` 
- [x] modified unit tests to test cases of getting body velocity in the vision frame
  • Loading branch information
@khughes-bdai
khughes-bdai authored Dec 19, 2024
1 parent 9b77d8a commit 3c2f3c9
Show file tree
Hide file tree
Showing 6 changed files with 62 additions and 42 deletions.
7 changes: 4 additions & 3 deletions spot_driver/include/spot_driver/conversions/robot_state.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,8 +134,9 @@ std::optional<tf2_msgs::msg::TFMessage> 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<geometry_msgs::msg::TwistWithCovarianceStamped> getOdomTwist(
const ::bosdyn::api::RobotState& robot_state, const google::protobuf::Duration& clock_skew);
std::optional<geometry_msgs::msg::TwistWithCovarianceStamped> 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
Expand All @@ -152,7 +153,7 @@ std::optional<geometry_msgs::msg::TwistWithCovarianceStamped> getOdomTwist(
*/
std::optional<nav_msgs::msg::Odometry> 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.
Expand Down
39 changes: 20 additions & 19 deletions spot_driver/launch/spot_driver.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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,
Expand All @@ -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",
Expand All @@ -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)
Expand All @@ -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",
Expand All @@ -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",
Expand All @@ -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,
Expand All @@ -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(),
Expand All @@ -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(
Expand Down Expand Up @@ -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]))

Expand Down
27 changes: 19 additions & 8 deletions spot_driver/src/conversions/robot_state.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -163,30 +163,41 @@ std::optional<tf2_msgs::msg::TFMessage> getTf(const ::bosdyn::api::FrameTreeSnap
return tf_msg;
}

std::optional<geometry_msgs::msg::TwistWithCovarianceStamped> 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<geometry_msgs::msg::TwistWithCovarianceStamped> 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<nav_msgs::msg::Odometry> 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;
}
Expand Down
2 changes: 1 addition & 1 deletion spot_driver/src/robot_state/state_publisher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down
12 changes: 12 additions & 0 deletions spot_driver/test/include/spot_driver/robot_state_test_tools.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
17 changes: 6 additions & 11 deletions spot_driver/test/src/conversions/test_robot_state.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand All @@ -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());
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit 3c2f3c9

Please sign in to comment.