Skip to content

Commit

Permalink
[EK-25] Use bosdyn_msgs bundles (bdaiinstitute#234)
Browse files Browse the repository at this point in the history
Signed-off-by: Michel Hidalgo <mhidalgo@theaiinstitute.com>
  • Loading branch information
mhidalgo-bdai authored Mar 6, 2024
1 parent 9e39e2f commit d21055f
Show file tree
Hide file tree
Showing 49 changed files with 755 additions and 21,684 deletions.
21 changes: 11 additions & 10 deletions .devcontainer/Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -30,23 +30,24 @@ RUN DEBIAN_FRONTEND=noninteractive apt-get update -q && \
RUN source "/opt/ros/${ROS_DISTRO}/setup.bash"

# Install bosdyn_msgs package
RUN curl -sL https://github.com/bdaiinstitute/bosdyn_msgs/releases/download/v3.3.2-AMD64/ros-humble-bosdyn-msgs_3.3.2-0jammy_amd64.deb --output /tmp/ros-humble-bosdyn-msgs_3.3.2-0jammy_amd64.deb --silent \
&& dpkg -i /tmp/ros-humble-bosdyn-msgs_3.3.2-0jammy_amd64.deb \
&& rm /tmp/ros-humble-bosdyn-msgs_3.3.2-0jammy_amd64.deb
RUN curl -sL https://github.com/bdaiinstitute/bosdyn_msgs/releases/download/4.0.0-2/ros-humble-bosdyn_msgs_4.0.0-2-jammy_amd64.run --output /tmp/ros-humble-bosdyn_msgs_4.0.0-2-jammy_amd64.run --silent \
&& chmod +x /tmp/ros-humble-bosdyn_msgs_4.0.0-2-jammy_amd64.run \
&& ((yes || true) | /tmp/ros-humble-bosdyn_msgs_4.0.0-2-jammy_amd64.run) \
&& rm /tmp/ros-humble-bosdyn_msgs_4.0.0-2-jammy_amd64.run

# Install spot_cpp_sdk package
RUN curl -sL https://github.com/bdaiinstitute/spot-cpp-sdk/releases/download/3.3.2/spot-cpp-sdk_3.3.2_amd64.deb --output /tmp/spot-cpp-sdk_3.3.2_amd64.deb --silent \
&& dpkg -i /tmp/spot-cpp-sdk_3.3.2_amd64.deb \
&& rm /tmp/spot-cpp-sdk_3.3.2_amd64.deb
RUN curl -sL https://github.com/bdaiinstitute/spot-cpp-sdk/releases/download/4.0.0/spot-cpp-sdk_4.0.0_amd64.deb --output /tmp/spot-cpp-sdk_4.0.0_amd64.deb --silent \
&& dpkg -i /tmp/spot-cpp-sdk_4.0.0_amd64.deb \
&& rm /tmp/spot-cpp-sdk_4.0.0_amd64.deb

# Install bosdyn_msgs missing dependencies
RUN python -m pip install --no-cache-dir --upgrade pip==22.3.1 \
&& pip install --root-user-action=ignore --no-cache-dir --default-timeout=900 \
numpy==1.24.1 \
bosdyn-client==3.3.2 \
bosdyn-mission==3.3.2 \
bosdyn-api==3.3.2 \
bosdyn-core==3.3.2 \
bosdyn-client==4.0.0 \
bosdyn-mission==4.0.0 \
bosdyn-api==4.0.0 \
bosdyn-core==4.0.0 \
pip cache purge

# Install spot_wrapper requirements
Expand Down
2 changes: 1 addition & 1 deletion README.md
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@

# Overview
This is a ROS2 package for Boston Dynamics' Spot. The package contains all necessary topics, services and actions to teleoperate or navigate Spot.
This package is derived from this [ROS1 package](https://github.com/heuristicus/spot_ros). This package currently corresponds to version 3.3.2 of the [spot-sdk](https://github.com/boston-dynamics/spot-sdk/releases/tag/v3.3.2).
This package is derived from this [ROS1 package](https://github.com/heuristicus/spot_ros). This package currently corresponds to version 4.0.0 of the [spot-sdk](https://github.com/boston-dynamics/spot-sdk/releases/tag/v4.0.0).

## Prerequisites
This package is tested for Ubuntu 22.04 and ROS 2 Humble, which can be installed following [this guide](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html).
Expand Down
4 changes: 2 additions & 2 deletions examples/inverse_kinematics_example/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -121,10 +121,10 @@ In ROS2, we convert the created protobuf ROS2 request and call a ROS2 service.
),
)
request = GetInverseKinematicSolutions.Request()
conv.convert_proto_to_bosdyn_msgs_inverse_kinematics_request(ik_request, request.request)
convert(ik_request, request.request)
ik_reponse = self._ik_client.call(request)
proto = inverse_kinematics_pb2.InverseKinematicsResponse()
conv.convert_bosdyn_msgs_inverse_kinematics_response_to_proto(ik_reponse.response, proto)
convert(ik_reponse.response, proto)
return proto
```
Original file line number Diff line number Diff line change
Expand Up @@ -35,12 +35,12 @@
)
from bosdyn.client.math_helpers import Quat, SE3Pose
from bosdyn.client.robot_command import RobotCommandBuilder
from bosdyn_msgs.conversions import convert
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
from utilities.simple_spot_commander import SimpleSpotCommander
from utilities.tf_listener_wrapper import TFListenerWrapper

import spot_driver.conversions as conv
from spot_msgs.action import RobotCommand # type: ignore
from spot_msgs.srv import GetInverseKinematicSolutions # type: ignore

Expand Down Expand Up @@ -102,7 +102,7 @@ def _ready_arm(self) -> bool:
"""
command = RobotCommandBuilder.arm_ready_command()
action_goal = RobotCommand.Goal()
conv.convert_proto_to_bosdyn_msgs_robot_command(command, action_goal.command)
convert(command, action_goal.command)
return self._robot_command_client.send_goal_and_wait("ready_arm", action_goal)

def _arm_stow(self) -> bool:
Expand All @@ -111,7 +111,7 @@ def _arm_stow(self) -> bool:
"""
command = RobotCommandBuilder.arm_stow_command()
action_goal = RobotCommand.Goal()
conv.convert_proto_to_bosdyn_msgs_robot_command(command, action_goal.command)
convert(command, action_goal.command)
return self._robot_command_client.send_goal_and_wait("arm_stow", action_goal)

def _send_ik_request(
Expand All @@ -133,11 +133,11 @@ def _send_ik_request(
),
)
request = GetInverseKinematicSolutions.Request()
conv.convert_proto_to_bosdyn_msgs_inverse_kinematics_request(ik_request, request.request)
convert(ik_request, request.request)
ik_reponse = self._ik_client.call(request)

proto = inverse_kinematics_pb2.InverseKinematicsResponse()
conv.convert_bosdyn_msgs_inverse_kinematics_response_to_proto(ik_reponse.response, proto)
convert(ik_reponse.response, proto)
return proto

def test_run(self) -> bool:
Expand Down Expand Up @@ -275,7 +275,7 @@ def test_run(self) -> bool:
wr1_T_tool.to_proto()
)
arm_command_goal = RobotCommand.Goal()
conv.convert_proto_to_bosdyn_msgs_robot_command(arm_command, arm_command_goal.command)
convert(arm_command, arm_command_goal.command)
result = self._robot_command_client.send_goal_and_wait(
action_name="arm_move_one", goal=arm_command_goal, timeout_sec=5
)
Expand Down
2 changes: 1 addition & 1 deletion examples/simple_arm_motion/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -134,7 +134,7 @@ In ROS2, we convert the created protobuf to a ROS2 action goal and we use the ac
```python
# Convert to a ROS message
action_goal = RobotCommand.Goal()
conv.convert_proto_to_bosdyn_msgs_robot_command(command, action_goal.command)
convert(command, action_goal.command)
# Send the request and wait until the arm arrives at the goal
node.get_logger().info('Moving arm to position 1.')
robot_command_client.send_goal_and_wait(action_goal)
Expand Down
6 changes: 3 additions & 3 deletions examples/simple_arm_motion/simple_arm_motion/arm_simple.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,10 @@
from bosdyn.client import math_helpers
from bosdyn.client.frame_helpers import GRAV_ALIGNED_BODY_FRAME_NAME, ODOM_FRAME_NAME
from bosdyn.client.robot_command import RobotCommandBuilder
from bosdyn_msgs.conversions import convert
from utilities.simple_spot_commander import SimpleSpotCommander
from utilities.tf_listener_wrapper import TFListenerWrapper

import spot_driver.conversions as conv
from spot_msgs.action import RobotCommand # type: ignore


Expand Down Expand Up @@ -97,7 +97,7 @@ def hello_arm(robot_name: Optional[str] = None) -> bool:

# Convert to a ROS message
action_goal = RobotCommand.Goal()
conv.convert_proto_to_bosdyn_msgs_robot_command(command, action_goal.command)
convert(command, action_goal.command)
# Send the request and wait until the arm arrives at the goal
logger.info("Moving arm to position 1.")
robot_command_client.send_goal_and_wait("arm_move_one", action_goal)
Expand Down Expand Up @@ -133,7 +133,7 @@ def hello_arm(robot_name: Optional[str] = None) -> bool:

# Convert to a ROS message
action_goal = RobotCommand.Goal()
conv.convert_proto_to_bosdyn_msgs_robot_command(command, action_goal.command)
convert(command, action_goal.command)
# Send the request and wait until the arm arrives at the goal
logger.info("Moving arm to position 2.")
robot_command_client.send_goal_and_wait("arm_move_two", action_goal)
Expand Down
2 changes: 1 addition & 1 deletion examples/simple_walk_forward/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ Now we construct a goal to send to the Spot driver:
goal_x=world_t_goal.x, goal_y=world_t_goal.y, goal_heading=world_t_goal.angle,
frame_name=VISION_FRAME_NAME)
action_goal = RobotCommand.Goal()
conv.convert_proto_to_bosdyn_msgs_robot_command(proto_goal, action_goal.command)
convert(proto_goal, action_goal.command)
```
This constructs a protobuf message using the BD built in tools and converts it into a ROS action goal of a type that can be sent by our action client.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,11 @@
from bosdyn.client.frame_helpers import BODY_FRAME_NAME, VISION_FRAME_NAME
from bosdyn.client.math_helpers import SE2Pose
from bosdyn.client.robot_command import RobotCommandBuilder
from bosdyn_msgs.conversions import convert
from rclpy.node import Node
from utilities.simple_spot_commander import SimpleSpotCommander
from utilities.tf_listener_wrapper import TFListenerWrapper

import spot_driver.conversions as conv
from spot_msgs.action import RobotCommand # type: ignore

# Where we want the robot to walk to relative to itself
Expand Down Expand Up @@ -74,7 +74,7 @@ def walk_forward_with_world_frame_goal(self) -> None:
frame_name=VISION_FRAME_NAME, # use Boston Dynamics' frame conventions
)
action_goal = RobotCommand.Goal()
conv.convert_proto_to_bosdyn_msgs_robot_command(proto_goal, action_goal.command)
convert(proto_goal, action_goal.command)
self._robot_command_client.send_goal_and_wait("walk_forward", action_goal)
self._logger.info("Successfully walked forward")

Expand Down
36 changes: 13 additions & 23 deletions install_spot_ros2.sh
Original file line number Diff line number Diff line change
@@ -1,11 +1,13 @@
ARM=false
ARCH="amd64"
SDK_VERSION="4.0.0"
MSG_VERSION="${SDK_VERSION}-2"
ROS_DISTRO=humble
HELP=$'--arm64: Installs ARM64 version'
REQUIREMENTS_FILE=spot_wrapper/requirements.txt

while true; do
case "$1" in
--arm64 ) ARM=true; shift ;;
--arm64 ) ARCH="arm64"; shift ;;
-h | --help ) echo "$HELP"; exit 0;;
-- ) shift; break ;;
* ) break ;;
Expand All @@ -29,25 +31,13 @@ sudo apt-get install -y python3-distutils
sudo apt-get install -y python3-apt
sudo pip3 install --force-reinstall -v "setuptools==59.6.0"

# Install bosdyn_msgs - automatic conversions of BD protobufs to ROS messages
wget -q -O /tmp/ros-humble-bosdyn_msgs_${MSG_VERSION}-jammy_${ARCH}.run https://github.com/bdaiinstitute/bosdyn_msgs/releases/download/${MSG_VERSION}/ros-humble-bosdyn_msgs_${MSG_VERSION}-jammy_${ARCH}.run
chmod +x /tmp/ros-humble-bosdyn_msgs_${MSG_VERSION}-jammy_${ARCH}.run
sudo /tmp/ros-humble-bosdyn_msgs_${MSG_VERSION}-jammy_${ARCH}.run
rm /tmp/ros-humble-bosdyn_msgs_${MSG_VERSION}-jammy_${ARCH}.run

if $ARM; then
# Install bosdyn_msgs - automatic conversions of BD protobufs to ROS messages
wget -q -O /tmp/ros-humble-bosdyn-msgs_3.3.2-0jammy_arm64.deb https://github.com/bdaiinstitute/bosdyn_msgs/releases/download/v3.3.2-ARM64/ros-humble-bosdyn-msgs_3.3.2-0jammy_arm64.deb
sudo dpkg -i /tmp/ros-humble-bosdyn-msgs_3.3.2-0jammy_arm64.deb
rm /tmp/ros-humble-bosdyn-msgs_3.3.2-0jammy_arm64.deb

# Install spot-cpp-sdk
wget -q -O /tmp/spot-cpp-sdk_3.3.2_arm64.deb https://github.com/bdaiinstitute/spot-cpp-sdk/releases/download/3.3.2/spot-cpp-sdk_3.3.2_arm64.deb
sudo dpkg -i /tmp/spot-cpp-sdk_3.3.2_arm64.deb
rm /tmp/spot-cpp-sdk_3.3.2_arm64.deb
else
# Install bosdyn_msgs - automatic conversions of BD protobufs to ROS messages
wget -q -O /tmp/ros-humble-bosdyn-msgs_3.3.2-0jammy_amd64.deb https://github.com/bdaiinstitute/bosdyn_msgs/releases/download/v3.3.2-AMD64/ros-humble-bosdyn-msgs_3.3.2-0jammy_amd64.deb
sudo dpkg -i /tmp/ros-humble-bosdyn-msgs_3.3.2-0jammy_amd64.deb
rm /tmp/ros-humble-bosdyn-msgs_3.3.2-0jammy_amd64.deb

# Install spot-cpp-sdk
wget -q -O /tmp/spot-cpp-sdk_3.3.2_amd64.deb https://github.com/bdaiinstitute/spot-cpp-sdk/releases/download/3.3.2/spot-cpp-sdk_3.3.2_amd64.deb
sudo dpkg -i /tmp/spot-cpp-sdk_3.3.2_amd64.deb
rm /tmp/spot-cpp-sdk_3.3.2_amd64.deb
fi
# Install spot-cpp-sdk
wget -q -O /tmp/spot-cpp-sdk_${SDK_VERSION}_${ARCH}.deb https://github.com/bdaiinstitute/spot-cpp-sdk/releases/download/${SDK_VERSION}/spot-cpp-sdk_${SDK_VERSION}_${ARCH}.deb
sudo dpkg -i /tmp/spot-cpp-sdk_${SDK_VERSION}_${ARCH}.deb
rm /tmp/spot-cpp-sdk_${SDK_VERSION}_${ARCH}.deb
2 changes: 2 additions & 0 deletions spot_driver/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,8 @@ endif()
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

set(THIS_PACKAGE_INCLUDE_ROS_DEPENDS
bosdyn_api_msgs
bosdyn_spot_api_msgs
cv_bridge
geometry_msgs
rclcpp
Expand Down
19 changes: 9 additions & 10 deletions spot_driver/include/spot_driver/conversions/common_conversions.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,10 @@
#include <google/protobuf/timestamp.pb.h>
#include <google/protobuf/wrappers.pb.h>

#include <bosdyn_msgs/msg/arm_joint_position.hpp>
#include <bosdyn_msgs/msg/common_error.hpp>
#include <bosdyn_msgs/msg/quaternion.hpp>
#include <bosdyn_msgs/msg/request_header.hpp>
#include <bosdyn_msgs/msg/response_header.hpp>
#include <bosdyn_api_msgs/msg/arm_joint_position.hpp>
#include <bosdyn_api_msgs/msg/common_error.hpp>
#include <bosdyn_api_msgs/msg/request_header.hpp>
#include <bosdyn_api_msgs/msg/response_header.hpp>

#include <bosdyn/api/arm_command.pb.h>
#include <bosdyn/api/geometry.pb.h>
Expand All @@ -28,7 +27,7 @@ namespace spot_ros2 {

void convertToProto(const builtin_interfaces::msg::Time& ros_msg, google::protobuf::Timestamp& proto);

void convertToProto(const bosdyn_msgs::msg::RequestHeader& ros_msg, bosdyn::api::RequestHeader& proto);
void convertToProto(const bosdyn_api_msgs::msg::RequestHeader& ros_msg, bosdyn::api::RequestHeader& proto);

void convertToProto(const geometry_msgs::msg::Vector3& ros_msg, bosdyn::api::Vec3& proto);

Expand All @@ -40,16 +39,16 @@ void convertToProto(const geometry_msgs::msg::Pose& ros_msg, bosdyn::api::SE3Pos

void convertToProto(const double& ros_msg, google::protobuf::DoubleValue& proto);

void convertToProto(const bosdyn_msgs::msg::ArmJointPosition& ros_msg, bosdyn::api::ArmJointPosition& proto);
void convertToProto(const bosdyn_api_msgs::msg::ArmJointPosition& ros_msg, bosdyn::api::ArmJointPosition& proto);

///////////////////////////////////////////////////////////////////////////////
// Protobuf to ROS.

void convertToRos(const bosdyn::api::RequestHeader& proto, bosdyn_msgs::msg::RequestHeader& ros_msg);
void convertToRos(const bosdyn::api::RequestHeader& proto, bosdyn_api_msgs::msg::RequestHeader& ros_msg);

void convertToRos(const bosdyn::api::CommonError& proto, bosdyn_msgs::msg::CommonError& ros_msg);
void convertToRos(const bosdyn::api::CommonError& proto, bosdyn_api_msgs::msg::CommonError& ros_msg);

void convertToRos(const bosdyn::api::ResponseHeader& proto, bosdyn_msgs::msg::ResponseHeader& ros_msg);
void convertToRos(const bosdyn::api::ResponseHeader& proto, bosdyn_api_msgs::msg::ResponseHeader& ros_msg);

void convertToRos(const google::protobuf::Timestamp& proto, builtin_interfaces::msg::Time& ros_msg);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,57 +4,57 @@

#include <bosdyn/api/spot/inverse_kinematics.pb.h>

#include <bosdyn_msgs/msg/inverse_kinematics_request.hpp>
#include <bosdyn_msgs/msg/inverse_kinematics_response.hpp>
#include <bosdyn_spot_api_msgs/msg/inverse_kinematics_request.hpp>
#include <bosdyn_spot_api_msgs/msg/inverse_kinematics_response.hpp>

namespace spot_ros2 {

///////////////////////////////////////////////////////////////////////////////
// ROS to Protobuf.

void convertToProto(const bosdyn_msgs::msg::InverseKinematicsRequestFixedStance& ros_msg,
void convertToProto(const bosdyn_spot_api_msgs::msg::InverseKinematicsRequestFixedStance& ros_msg,
bosdyn::api::spot::InverseKinematicsRequest::FixedStance& proto);

void convertToProto(const bosdyn_msgs::msg::InverseKinematicsRequestOnGroundPlaneStance& ros_msg,
void convertToProto(const bosdyn_spot_api_msgs::msg::InverseKinematicsRequestOnGroundPlaneStance& ros_msg,
bosdyn::api::spot::InverseKinematicsRequest::OnGroundPlaneStance& proto);

void convertToProto(const bosdyn_msgs::msg::InverseKinematicsRequestOneOfStanceSpecification& ros_msg,
void convertToProto(const bosdyn_spot_api_msgs::msg::InverseKinematicsRequestOneOfStanceSpecification& ros_msg,
bosdyn::api::spot::InverseKinematicsRequest& proto);

void convertToProto(const bosdyn_msgs::msg::InverseKinematicsRequestWristMountedTool& ros_msg,
void convertToProto(const bosdyn_spot_api_msgs::msg::InverseKinematicsRequestWristMountedTool& ros_msg,
bosdyn::api::spot::InverseKinematicsRequest::WristMountedTool& proto);

void convertToProto(const bosdyn_msgs::msg::InverseKinematicsRequestBodyMountedTool& ros_msg,
void convertToProto(const bosdyn_spot_api_msgs::msg::InverseKinematicsRequestBodyMountedTool& ros_msg,
bosdyn::api::spot::InverseKinematicsRequest::BodyMountedTool& proto);

void convertToProto(const bosdyn_msgs::msg::InverseKinematicsRequestOneOfToolSpecification& ros_msg,
void convertToProto(const bosdyn_spot_api_msgs::msg::InverseKinematicsRequestOneOfToolSpecification& ros_msg,
bosdyn::api::spot::InverseKinematicsRequest& proto);

void convertToProto(const bosdyn_msgs::msg::InverseKinematicsRequestToolPoseTask& ros_msg,
void convertToProto(const bosdyn_spot_api_msgs::msg::InverseKinematicsRequestToolPoseTask& ros_msg,
bosdyn::api::spot::InverseKinematicsRequest::ToolPoseTask& proto);

void convertToProto(const bosdyn_msgs::msg::InverseKinematicsRequestToolGazeTask& ros_msg,
void convertToProto(const bosdyn_spot_api_msgs::msg::InverseKinematicsRequestToolGazeTask& ros_msg,
bosdyn::api::spot::InverseKinematicsRequest::ToolGazeTask& proto);

void convertToProto(const bosdyn_msgs::msg::InverseKinematicsRequestOneOfTaskSpecification& ros_msg,
void convertToProto(const bosdyn_spot_api_msgs::msg::InverseKinematicsRequestOneOfTaskSpecification& ros_msg,
bosdyn::api::spot::InverseKinematicsRequest& proto);

void convertToProto(const bosdyn_msgs::msg::InverseKinematicsRequest& ros_msg,
void convertToProto(const bosdyn_spot_api_msgs::msg::InverseKinematicsRequest& ros_msg,
bosdyn::api::spot::InverseKinematicsRequest& proto);

///////////////////////////////////////////////////////////////////////////////
// Protobuf to ROS.

void convertToRos(const bosdyn::api::JointState& proto, bosdyn_msgs::msg::JointState& ros_msg);
void convertToRos(const bosdyn::api::JointState& proto, bosdyn_api_msgs::msg::JointState& ros_msg);

void convertToRos(const bosdyn::api::FrameTreeSnapshot::ParentEdge& proto,
bosdyn_msgs::msg::FrameTreeSnapshotParentEdge& ros_msg);
bosdyn_api_msgs::msg::FrameTreeSnapshotParentEdge& ros_msg);

void convertToRos(const bosdyn::api::FrameTreeSnapshot& proto, bosdyn_msgs::msg::FrameTreeSnapshot& ros_msg);
void convertToRos(const bosdyn::api::FrameTreeSnapshot& proto, bosdyn_api_msgs::msg::FrameTreeSnapshot& ros_msg);

void convertToRos(const bosdyn::api::KinematicState& proto, bosdyn_msgs::msg::KinematicState& ros_msg);
void convertToRos(const bosdyn::api::KinematicState& proto, bosdyn_api_msgs::msg::KinematicState& ros_msg);

void convertToRos(const bosdyn::api::spot::InverseKinematicsResponse& proto,
bosdyn_msgs::msg::InverseKinematicsResponse& ros_msg);
bosdyn_spot_api_msgs::msg::InverseKinematicsResponse& ros_msg);

} // namespace spot_ros2
4 changes: 2 additions & 2 deletions spot_driver/include/spot_driver/conversions/robot_state.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
#pragma once

#include <bosdyn/api/robot_state.pb.h>
#include <bosdyn_msgs/msg/manipulator_state.hpp>
#include <bosdyn_api_msgs/msg/manipulator_state.hpp>
#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/vector3_stamped.hpp>
#include <map>
Expand Down Expand Up @@ -162,7 +162,7 @@ std::optional<spot_msgs::msg::SystemFaultState> getSystemFaultState(const ::bosd
* @return If the robot state message contains data about Spot's manipulator state, return a ManipulatorState message
* containing this data. Otherwise, return nullopt.
*/
std::optional<bosdyn_msgs::msg::ManipulatorState> getManipulatorState(const ::bosdyn::api::RobotState& robot_state);
std::optional<bosdyn_api_msgs::msg::ManipulatorState> getManipulatorState(const ::bosdyn::api::RobotState& robot_state);

/**
* @brief Create a Vector3Stamped ROS message representing the force exerted on Spot's end effector by parsing a
Expand Down
Loading

0 comments on commit d21055f

Please sign in to comment.