moveit_visual_tools.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, University of Colorado, Boulder
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Univ of CO, Boulder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* \author Dave Coleman
36  * \desc Helper functions for displaying and debugging MoveIt! data in Rviz via published markers
37  * and MoveIt! collision objects. Very useful for debugging complex software
38  */
39 
40 #ifndef MOVEIT_VISUAL_TOOLS_MOVEIT_VISUAL_TOOLS_H
41 #define MOVEIT_VISUAL_TOOLS_MOVEIT_VISUAL_TOOLS_H
42 
43 // Rviz Visualization Tool
45 
46 // MoveIt
48 // #include <moveit/macros/deprecation.h>
50 
51 // MoveIt Messages
52 #include <moveit_msgs/Grasp.h>
53 #include <moveit_msgs/DisplayRobotState.h>
54 #include <moveit_msgs/WorkspaceParameters.h>
55 
56 // ROS Messages
57 #include <trajectory_msgs/JointTrajectory.h>
58 
59 // C++
60 #include <map>
61 #include <string>
62 #include <vector>
63 
64 namespace moveit_visual_tools
65 {
66 // Default constants
67 static const std::string ROBOT_DESCRIPTION = "robot_description"; // this is the default used in ROS
68 static const std::string DISPLAY_PLANNED_PATH_TOPIC =
69  "/move_group/display_planned_path"; // this is the default when adding the Rviz plugin
70 static const std::string DISPLAY_ROBOT_STATE_TOPIC =
71  "display_robot_state"; // this is the default when adding the Rviz plugin
72 static const std::string PLANNING_SCENE_TOPIC = "planning_scene"; // this is the default when adding the Rviz plugin
73 
75 {
76 public:
84  MoveItVisualTools(const std::string& base_frame, const std::string& marker_topic,
85  planning_scene_monitor::PlanningSceneMonitorPtr psm);
86 
93  MoveItVisualTools(const std::string& base_frame,
94  const std::string& marker_topic = rviz_visual_tools::RVIZ_MARKER_TOPIC,
95  robot_model::RobotModelConstPtr robot_model = robot_model::RobotModelConstPtr());
96 
101  void setRobotStateTopic(const std::string& robot_state_topic)
102  {
103  robot_state_topic_ = robot_state_topic;
104  }
105 
110  void setPlanningSceneTopic(const std::string& planning_scene_topic)
111  {
112  planning_scene_topic_ = planning_scene_topic;
113  }
114 
120 
127  bool processCollisionObjectMsg(const moveit_msgs::CollisionObject& msg,
129 
135  bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject& msg);
136 
143  bool moveCollisionObject(const Eigen::Affine3d& pose, const std::string& name,
144  const rviz_visual_tools::colors& color);
145  bool moveCollisionObject(const geometry_msgs::Pose& pose, const std::string& name,
146  const rviz_visual_tools::colors& color);
147 
153 
158  bool loadSharedRobotState();
159 
164  moveit::core::RobotStatePtr& getSharedRobotState();
165 
170  moveit::core::RobotStatePtr& getRootRobotState()
171  {
172  return root_robot_state_;
173  }
174 
179  moveit::core::RobotModelConstPtr getRobotModel();
180 
187  bool loadEEMarker(const robot_model::JointModelGroup* ee_jmg, const std::vector<double>& ee_joint_pos = {});
188 
192  void loadTrajectoryPub(const std::string& display_planned_path_topic = DISPLAY_PLANNED_PATH_TOPIC,
193  bool blocking = true);
194  void loadRobotStatePub(const std::string& robot_state_topic = "", bool blocking = true);
195 
200  void setPlanningSceneMonitor(planning_scene_monitor::PlanningSceneMonitorPtr psm)
201  {
202  psm_ = psm;
203  }
204 
209  void setManualSceneUpdating(bool enable_manual = true)
210  {
211  mannual_trigger_update_ = enable_manual;
212  }
213 
222  bool publishEEMarkers(const Eigen::Affine3d& pose, const robot_model::JointModelGroup* ee_jmg,
223  const std::vector<double>& ee_joint_pos,
225  const std::string& ns = "end_effector")
226  {
227  return publishEEMarkers(convertPose(pose), ee_jmg, ee_joint_pos, color, ns);
228  }
229  bool publishEEMarkers(const Eigen::Affine3d& pose, const robot_model::JointModelGroup* ee_jmg,
231  const std::string& ns = "end_effector")
232  {
233  return publishEEMarkers(convertPose(pose), ee_jmg, {}, color, ns);
234  }
235  bool publishEEMarkers(const geometry_msgs::Pose& pose, const robot_model::JointModelGroup* ee_jmg,
237  const std::string& ns = "end_effector")
238  {
239  return publishEEMarkers(pose, ee_jmg, {}, color, ns);
240  }
241  bool publishEEMarkers(const geometry_msgs::Pose& pose, const robot_model::JointModelGroup* ee_jmg,
242  const std::vector<double>& ee_joint_pos,
244  const std::string& ns = "end_effector");
245 
252  bool publishGrasps(const std::vector<moveit_msgs::Grasp>& possible_grasps, const robot_model::JointModelGroup* ee_jmg,
253  double animate_speed = 0.1);
254 
261  bool publishAnimatedGrasps(const std::vector<moveit_msgs::Grasp>& possible_grasps,
262  const robot_model::JointModelGroup* ee_jmg, double animate_speed = 0.01);
263 
271  bool publishAnimatedGrasp(const moveit_msgs::Grasp& grasp, const robot_model::JointModelGroup* ee_jmg,
272  double animate_speed);
273 
281  bool publishIKSolutions(const std::vector<trajectory_msgs::JointTrajectoryPoint>& ik_solutions,
282  const robot_model::JointModelGroup* arm_jmg, double display_time = 0.4);
283 
291 
297  bool cleanupCO(const std::string& name);
298 
304  bool cleanupACO(const std::string& name);
305 
312  bool attachCO(const std::string& name, const std::string& ee_parent_link);
313 
321  bool publishCollisionFloor(double z = 0.0, const std::string& plane_name = "Floor",
323 
332  bool publishCollisionBlock(const geometry_msgs::Pose& block_pose, const std::string& block_name = "block",
333  double block_size = 0.1,
335 
344  bool publishCollisionCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std::string& name,
346  bool publishCollisionCuboid(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2,
347  const std::string& name,
349 
360  bool publishCollisionCuboid(const Eigen::Affine3d& pose, double width, double depth, double height,
361  const std::string& name, const rviz_visual_tools::colors& color);
362 
363  bool publishCollisionCuboid(const geometry_msgs::Pose& pose, double width, double depth, double height,
364  const std::string& name, const rviz_visual_tools::colors& color);
365 
375  bool publishCollisionCylinder(const geometry_msgs::Point& a, const geometry_msgs::Point& b,
376  const std::string& object_name, double radius,
378  bool publishCollisionCylinder(const Eigen::Vector3d& a, const Eigen::Vector3d& b, const std::string& object_name,
379  double radius, const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
380 
390  bool publishCollisionCylinder(const Eigen::Affine3d& object_pose, const std::string& object_name, double radius,
391  double height, const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
392  bool publishCollisionCylinder(const geometry_msgs::Pose& object_pose, const std::string& object_name, double radius,
393  double height, const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
394 
403  bool publishCollisionMesh(const geometry_msgs::Pose& object_pose, const std::string& object_name,
404  const std::string& mesh_path,
406  bool publishCollisionMesh(const Eigen::Affine3d& object_pose, const std::string& object_name,
407  const std::string& mesh_path,
409  bool publishCollisionMesh(const Eigen::Affine3d& object_pose, const std::string& object_name,
410  const shape_msgs::Mesh& mesh_msg,
412  bool publishCollisionMesh(const geometry_msgs::Pose& object_pose, const std::string& object_name,
413  const shape_msgs::Mesh& mesh_msg,
415 
423  bool publishCollisionGraph(const graph_msgs::GeometryGraph& graph, const std::string& object_name, double radius,
425 
429  void getCollisionWallMsg(double x, double y, double z, double angle, double width, double height,
430  const std::string name, moveit_msgs::CollisionObject& collision_obj);
431 
443  bool publishCollisionWall(double x, double y, double angle = 0.0, double width = 2.0, double height = 1.5,
444  const std::string name = "wall",
446  bool publishCollisionWall(double x, double y, double z, double angle = 0.0, double width = 2.0, double height = 1.5,
447  const std::string name = "wall",
449 
463  bool publishCollisionTable(double x, double y, double angle, double width, double height, double depth,
464  const std::string name, const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN)
465  {
466  return publishCollisionTable(x, y, 0, angle, width, height, depth, name, color);
467  }
468 
469  bool publishCollisionTable(double x, double y, double z, double angle, double width, double height, double depth,
470  const std::string name, const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
471 
478  bool loadCollisionSceneFromFile(const std::string& path);
479  bool loadCollisionSceneFromFile(const std::string& path, const Eigen::Affine3d& offset);
480 
486  bool publishWorkspaceParameters(const moveit_msgs::WorkspaceParameters& params);
487 
495  bool publishContactPoints(const moveit::core::RobotState& robot_state,
498 
510  bool publishTrajectoryPoint(const trajectory_msgs::JointTrajectoryPoint& trajectory_pt,
511  const std::string& planning_group, double display_time = 0.1);
512 
521  bool publishTrajectoryPath(const std::vector<moveit::core::RobotStatePtr>& trajectory,
522  const moveit::core::JointModelGroup* jmg, double speed = 0.01, bool blocking = false);
523  bool publishTrajectoryPath(const robot_trajectory::RobotTrajectoryPtr& trajectory, bool blocking = false);
524  bool publishTrajectoryPath(const robot_trajectory::RobotTrajectory& trajectory, bool blocking = false);
525  bool publishTrajectoryPath(const moveit_msgs::RobotTrajectory& trajectory_msg,
526  const moveit::core::RobotStateConstPtr robot_state, bool blocking = false);
527  bool publishTrajectoryPath(const moveit_msgs::RobotTrajectory& trajectory_msg,
528  const moveit::core::RobotState& robot_state, bool blocking = false);
529  bool publishTrajectoryPath(const moveit_msgs::RobotTrajectory& trajectory_msg,
530  const moveit_msgs::RobotState& robot_state, bool blocking = false);
531 
540  bool publishTrajectoryLine(const moveit_msgs::RobotTrajectory& trajectory_msg,
541  const moveit::core::LinkModel* ee_parent_link, const robot_model::JointModelGroup* arm_jmg,
543  bool publishTrajectoryLine(const robot_trajectory::RobotTrajectoryPtr robot_trajectory,
544  const moveit::core::LinkModel* ee_parent_link,
546  bool publishTrajectoryLine(const robot_trajectory::RobotTrajectory& robot_trajectory,
547  const moveit::core::LinkModel* ee_parent_link,
549 
558  bool publishTrajectoryLine(const moveit_msgs::RobotTrajectory& trajectory_msg,
559  const robot_model::JointModelGroup* arm_jmg,
561  bool publishTrajectoryLine(const robot_trajectory::RobotTrajectoryPtr robot_trajectory,
562  const robot_model::JointModelGroup* arm_jmg,
564  bool publishTrajectoryLine(const robot_trajectory::RobotTrajectory& robot_trajectory,
565  const robot_model::JointModelGroup* arm_jmg,
567 
574  bool publishTrajectoryPoints(const std::vector<moveit::core::RobotStatePtr>& robot_state_trajectory,
575  const moveit::core::LinkModel* ee_parent_link,
577 
579  void enableRobotStateRootOffet(const Eigen::Affine3d& offset);
580 
583 
591  bool publishRobotState(const trajectory_msgs::JointTrajectoryPoint& trajectory_pt,
592  const robot_model::JointModelGroup* jmg,
594 
602  bool publishRobotState(const std::vector<double> joint_positions, const robot_model::JointModelGroup* jmg,
604 
611  bool publishRobotState(const moveit::core::RobotState& robot_state,
613  bool publishRobotState(const moveit::core::RobotStatePtr& robot_state,
615 
620  bool hideRobot();
621 
623  static bool applyVirtualJointTransform(moveit::core::RobotState& robot_state, const Eigen::Affine3d& offset);
624 
629  void showJointLimits(moveit::core::RobotStatePtr robot_state);
630 
635  planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitor();
636 
637 private:
642  static bool checkForVirtualJoint(const moveit::core::RobotState& robot_state);
643 
644 protected:
645  // Pointer to a Planning Scene Monitor
646  planning_scene_monitor::PlanningSceneMonitorPtr psm_;
647 
648  // Prevent the planning scene from always auto-pushing, but rather do it manually
650 
651  // ROS topic names to use when starting publishers
652  std::string robot_state_topic_;
654 
655  // ROS publishers
656  ros::Publisher pub_display_path_; // for MoveIt trajectories
657  ros::Publisher pub_robot_state_; // publish a RobotState message
658 
659  robot_model_loader::RobotModelLoaderPtr rm_loader_; // so that we can specify our own options
660 
661  // End Effector Markers
662  std::map<const robot_model::JointModelGroup*, visualization_msgs::MarkerArray> ee_markers_map_;
663  std::map<const robot_model::JointModelGroup*, EigenSTL::vector_Affine3d> ee_poses_map_;
664  std::map<const robot_model::JointModelGroup*, std::vector<double> > ee_joint_pos_map_;
665 
666  // Cached robot state marker - cache the colored links.
667  // Note: Only allows colors provided in rviz_visual_tools to prevent too many robot state messages from being loaded
668  // and ensuring efficiency
669  std::map<rviz_visual_tools::colors, moveit_msgs::DisplayRobotState> display_robot_msgs_;
670 
671  // Pointer to the robot model
672  moveit::core::RobotModelConstPtr robot_model_;
673 
674  // Note: call loadSharedRobotState() before using this
675  moveit::core::RobotStatePtr shared_robot_state_;
676 
677  // Note: call loadSharedRobotState() before using this. Use only for hiding the robot
678  moveit::core::RobotStatePtr hidden_robot_state_;
679 
680  // A robot state whose virtual_joint remains at identity so that getGlobalLinkTransform() isn't tainted
681  // Note: call loadSharedRobotState() before using this
682  moveit::core::RobotStatePtr root_robot_state_;
683 
684  // Optional offset that can be applied to all outgoing/published robot states
686  Eigen::Affine3d robot_state_root_offset_;
687 
688 public:
689  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
690 }; // class
691 
692 typedef std::shared_ptr<MoveItVisualTools> MoveItVisualToolsPtr;
693 typedef std::shared_ptr<const MoveItVisualTools> MoveItVisualToolsConstPtr;
694 
695 } // namespace moveit_visual_tools
696 
697 #endif // MOVEIT_VISUAL_TOOLS_MOVEIT_VISUAL_TOOLS_H
moveit::core::RobotStatePtr & getRootRobotState()
Allow robot state to be altered.
static const std::string DISPLAY_ROBOT_STATE_TOPIC
bool hideRobot()
Fake removing a Robot State display in Rviz by simply moving it very far away.
#define RVIZ_VISUAL_TOOLS_DEPRECATED
std::shared_ptr< MoveItVisualTools > MoveItVisualToolsPtr
bool publishCollisionCylinder(const geometry_msgs::Point &a, const geometry_msgs::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.
planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitor()
Get the planning scene monitor that this class is using.
void setPlanningSceneTopic(const std::string &planning_scene_topic)
Set the planning scene topic.
bool publishTrajectoryLine(const moveit_msgs::RobotTrajectory &trajectory_msg, const moveit::core::LinkModel *ee_parent_link, const robot_model::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.
MoveItVisualTools(const std::string &base_frame, const std::string &marker_topic, planning_scene_monitor::PlanningSceneMonitorPtr psm)
Constructor.
moveit::core::RobotModelConstPtr robot_model_
robot_model_loader::RobotModelLoaderPtr rm_loader_
moveit::core::RobotStatePtr root_robot_state_
bool attachCO(const std::string &name, const std::string &ee_parent_link)
Attach a collision object from the planning scene.
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 publ...
bool publishCollisionMesh(const geometry_msgs::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.
bool loadCollisionSceneFromFile(const std::string &path)
Load a planning scene to a planning_scene_monitor from file.
bool processCollisionObjectMsg(const moveit_msgs::CollisionObject &msg, const rviz_visual_tools::colors &color=rviz_visual_tools::GREEN)
Skip a ROS message call by sending directly to planning scene monitor.
bool cleanupACO(const std::string &name)
Remove an active collision object from the planning scene.
std::map< rviz_visual_tools::colors, moveit_msgs::DisplayRobotState > display_robot_msgs_
std::map< const robot_model::JointModelGroup *, std::vector< double > > ee_joint_pos_map_
void showJointLimits(moveit::core::RobotStatePtr robot_state)
Print to console the current robot state&#39;s joint values within its limits visually.
bool publishTrajectoryPoint(const trajectory_msgs::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...
std::map< const robot_model::JointModelGroup *, visualization_msgs::MarkerArray > ee_markers_map_
bool loadEEMarker(const robot_model::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...
double y
planning_scene_monitor::PlanningSceneMonitorPtr psm_
moveit::core::RobotStatePtr hidden_robot_state_
void setRobotStateTopic(const std::string &robot_state_topic)
Set the ROS topic for publishing a robot state.
void setManualSceneUpdating(bool enable_manual=true)
Prevent the planning scene from always auto-pushing, but rather do it manually.
bool publishEEMarkers(const Eigen::Affine3d &pose, const robot_model::JointModelGroup *ee_jmg, const std::vector< double > &ee_joint_pos, const rviz_visual_tools::colors &color=rviz_visual_tools::CLEAR, const std::string &ns="end_effector")
Publish an end effector to rviz.
bool publishWorkspaceParameters(const moveit_msgs::WorkspaceParameters &params)
Display size of workspace used for planning with OMPL, etc. Important for virtual joints...
void enableRobotStateRootOffet(const Eigen::Affine3d &offset)
All published robot states will have their virtual joint moved by offset.
bool publishCollisionBlock(const geometry_msgs::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.
void setPlanningSceneMonitor(planning_scene_monitor::PlanningSceneMonitorPtr psm)
Allow a pre-configured planning scene monitor to be set for publishing collision objects, etc.
bool loadPlanningSceneMonitor()
Load a planning scene monitor if one was not passed into the constructor.
static bool applyVirtualJointTransform(moveit::core::RobotState &robot_state, const Eigen::Affine3d &offset)
Before publishing a robot state, optionally change its root transform.
bool publishAnimatedGrasp(const moveit_msgs::Grasp &grasp, const robot_model::JointModelGroup *ee_jmg, double animate_speed)
Animate a single grasp in its movement direction.
bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject &msg)
Skip a ROS message call by sending directly to planning scene monitor.
static bool checkForVirtualJoint(const moveit::core::RobotState &robot_state)
Error check that the robot&#39;s SRDF was properly setup with a virtual joint that was named a certain wa...
robot_trajectory::RobotTrajectory trajectory(rmodel,"right_arm")
bool publishAnimatedGrasps(const std::vector< moveit_msgs::Grasp > &possible_grasps, const robot_model::JointModelGroup *ee_jmg, double animate_speed=0.01)
Display an animated vector of grasps including its approach movement in Rviz.
double z
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.
bool removeAllCollisionObjects()
Remove all collision objects that this class has added to the MoveIt! planning scene Communicates dir...
moveit::core::RobotModelConstPtr getRobotModel()
Get a pointer to the robot model.
moveit::core::RobotStatePtr & getSharedRobotState()
Allow robot state to be altered.
void getCollisionWallMsg(double x, double y, double z, double angle, double width, double height, const std::string name, moveit_msgs::CollisionObject &collision_obj)
Helper for publishCollisionWall.
bool publishEEMarkers(const Eigen::Affine3d &pose, const robot_model::JointModelGroup *ee_jmg, const rviz_visual_tools::colors &color=rviz_visual_tools::CLEAR, const std::string &ns="end_effector")
std::map< const robot_model::JointModelGroup *, EigenSTL::vector_Affine3d > ee_poses_map_
bool publishIKSolutions(const std::vector< trajectory_msgs::JointTrajectoryPoint > &ik_solutions, const robot_model::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 t...
static geometry_msgs::Pose convertPose(const Eigen::Affine3d &pose)
bool cleanupCO(const std::string &name)
Remove a collision object from the planning scene.
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.
void loadRobotStatePub(const std::string &robot_state_topic="", bool blocking=true)
moveit::core::RobotStatePtr shared_robot_state_
bool publishCollisionGraph(const graph_msgs::GeometryGraph &graph, const std::string &object_name, double radius, const rviz_visual_tools::colors &color=rviz_visual_tools::GREEN)
Publish a connected birectional graph.
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.
static const std::string RVIZ_MARKER_TOPIC
bool moveCollisionObject(const Eigen::Affine3d &pose, const std::string &name, const rviz_visual_tools::colors &color)
Move an already published collision object to a new locaiton in space.
RVIZ_VISUAL_TOOLS_DEPRECATED bool publishCollisionTable(double x, double y, 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.
static const std::string ROBOT_DESCRIPTION
static const std::string PLANNING_SCENE_TOPIC
bool publishEEMarkers(const geometry_msgs::Pose &pose, const robot_model::JointModelGroup *ee_jmg, const rviz_visual_tools::colors &color=rviz_visual_tools::CLEAR, const std::string &ns="end_effector")
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.
void loadTrajectoryPub(const std::string &display_planned_path_topic=DISPLAY_PLANNED_PATH_TOPIC, bool blocking=true)
Load publishers as needed.
bool publishRobotState(const trajectory_msgs::JointTrajectoryPoint &trajectory_pt, const robot_model::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.
double x
void disableRobotStateRootOffet()
Turn off the root offset feature.
bool loadSharedRobotState()
Load robot state only as needed.
bool triggerPlanningSceneUpdate()
When mannual_trigger_update_ is true, use this to tell the planning scene to send an update out...
static const std::string DISPLAY_PLANNED_PATH_TOPIC
std::shared_ptr< const MoveItVisualTools > MoveItVisualToolsConstPtr
bool publishGrasps(const std::vector< moveit_msgs::Grasp > &possible_grasps, const robot_model::JointModelGroup *ee_jmg, double animate_speed=0.1)
Show grasps generated from moveit_simple_grasps or other MoveIt Grasp message sources.
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.


moveit_visual_tools
Author(s): Dave Coleman
autogenerated on Thu Jun 6 2019 19:40:28