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.

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

[EK-25] Use bosdyn_msgs bundles #234

Merged
merged 22 commits into from
Mar 6, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
22 commits
Select commit Hold shift + click to select a range
b3cb677
Update spot_msgs to depend on new bosdyn_msgs packages
mhidalgo-bdai Jan 19, 2024
2e98a43
Update spot_driver to depend on new bosdyn_msgs packages
mhidalgo-bdai Jan 19, 2024
6878f1e
Update examples to depend on new bosdyn_msgs packages
mhidalgo-bdai Jan 19, 2024
bb3243a
Update installation procedures to use bosdyn_msgs bundles
mhidalgo-bdai Jan 19, 2024
eb514a6
Please ruff
mhidalgo-bdai Jan 19, 2024
6d053b4
Peg Spot SDK version in install scripts
mhidalgo-bdai Jan 23, 2024
c8190bc
Auto-confirm bundle install in Dockerfile
mhidalgo-bdai Jan 29, 2024
e886887
Migrate new conversion calls
mhidalgo-bdai Feb 20, 2024
04a760a
Drop spot_driver_cpp dependency
mhidalgo-bdai Feb 20, 2024
8b8a50c
Bump Spot SDK version to 4.0.0
mhidalgo-bdai Feb 29, 2024
f0c488e
Update spot_wrapper submodule
mhidalgo-bdai Feb 29, 2024
2ab53cf
Repair after rebase
mhidalgo-bdai Feb 29, 2024
e0899e7
Restore manual conversions
mhidalgo-bdai Mar 1, 2024
d47fc15
Migrate missing examples
mhidalgo-bdai Mar 1, 2024
28561a6
Please coveralls
mhidalgo-bdai Mar 1, 2024
8f6e9f3
Please linters
mhidalgo-bdai Mar 1, 2024
90c61a2
Merge branch 'main' into use-bosdyn_msgs-bundles
mhidalgo-bdai Mar 1, 2024
92e07fa
Bump bosdyn_msgs release version
mhidalgo-bdai Mar 6, 2024
8abe96e
Merge branch 'main' into use-bosdyn_msgs-bundles
mhidalgo-bdai Mar 6, 2024
6665599
Fix bad bundle name
mhidalgo-bdai Mar 6, 2024
4c3ca8b
Update submodules
mhidalgo-bdai Mar 6, 2024
b35dbd3
Merge branch 'main' into use-bosdyn_msgs-bundles
mhidalgo-bdai Mar 6, 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
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"
mhidalgo-bdai marked this conversation as resolved.
Show resolved Hide resolved
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
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
Loading