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 &base_link, const std::string &tip_link, IkSolveFunction ik_solve_fn)

Constructs the IK solver and marker.

Parameters:

• scene – A fully configured RoboPlan Scene.

• base_link – Base link of the IK chain.

• tip_link – Tip link of the IK chain.

• solve_fn – Callback invoked on each marker pose update to solve IK.

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 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.

IkSolveFunction ik_solve_fn_

Function used to solve IK.

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::string &group_name = "", 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.

• group_name – Default joint group to render. When empty, the entire scene is rendered. Otherwise, only the geometries of to the named group’s links are emitted.

• 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. Only the geometry belonging to the currently selected group (see the constructor and set_group) is emitted, which is useful for cutting down on visual noise. The group’s geometry selection is cached, so this is cheap to call repeatedly.

Parameters:

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

Returns:

MarkerArray with one marker per supported visual geometry in the selection.

void set_group(const std::string &group_name)

Select the joint group whose links should be rendered.

Recomputes and caches the geometry selection for the given group. Pass an empty string to render the entire scene. A group’s links are derived from the scene’s JointGroupInfo.

Parameters:

group_name – The joint group name, or an empty string for the whole scene.

Throws:

std::runtime_error – if the group name is not found in the scene.

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::vector<std::size_t> geometry_indices_for_group(const std::string &group_name) const

Determine which visual geometry objects belong to a joint group.

Parameters:

group_name – The joint group name, or an empty string for the whole scene.

Returns:

The indices into visual_model_.geometryObjects that should be rendered. For an empty group name, every geometry index in the model is returned.

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::string group_name_

Currently selected joint group to render. Empty means render the entire scene.

std::vector<std::size_t> geometry_indices_

Cached geometry indices for the selected group. Recomputed only when the group changes (construction or set_group), so rendering does not redo the selection each call.

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

Optionally set to override colors of all markers before rendering.

namespace roboplan_ros_visualization

Typedefs

using IkSolveFunction = std::function<std::optional<Eigen::VectorXd>(const Eigen::Matrix4d &target_pose, const Eigen::VectorXd &seed_configuration)>

Callback type for IK solving.

Param target_pose:

The desired end-effector pose in the base_link frame.

Param seed_configuration:

The full-model joint configuration to seed the solve from.

Return:

Full joint positions on success, std::nullopt on failure.

Functions

visualization_msgs::msg::Marker markerFromJointTrajectory(const roboplan::Scene &scene, const roboplan::JointTrajectory &trajectory, const std::vector<std::string> &frame_names, const std::string &frame_id = "world", const std::string &ns = "/roboplan_path", const std::optional<std_msgs::msg::ColorRGBA> &color = std::nullopt, double line_width = 0.01)

Builds a RViz line-list marker tracing a joint trajectory in Cartesian space.

Runs forward kinematics on each of the trajectory’s positions (via roboplan::computeFramePath) for every requested frame and connects the resulting frame origins with line segments. Each frame name is traced independently, so the same marker can show the Cartesian path of multiple end effectors at once. The trajectory’s positions are group positions (in the order of trajectory.joint_names), so they are embedded into a full configuration vector seeded from the scene’s current state before computing kinematics.

Parameters:

• scene – The scene used for forward kinematics.

• trajectory – The joint trajectory to trace. An empty trajectory yields an empty marker.

• frame_names – The frames (e.g., end effectors) whose origins are traced.

• frame_id – The frame_id written into the marker header.

• ns – The marker namespace.

• color – Optional line color. Defaults to opaque white when not provided.

• line_width – The rendered line width, in meters (Marker::scale.x).

Returns:

A LINE_LIST marker containing the traced segments for every requested frame.

file path_visualization.hpp

#include <optional>

#include <string>

#include <vector>

#include <roboplan/core/scene.hpp>

#include <roboplan/core/types.hpp>

#include <std_msgs/msg/color_rgba.hpp>

#include <visualization_msgs/msg/marker.hpp>

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