C++ API Reference

roboplan_ros_cpp

struct JointMapping

#include <type_conversions.hpp>

Mapping for an individual joint.

Public Members

std::string joint_name

the String name of the joint.

size_t ros_index

Index in the ROS JointState type.

size_t q_start

The start index in the positions vector.

size_t v_start

The start index in velocities vector.

roboplan::JointType type

The RoboPlan type of the Joint.

struct JointStateConverterMap

#include <type_conversions.hpp>

Pre-computed mapping for ROS JointState to RoboPlan type conversions.

JointState messages may be in different orders, will not contain mimic states, and have different representation of continuous types. This structure maintains a mapping from ROS JointStates to RoboPlan JointConfigurations to enable efficient conversion from one type to the other.

Public Members

std::vector<JointMapping> mappings

Index of JointState joints in the Scene.

size_t nq

Number of position states in the Scene.

size_t nv

Numbef of velocity states in the Scene.

namespace roboplan_ros_cpp

Functions

tl::expected<JointStateConverterMap, std::string> buildConversionMap(const roboplan::Scene &scene, const sensor_msgs::msg::JointState &joint_state)

Constructs a JointState conversion map given a RoboPlan scene and JointState message.

Parameters:

• scene – The RoboPlan Scene.

• joint_state – Sample ROS joint_state message.

Returns:

The joint information struct if successful, else a string describing the error.

inline builtin_interfaces::msg::Duration toDuration(const double time_sec)

Converts a double timestamp to an equivalent ROS Duration.

Parameters:

time_sec – Timestamp in seconds.

Returns:

ROS 2 Duration with seconds and nanoseconds.

inline double fromDuration(const builtin_interfaces::msg::Duration &duration)

Converts a ROS 2 Duration with seconds and nanoseconds to an equivalent double timestamp.

Parameters:

duration – ROS 2 Duration with seconds and nanoseconds.

Returns:

An equivalent double timestamp.

tl::expected<sensor_msgs::msg::JointState, std::string> toJointState(const roboplan::JointConfiguration &config, const roboplan::Scene &scene)

Converts a roboplan::JointConfiguration object to a ROS 2 JointState message.

Parameters:

• config – The roboplan JointConfiguration to convert

• scene – The RoboPlan scene containing the model’s joint information.

Returns:

An equivalent ROS 2 JointState message, or an error.

tl::expected<roboplan::JointConfiguration, std::string> fromJointState(const sensor_msgs::msg::JointState &joint_state, const roboplan::Scene &scene, const JointStateConverterMap &joint_conversion_map)

Convert the provided ROS 2 JointState message to an equivalent roboplan::JointConfiguration.

Parameters:

• joint_state – The ROS 2 JointState message to convert.

• scene – The RoboPlan scene containing the model’s joint information.

• joint_conversion_map – A mapping of joint names to their indexes in the model.

Returns:

An equivalent roboplan::JointConfiguration, or an error.

trajectory_msgs::msg::JointTrajectory toJointTrajectory(const roboplan::JointTrajectory &roboplan_trajectory)

Converts a roboplan::JointTrajectory object to a ROS 2 JointTrajectory message.

This function will convert joint names and joint trajectory points. The caller is responsible for any additional configuration that is required in the message.

Parameters:

roboplan_trajectory – The roboplan JointTrajectory to convert

Returns:

An equivalent ROS 2 JointTrajectory message.

roboplan::JointTrajectory fromJointTrajectory(const trajectory_msgs::msg::JointTrajectory &ros_trajectory)

Convert the provided ROS 2 JointTrajectory message to an equivalent roboplan::JointTrajectory.

Parameters:

ros_trajectory – The ROS 2 JointTrajectory message to convert.

Returns:

An equivalent roboplan::JointTrajectory.

geometry_msgs::msg::TransformStamped toTransformStamped(const roboplan::CartesianConfiguration &cartesian_configuration)

Converts a roboplan::CartesianConfiguration to a ROS 2 TransformStamped message.

Parameters:

cartesian_configuration – The roboplan CartesianConfiguration to convert.

Returns:

An equivalent geometry_msgs::msg::TransformStamped.

roboplan::CartesianConfiguration fromTransformStamped(const geometry_msgs::msg::TransformStamped &transform)

Converts a geometry_msgs::msg::TransformStamped to a roboplan CartesianConfiguration.

Parameters:

transform – The ROS 2 TransformStamped message to convert.

Returns:

An equivalent roboplan::CartesianConfiguration.

Eigen::Matrix4d poseToSE3(const geometry_msgs::msg::Pose &pose)

Convert ROS Pose to a 4x4 SE3 transformation matrix.

Parameters:

pose – ROS Pose message

Returns:

4x4 Eigen matrix representing the SE3 transformation

geometry_msgs::msg::Pose se3ToPose(const Eigen::Matrix4d &transform)

Convert a 4x4 SE3 transformation matrix to ROS Pose.

Parameters:

transform – 4x4 Eigen matrix representing an SE3 transformation

Returns:

An equivalent ROS Pose message

file type_conversions.hpp

#include <Eigen/Dense>

#include <geometry_msgs/msg/pose.hpp>

#include <geometry_msgs/msg/transform_stamped.hpp>

#include <pinocchio/spatial/se3.hpp>

#include <roboplan/core/scene.hpp>

#include <roboplan/core/types.hpp>

#include <sensor_msgs/msg/joint_state.hpp>

#include <tl/expected.hpp>

#include <trajectory_msgs/msg/joint_trajectory.hpp>

#include <trajectory_msgs/msg/joint_trajectory_point.hpp>

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan-ros/checkouts/latest/roboplan_ros_cpp/include

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan-ros/checkouts/latest/roboplan_ros_cpp

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan-ros/checkouts/latest/roboplan_ros_cpp/include/roboplan_ros_cpp

roboplan_ros_visualization

class RoboplanIKMarker

#include <roboplan_ik_marker.hpp>

Solves IK for a target pose within a joint group with interactive marker feedback.

Can be used to generate IK solutions using feedback from an interactive marker server. The consumer is responsible for connecting this to the appropriate ROS infrastructure, as needed for the specific application.

Public Functions

RoboplanIKMarker(std::shared_ptr<const roboplan::Scene> scene, const std::string &joint_group, const std::string &base_link, const std::string &tip_link, const roboplan::SimpleIkOptions &options = roboplan::SimpleIkOptions())

Constructs the IK solver and marker.

Parameters:

• scene – A fully configured RoboPlan Scene.

• joint_group – The joint group name for the IK solver.

• base_link – Base link of the IK chain.

• tip_link – Tip link of the IK chain.

• options – Options for the IK solver.

visualization_msgs::msg::InteractiveMarker construct_imarker() const

Build an InteractiveMarker message for the current target pose.

Returns:

A configured InteractiveMarker with 6-DOF controls.

std::optional<Eigen::VectorXd> process_feedback(const visualization_msgs::msg::InteractiveMarkerFeedback &feedback)

Process feedback from an InteractiveMarkerServer.

Parameters:

feedback – The feedback message from the interactive marker server.

Returns:

Joint positions if IK succeeded, std::nullopt otherwise.

void set_seed_configuration(const Eigen::VectorXd &q)

Sets the seed for the next IK solve (assumes the full configuration).

Private Members

std::shared_ptr<const roboplan::Scene> scene_

Shared ptr to avoid ownership issues between C++ and Python.

std::string joint_group_

The name of the joint group in the scene.

std::string base_link_

The base link used for the IK solver, and frame for the marker header.

std::string tip_link_

@ brief The tip link of the IK solver chain.

Eigen::VectorXi q_indices_

Indices into the full configuration vector for this group.

std::unique_ptr<roboplan::SimpleIk> ik_solver_

The underlying IK solver.

geometry_msgs::msg::Pose target_pose_

Current target pose for IK.

Eigen::VectorXd seed_configuration_

Set the seed for the IK solver.

class RoboplanVisualizer

#include <roboplan_visualizer.hpp>

Computes RViz visualization markers for a robot configuration.

Uses the Pinocchio model from a Scene and builds a visual GeometryModel from the URDF to produce marker messages directly, no ROS node, TF, or robot_state_publisher required.

Public Functions

RoboplanVisualizer(std::shared_ptr<const roboplan::Scene> scene, const std::string &urdf_xml, const std::string &frame_id = "world", const std::string &ns = "/roboplan", const std::optional<std_msgs::msg::ColorRGBA> &color = std::nullopt)

Construct the visualizer.

Parameters:

• scene – A fully configured RoboPlan Scene for model reference.

• urdf_xml – URDF XML string (needed to build the visual geometry model, since Scene only holds the collision geometry model).

• frame_id – The frame_id written into every marker header.

• ns – Marker namespace prefix.

• color – Optional override color applied to every marker.

visualization_msgs::msg::MarkerArray markers_from_configuration(const Eigen::VectorXd &q) const

Compute visualization markers for the given joint configuration.

Runs Pinocchio geometry placement updates and returns a MarkerArray that the caller can publish however they like.

Parameters:

q – Joint positions (size must match the scene model’s nq).

Returns:

MarkerArray with one marker per supported visual geometry.

void set_color(const std_msgs::msg::ColorRGBA &color)

Override the color applied to every marker.

void clear_color()

Remove any color override so per-geometry colors are used.

Public Static Functions

static visualization_msgs::msg::MarkerArray clear_markers()

Build a MarkerArray containing a single DELETEALL marker.

Private Functions

std::optional<visualization_msgs::msg::Marker> create_geometry_marker(int marker_id, const pinocchio::GeometryObject &geom_obj, const pinocchio::SE3 &placement) const

Create a visualization marker for a single geometry object.

Parameters:

• marker_id – Unique marker ID.

• geom_obj – The Pinocchio geometry object to visualize.

• placement – The SE3 placement of the geometry in world frame.

Returns:

A marker if the geometry type is supported, std::nullopt otherwise.

Private Members

std::shared_ptr<const roboplan::Scene> scene_

Shared ptr to avoid ownership issues between C++ and Python.

pinocchio::GeometryModel visual_model_

Visual geometry model built from URDF.

std::string frame_id_

Frame id for the marker headers.

std::string ns_

Namespace for the generated markers.

std::optional<std_msgs::msg::ColorRGBA> color_

Optionally set to override colors of all markers before rendering.

namespace roboplan_ros_visualization

file roboplan_ik_marker.hpp

#include <functional>

#include <memory>

#include <optional>

#include <string>

#include <vector>

#include <Eigen/Dense>

#include <geometry_msgs/msg/pose.hpp>

#include <roboplan/core/scene.hpp>

#include <roboplan_simple_ik/simple_ik.hpp>

#include <visualization_msgs/msg/interactive_marker.hpp>

#include <visualization_msgs/msg/interactive_marker_control.hpp>

#include <visualization_msgs/msg/interactive_marker_feedback.hpp>

#include <visualization_msgs/msg/marker.hpp>

file roboplan_visualizer.hpp

#include <memory>

#include <optional>

#include <string>

#include <vector>

#include <Eigen/Dense>

#include <pinocchio/multibody/geometry.hpp>

#include <roboplan/core/scene.hpp>

#include <std_msgs/msg/color_rgba.hpp>

#include <visualization_msgs/msg/marker.hpp>

#include <visualization_msgs/msg/marker_array.hpp>

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan-ros/checkouts/latest/roboplan_ros_visualization/include

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan-ros/checkouts/latest/roboplan_ros_visualization

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan-ros/checkouts/latest/roboplan_ros_visualization/include/roboplan_ros_visualization