Class MoveItVisualTools

Defined in File moveit_visual_tools.h

Inheritance Relationships

Base Type

public rviz_visual_tools::RvizVisualTools

Class Documentation

class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools

Public Functions

MoveItVisualTools(const rclcpp::Node::SharedPtr &node)

Constructor.

All Markers will be rendered in the planning frame of the model ROBOT_DESCRIPTION and are published to rviz_visual_tools::RVIZ_MARKER_TOPIC

MoveItVisualTools(const rclcpp::Node::SharedPtr &node, const std::string &base_frame, const std::string &marker_topic, planning_scene_monitor::PlanningSceneMonitorPtr psm)

Constructor.

Parameters:

base_frame – - common base for all visualization markers, usually “/world” or “/odom”
marker_topic – - rostopic to publish markers to - your Rviz display should match
planning_scene_monitor – - optionally pass in a pre-loaded planning scene monitor to avoid having to re-load the URDF, kinematic solvers, etc

MoveItVisualTools(const rclcpp::Node::SharedPtr &node, const std::string &base_frame, const std::string &marker_topic = rviz_visual_tools::RVIZ_MARKER_TOPIC, moveit::core::RobotModelConstPtr robot_model = moveit::core::RobotModelConstPtr())

Constructor.

Parameters:

base_frame – - common base for all visualization markers, usually “/world” or “/odom”
marker_topic – - rostopic to publish markers to - your Rviz display should match
robot_model – - load robot model pointer so that we don’t have do re-parse it here

inline void setRobotStateTopic(const std::string &robot_state_topic)

Set the ROS topic for publishing a robot state.

Parameters:: topic –

inline void setPlanningSceneTopic(const std::string &planning_scene_topic)

Set the planning scene topic.

Parameters:: topic –

bool loadPlanningSceneMonitor()

Load a planning scene monitor if one was not passed into the constructor.

Returns:: true if successful in loading

bool processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject &msg, const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

Skip a ROS message call by sending directly to planning scene monitor.

Parameters:

collision – object message
color – to display the collision object with

Returns:

true on success

bool processAttachedCollisionObjectMsg(const moveit_msgs::msg::AttachedCollisionObject &msg)

Skip a ROS message call by sending directly to planning scene monitor.

Parameters:: attached – collision object message
Returns:: true on success

bool moveCollisionObject(const Eigen::Isometry3d &pose, const std::string &name, const rviz_visual_tools::Colors &color)

Move an already published collision object to a new locaiton in space.

Parameters:

pose – - location of center of object
name – - semantic name of MoveIt collision object

Returns:

true on success

bool moveCollisionObject(const geometry_msgs::msg::Pose &pose, const std::string &name, const rviz_visual_tools::Colors &color)

bool triggerPlanningSceneUpdate(): When manual_trigger_update_ is true, use this to tell the planning scene to send an update out. Do not use otherwise.

bool loadSharedRobotState()

Load robot state only as needed.

Returns:: true if successful in loading

moveit::core::RobotStatePtr &getSharedRobotState()

Allow robot state to be altered.

Returns:: shared pointer to robot state

inline moveit::core::RobotStatePtr &getRootRobotState()

Allow robot state to be altered.

Returns:: shared pointer to robot state

moveit::core::RobotModelConstPtr getRobotModel()

Get a pointer to the robot model.

Returns:: const RobotModel

bool loadEEMarker(const moveit::core::JointModelGroup *ee_jmg, const std::vector<double> &ee_joint_pos = {})

Call this once at begining to load the end effector markers and then whenever a joint changes.

Parameters:

ee_jmg – - the set of joints to use, e.g. the MoveIt planning group, e.g. “left_gripper”
ee_joint_pos – - the values of all active joints in this planning group

Returns:

true if it is successful

void loadTrajectoryPub(const std::string &display_planned_path_topic = DISPLAY_PLANNED_PATH_TOPIC, bool blocking = true): Load publishers as needed.

void loadRobotStatePub(const std::string &robot_state_topic = DISPLAY_ROBOT_STATE_TOPIC, bool blocking = true)

inline void setPlanningSceneMonitor(planning_scene_monitor::PlanningSceneMonitorPtr psm)

Allow a pre-configured planning scene monitor to be set for publishing collision objects, etc.

Parameters:: a – pointer to a load planning scene

inline void setManualSceneUpdating(bool enable_manual = true)

Prevent the planning scene from always auto-pushing, but rather do it manually.

Parameters:: bool – true to enable manual mode

inline bool publishEEMarkers(const Eigen::Isometry3d &pose, const moveit::core::JointModelGroup *ee_jmg, const std::vector<double> &ee_joint_pos, const rviz_visual_tools::Colors &color = rviz_visual_tools::DEFAULT, const std::string &ns = "end_effector")

Publish an end effector to rviz.

Parameters:

pose – - the location to publish the marker with respect to the base frame
ee_jmg – - joint model group of the end effector, e.g. “left_hand”
ee_joint_pos – - position of all active joints in the end effector
color – to display the collision object with

Returns:

true on success

inline bool publishEEMarkers(const Eigen::Isometry3d &pose, const moveit::core::JointModelGroup *ee_jmg, const rviz_visual_tools::Colors &color = rviz_visual_tools::DEFAULT, const std::string &ns = "end_effector")

inline bool publishEEMarkers(const geometry_msgs::msg::Pose &pose, const moveit::core::JointModelGroup *ee_jmg, const rviz_visual_tools::Colors &color = rviz_visual_tools::DEFAULT, const std::string &ns = "end_effector")

bool publishEEMarkers(const geometry_msgs::msg::Pose &pose, const moveit::core::JointModelGroup *ee_jmg, const std::vector<double> &ee_joint_pos, const rviz_visual_tools::Colors &color = rviz_visual_tools::DEFAULT, const std::string &ns = "end_effector")

bool publishGrasps(const std::vector<moveit_msgs::msg::Grasp> &possible_grasps, const moveit::core::JointModelGroup *ee_jmg, double animate_speed = 0.1)

Show grasps generated from moveit_simple_grasps or other MoveIt Grasp message sources.

Parameters:

possible_grasps – - a set of grasp positions to visualize
ee_jmg – - the set of joints to use, e.g. the MoveIt planning group, e.g. “left_arm”
animate_speed – - how fast the gripper approach is animated in seconds, optional

bool publishAnimatedGrasps(const std::vector<moveit_msgs::msg::Grasp> &possible_grasps, const moveit::core::JointModelGroup *ee_jmg, double animate_speed = 0.01)

Display an animated vector of grasps including its approach movement in Rviz Note this function calls publish() automatically in order to achieve animations.

Parameters:

possible_grasps – - a set of grasp positions to visualize
ee_jmg – - the set of joints to use, e.g. the MoveIt planning group, e.g. “left_arm”
animate_speed – - how fast the gripper approach is animated in seconds, optional

bool publishAnimatedGrasp(const moveit_msgs::msg::Grasp &grasp, const moveit::core::JointModelGroup *ee_jmg, double animate_speed)

Animate a single grasp in its movement direction Note this function calls publish() automatically in order to achieve animations.

Parameters:

grasp –
ee_jmg – - the set of joints to use, e.g. the MoveIt planning group, e.g. “left_arm”
animate_speed – - how fast the gripper approach is animated in seconds

Returns:

true on sucess

bool publishIKSolutions(const std::vector<trajectory_msgs::msg::JointTrajectoryPoint> &ik_solutions, const moveit::core::JointModelGroup *arm_jmg, double display_time = 0.4)

Display an vector of inverse kinematic solutions for the IK service in Rviz Note: this is published to the ‘Planned Path’ section of the ‘MotionPlanning’ display in Rviz.

Parameters:

ik_solutions – - a set of corresponding arm positions to achieve each grasp
arm_jmg – - the set of joints to use, e.g. the MoveIt planning group, e.g. “left_arm”
display_time – - amount of time to sleep between sending trajectories, optional

bool removeAllCollisionObjects()

Remove all collision objects that this class has added to the MoveIt planning scene Communicates directly to a planning scene monitor e.g. if this is the move_group node.

Parameters:: the – scene to directly clear the collision objects from
Returns:: true on sucess

bool cleanupCO(const std::string &name)

Remove a collision object from the planning scene.

Parameters:: Name – of object
Returns:: true on sucess

bool cleanupACO(const std::string &name)

Remove an active collision object from the planning scene.

Parameters:: Name – of object
Returns:: true on sucess

bool attachCO(const std::string &name, const std::string &ee_parent_link)

Attach a collision object from the planning scene.

Parameters:: Name – of object
Param :

bool publishCollisionFloor(double z = 0.0, const std::string &plane_name = "Floor", const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

Make the floor a collision object.

Parameters:

z – location of floor
name – of floor
color – to display the collision object with

Returns:

true on success

bool publishCollisionBlock(const geometry_msgs::msg::Pose &block_pose, const std::string &block_name = "block", double block_size = 0.1, const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

Create a MoveIt Collision block at the given pose.

Parameters:

pose – - location of center of block
name – - semantic name of MoveIt collision object
size – - height=width=depth=size
color – to display the collision object with

Returns:

true on sucess

bool publishCollisionCuboid(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2, const std::string &name, const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

Create a MoveIt collision rectangular cuboid at the given pose.

Parameters:

point1 – - top left of rectangle
point2 – - bottom right of rectangle
name – - semantic name of MoveIt collision object
color – to display the collision object with

Returns:

true on sucess

bool publishCollisionCuboid(const geometry_msgs::msg::Point &point1, const geometry_msgs::msg::Point &point2, const std::string &name, const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

bool publishCollisionCuboid(const Eigen::Isometry3d &pose, double width, double depth, double height, const std::string &name, const rviz_visual_tools::Colors &color)

Create a MoveIt collision rectangular cuboid at the given pose.

Parameters:

pose – - position of the centroid of the cube
width – - width of the object in its local frame
depth – - depth of the object in its local frame
height – - height of the object in its local frame
name – - semantic name of MoveIt collision object
color – to display the collision object with

Returns:

true on sucess

bool publishCollisionCuboid(const geometry_msgs::msg::Pose &pose, double width, double depth, double height, const std::string &name, const rviz_visual_tools::Colors &color)

inline bool publishCollisionCuboid(const Eigen::Isometry3d &pose, const Eigen::Vector3d &size, const std::string &name, const rviz_visual_tools::Colors &color)

Create a MoveIt collision rectangular cuboid at the given pose.

Parameters:

pose – - position of the centroid of the cube
size – - the size (x,y,z) of the object in its local frame
name – - semantic name of MoveIt collision object
color – to display the collision object with

Returns:

true on sucess

inline bool publishCollisionCuboid(const geometry_msgs::msg::Pose &pose, const geometry_msgs::msg::Vector3 &size, const std::string &name, const rviz_visual_tools::Colors &color)

bool publishCollisionCylinder(const geometry_msgs::msg::Point &a, const geometry_msgs::msg::Point &b, const std::string &object_name, double radius, const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

Create a MoveIt Collision cylinder between two points.

Parameters:

point – a - x,y,z in space of a point
point – b - x,y,z in space of a point
name – - semantic name of MoveIt collision object
radius – - size of cylinder
color – to display the collision object with

Returns:

true on sucess

bool publishCollisionCylinder(const Eigen::Vector3d &a, const Eigen::Vector3d &b, const std::string &object_name, double radius, const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

bool publishCollisionCylinder(const Eigen::Isometry3d &object_pose, const std::string &object_name, double radius, double height, const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

Create a MoveIt Collision cylinder with a center point pose.

Parameters:

pose – - vector pointing along axis of cylinder
name – - semantic name of MoveIt collision object
radius – - size of cylinder
height – - size of cylinder
color – to display the collision object with

Returns:

true on sucess

bool publishCollisionCylinder(const geometry_msgs::msg::Pose &object_pose, const std::string &object_name, double radius, double height, const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

bool publishCollisionMesh(const geometry_msgs::msg::Pose &object_pose, const std::string &object_name, const std::string &mesh_path, const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

Create a collision object using a mesh.

Parameters:

pose – - location of center of mesh
name – - semantic name of MoveIt collision object
peth – - file location to an .stl file
color – to display the collision object with

Returns:

true on success

bool publishCollisionMesh(const Eigen::Isometry3d &object_pose, const std::string &object_name, const std::string &mesh_path, const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

bool publishCollisionMesh(const Eigen::Isometry3d &object_pose, const std::string &object_name, const shape_msgs::msg::Mesh &mesh_msg, const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

bool publishCollisionMesh(const geometry_msgs::msg::Pose &object_pose, const std::string &object_name, const shape_msgs::msg::Mesh &mesh_msg, const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

bool publishCollisionGraph(const graph_msgs::msg::GeometryGraph &graph, const std::string &object_name, double radius, const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

Publish a connected birectional graph.

Parameters:

graph – of nodes and edges
name – of collision object
color – to display the collision object with

Returns:

true on sucess

void getCollisionWallMsg(double x, double y, double z, double angle, double width, double height, const std::string &name, moveit_msgs::msg::CollisionObject &collision_obj): Helper for publishCollisionWall.

bool publishCollisionWall(double x, double y, double angle = 0.0, double width = 2.0, double height = 1.5, const std::string &name = "wall", const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

Publish a typical room wall.

Parameters:

x –
y –
angle –
width –
height –
name –
color – to display the collision object with

Returns:

true on sucess

bool publishCollisionWall(double x, double y, double z, double angle = 0.0, double width = 2.0, double height = 1.5, const std::string &name = "wall", const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

bool publishCollisionTable(double x, double y, double z, double angle, double width, double height, double depth, const std::string &name, const rviz_visual_tools::Colors &color = rviz_visual_tools::GREEN)

Publish a typical room table.

Parameters:

x –
y –
angle –
width –
height –
depth –
name –
color – to display the collision object with

Returns:

true on sucess

bool loadCollisionSceneFromFile(const std::string &path)

Load a planning scene to a planning_scene_monitor from file.

Parameters:

path – - path to planning scene, e.g. as exported from Rviz Plugin
offset – for scene to be placed

Returns:

true on success

bool loadCollisionSceneFromFile(const std::string &path, const Eigen::Isometry3d &offset)

bool publishWorkspaceParameters(const moveit_msgs::msg::WorkspaceParameters &params)

Display size of workspace used for planning with OMPL, etc. Important for virtual joints.

Parameters:: display – bounds of workspace
Returns:: true on sucess

bool checkAndPublishCollision(const moveit::core::RobotState &robot_state, const planning_scene::PlanningScene *planning_scene, const rviz_visual_tools::Colors &highlight_link_color = rviz_visual_tools::RED, const rviz_visual_tools::Colors &contact_point_color = rviz_visual_tools::PURPLE)

Check if the robot state is in collision inside the planning scene and visualize the result. If the state is not colliding only the robot state is published. If there is a collision the colliding links are ghlighted and the contact points are visualized with markers.

Parameters:

robot_state – - The robot state to check for collisions
planning_scene – - The planning scene to use for collision checks
highlight_link_color – - The color to use for highligting colliding links
contact_point_color – - The color to use for contact point markers

Returns:

- True if there is a collision

bool publishContactPoints(const moveit::core::RobotState &robot_state, const planning_scene::PlanningScene *planning_scene, const rviz_visual_tools::Colors &color = rviz_visual_tools::RED)

Given a planning scene and robot state, publish any collisions.

Parameters:

robot_state –
planning_scene –
color – - display color of markers

Returns:

true on success

bool publishContactPoints(const collision_detection::CollisionResult::ContactMap &contacts, const planning_scene::PlanningScene *planning_scene, const rviz_visual_tools::Colors &color = rviz_visual_tools::RED)

Given a contact map and planning scene, publish the contact points.

Parameters:

contacts – - The populated contacts to visualize
planning_scene –
color – - display color of markers

Returns:

true on success

bool publishTrajectoryPoint(const trajectory_msgs::msg::JointTrajectoryPoint &trajectory_pt, const std::string &planning_group, double display_time = 0.1)

Move a joint group in MoveIt for visualization make sure you have already set the planning group name this assumes the trajectory_pt position is the size of the number of joints in the planning group This will be displayed in the Planned Path section of the MoveIt Rviz plugin.

Parameters:

trajectory_pts – - a single joint configuration
planning_group – - the MoveIt planning group the trajectory applies to
display_time – - amount of time for the trajectory to “execute”

Returns:

true on success

bool publishTrajectoryPath(const std::vector<moveit::core::RobotStatePtr> &trajectory, const moveit::core::JointModelGroup *jmg, double speed = 0.01, bool blocking = false)

Animate trajectory in rviz. These functions do not need a trigger() called because use different publisher.

Parameters:

trajectory – the actual plan
blocking – whether we need to wait for the animation to complete
robot_state – - the base state to add the joint trajectory message to
ee_jmg – - the set of joints to use, e.g. the MoveIt planning group, e.g. “left_arm”

Returns:

true on success

bool publishTrajectoryPath(const robot_trajectory::RobotTrajectoryPtr &trajectory, bool blocking = false)

bool publishTrajectoryPath(const robot_trajectory::RobotTrajectory &trajectory, bool blocking = false)

bool publishTrajectoryPath(const moveit_msgs::msg::RobotTrajectory &trajectory_msg, const moveit::core::RobotStateConstPtr &robot_state, bool blocking = false)

bool publishTrajectoryPath(const moveit_msgs::msg::RobotTrajectory &trajectory_msg, const moveit::core::RobotState &robot_state, bool blocking = false)

bool publishTrajectoryPath(const moveit_msgs::msg::RobotTrajectory &trajectory_msg, const moveit_msgs::msg::RobotState &robot_state, bool blocking = false)

void publishTrajectoryPath(const moveit_msgs::msg::DisplayTrajectory &display_trajectory_msg)

bool publishTrajectoryLine(const moveit_msgs::msg::RobotTrajectory &trajectory_msg, const moveit::core::LinkModel *ee_parent_link, const moveit::core::JointModelGroup *arm_jmg, const rviz_visual_tools::Colors &color = rviz_visual_tools::LIME_GREEN)

Display a line of the end effector path from a robot trajectory path.

Parameters:

trajectory_msg – - the robot plan
ee_parent_link – - the link that we should trace a path of, e.g. the gripper link
arm_jmg – - the set of joints to use, e.g. the MoveIt planning group, e.g. “left_arm”
color – - display color of markers

Returns:

true on success

bool publishTrajectoryLine(const robot_trajectory::RobotTrajectoryPtr &robot_trajectory, const moveit::core::LinkModel *ee_parent_link, const rviz_visual_tools::Colors &color = rviz_visual_tools::LIME_GREEN)

bool publishTrajectoryLine(const robot_trajectory::RobotTrajectory &robot_trajectory, const moveit::core::LinkModel *ee_parent_link, const rviz_visual_tools::Colors &color = rviz_visual_tools::LIME_GREEN)

bool publishTrajectoryLine(const moveit_msgs::msg::RobotTrajectory &trajectory_msg, const moveit::core::JointModelGroup *arm_jmg, const rviz_visual_tools::Colors &color = rviz_visual_tools::LIME_GREEN)

Display a line of the end effector(s) path(s) from a robot trajectory path This version can visualize multiple end effectors.

Parameters:

trajectory_msg – - the robot plan
arm_jmg – - the set of joints to use, e.g. the MoveIt planning group, e.g. “left_arm”.
color – - display color of markers

Returns:

true on success

bool publishTrajectoryLine(const robot_trajectory::RobotTrajectoryPtr &robot_trajectory, const moveit::core::JointModelGroup *arm_jmg, const rviz_visual_tools::Colors &color = rviz_visual_tools::LIME_GREEN)

bool publishTrajectoryLine(const robot_trajectory::RobotTrajectory &robot_trajectory, const moveit::core::JointModelGroup *arm_jmg, const rviz_visual_tools::Colors &color = rviz_visual_tools::LIME_GREEN)

bool publishTrajectoryPoints(const std::vector<moveit::core::RobotStatePtr> &robot_state_trajectory, const moveit::core::LinkModel *ee_parent_link, const rviz_visual_tools::Colors &color = rviz_visual_tools::YELLOW)

Display trajectory as series of end effector position points.

Parameters:

trajectory – the actual plan
color – - display color of markers

Returns:

true on success

void enableRobotStateRootOffet(const Eigen::Isometry3d &offset): All published robot states will have their virtual joint moved by offset.

void disableRobotStateRootOffet(): Turn off the root offset feature.

bool publishRobotState(const trajectory_msgs::msg::JointTrajectoryPoint &trajectory_pt, const moveit::core::JointModelGroup *jmg, const rviz_visual_tools::Colors &color = rviz_visual_tools::DEFAULT)

Publish a MoveIt robot state to a topic that the Rviz “RobotState” display can show.

Parameters:

trajectory_pt – of joint positions
jmg – - the set of joints to use, e.g. the MoveIt planning group, e.g. “left_arm”
color – - how to highlight the robot (solid-ly) if desired, default keeps color as specified in URDF

Returns:

true on success

bool publishRobotState(const std::vector<double> &joint_positions, const moveit::core::JointModelGroup *jmg, const rviz_visual_tools::Colors &color = rviz_visual_tools::DEFAULT)

Publish a MoveIt robot state to a topic that the Rviz “RobotState” display can show.

Parameters:

joint_positions – - a vector of doubles corresponding 1-to-1 to the kinematic chain named in “jmg”
jmg – - the set of joints to use, e.g. the MoveIt planning group, e.g. “left_arm”
color – - how to highlight the robot (solid-ly) if desired, default keeps color as specified in URDF

Returns:

true on success

bool publishRobotState(const moveit::core::RobotState &robot_state, const rviz_visual_tools::Colors &color = rviz_visual_tools::DEFAULT, const std::vector<std::string> &highlight_links = {})

Publish a complete robot state to Rviz To use, add a RobotState marker to Rviz and subscribe to the DISPLAY_ROBOT_STATE_TOPIC, above.

Parameters:

robot_state – - joint values of robot
color – - how to highlight the robot (solid-ly) if desired, default keeps color as specified in URDF
highlight_links – - if the |color| is not |DEFAULT|, allows selective robot links to be highlighted. By default (empty) all links are highlighted.

bool publishRobotState(const moveit::core::RobotStatePtr &robot_state, const rviz_visual_tools::Colors &color = rviz_visual_tools::DEFAULT, const std::vector<std::string> &highlight_links = {})

void publishRobotState(const moveit_msgs::msg::DisplayRobotState &display_robot_state_msg)

bool hideRobot()

Hide robot in RobotState display in Rviz.

Returns:: true on success

void showJointLimits(const moveit::core::RobotStatePtr &robot_state)

Print to console the current robot state’s joint values within its limits visually.

Parameters:: robot_state – - the robot to show

planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitor()

Get the planning scene monitor that this class is using.

Returns:: a ptr to a planning scene

Public Static Functions

static bool applyVirtualJointTransform(moveit::core::RobotState &robot_state, const Eigen::Isometry3d &offset): Before publishing a robot state, optionally change its root transform.

Protected Attributes

planning_scene_monitor::PlanningSceneMonitorPtr psm_

bool manual_trigger_update_ = false

moveit::core::RobotModelConstPtr robot_model_

std::string robot_state_topic_

std::string planning_scene_topic_

rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>::SharedPtr pub_display_path_

rclcpp::Publisher<moveit_msgs::msg::DisplayRobotState>::SharedPtr pub_robot_state_

rclcpp::Node::SharedPtr node_

robot_model_loader::RobotModelLoaderPtr rm_loader_

std::map<const moveit::core::JointModelGroup*, visualization_msgs::msg::MarkerArray> ee_markers_map_

std::map<const moveit::core::JointModelGroup*, EigenSTL::vector_Isometry3d> ee_poses_map_

std::map<const moveit::core::JointModelGroup*, std::vector<double>> ee_joint_pos_map_

std::map<rviz_visual_tools::Colors, moveit_msgs::msg::DisplayRobotState> display_robot_msgs_

moveit::core::RobotStatePtr shared_robot_state_

moveit::core::RobotStatePtr hidden_robot_state_

moveit::core::RobotStatePtr root_robot_state_

bool robot_state_root_offset_enabled_ = false

Eigen::Isometry3d robot_state_root_offset_