Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[SW-1764] allow use of preferred_odom_frame parameter #538

Merged
merged 5 commits into from
Dec 19, 2024
Merged

[SW-1764] allow use of preferred_odom_frame parameter #538

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
24a8d59
tests not passing but just committing anyways
Dec 18, 2024
80522aa
Fix failing test
Dec 18, 2024
c749537
remove hardcoded referernce in launchfile
Dec 18, 2024
86806e3
Clean up launchfile minorly
Dec 18, 2024
39daae3
Delete unnecessary comment
Dec 18, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 4 additions & 3 deletions spot_driver/include/spot_driver/conversions/robot_state.hpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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,
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Teeny-tiny comment. I don't think it really matters since they're primitives, but since you added const to the boolean function arguments above, is const something relevant here too?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ah yeah I just based this on the function already above this, which also doesn't use const. Probably wouldn't change anything if i were to add it here

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
Open in desktop
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
Loading