moveit_visual_tools.cpp
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: Simple tools for showing parts of a robot in Rviz, such as the gripper or arm
37 
39 
40 // MoveIt Messages
41 #include <moveit_msgs/DisplayTrajectory.h>
42 #include <moveit_msgs/CollisionObject.h>
43 
44 // MoveIt
48 
49 // Conversions
52 
53 // Shape tools
56 
57 // C++
58 #include <string>
59 #include <algorithm>
60 #include <utility>
61 #include <vector>
62 #include <set>
63 #include <limits>
64 
65 namespace moveit_visual_tools
66 {
67 MoveItVisualTools::MoveItVisualTools(const std::string& base_frame, const std::string& marker_topic,
68  planning_scene_monitor::PlanningSceneMonitorPtr psm)
69  : RvizVisualTools::RvizVisualTools(base_frame, marker_topic)
70  , psm_(psm)
71  , robot_state_topic_(DISPLAY_ROBOT_STATE_TOPIC)
72  , planning_scene_topic_(PLANNING_SCENE_TOPIC)
73 {
74 }
75 
76 MoveItVisualTools::MoveItVisualTools(const std::string& base_frame, const std::string& marker_topic,
77  robot_model::RobotModelConstPtr robot_model)
78  : RvizVisualTools::RvizVisualTools(base_frame, marker_topic), robot_model_(robot_model)
79 {
80 }
81 
83 {
84  // Check if we already have one
85  if (psm_)
86  {
87  ROS_WARN_STREAM_NAMED(name_, "Will not load a new planning scene monitor when one has "
88  "already been set for Visual Tools");
89  return false;
90  }
91  ROS_DEBUG_STREAM_NAMED(name_, "Loading planning scene monitor");
92 
93  // Create tf transformer
95 
96  // Regular version b/c the other one causes problems with recognizing end effectors
97  psm_.reset(new planning_scene_monitor::PlanningSceneMonitor(ROBOT_DESCRIPTION, tf, "visual_tools_scene"));
98 
99  ros::spinOnce();
100  ros::Duration(0.1).sleep();
101  ros::spinOnce();
102 
103  if (psm_->getPlanningScene())
104  {
105  // Optional monitors to start:
106  // psm_->startWorldGeometryMonitor();
107  // psm_->startSceneMonitor("/move_group/monitored_planning_scene");
108  // psm_->startStateMonitor("/joint_states", "/attached_collision_object");
109 
112  ROS_DEBUG_STREAM_NAMED(name_, "Publishing planning scene on " << planning_scene_topic_);
113 
115  planning_scene->setName("visual_tools_scene");
116  }
117  else
118  {
119  ROS_ERROR_STREAM_NAMED(name_, "Planning scene not configured");
120  return false;
121  }
122 
123  return true;
124 }
125 
126 bool MoveItVisualTools::processCollisionObjectMsg(const moveit_msgs::CollisionObject& msg,
127  const rviz_visual_tools::colors& color)
128 {
129  // Apply command directly to planning scene to avoid a ROS msg call
130  {
132  scene->getCurrentStateNonConst().update(); // TODO: remove hack to prevent bad transforms
133  scene->processCollisionObjectMsg(msg);
134  scene->setObjectColor(msg.id, getColor(color));
135  }
136  // Trigger an update
138  {
140  }
141 
142  return true;
143 }
144 
145 bool MoveItVisualTools::processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject& msg)
146 {
147  // Apply command directly to planning scene to avoid a ROS msg call
148  {
150  // scene->getCurrentStateNonConst().update(); // hack to prevent bad transforms
151  scene->processAttachedCollisionObjectMsg(msg);
152  }
153 
154  // Trigger an update
156  {
158  }
159 
160  return true;
161 }
162 
163 bool MoveItVisualTools::moveCollisionObject(const Eigen::Affine3d& pose, const std::string& name,
164  const rviz_visual_tools::colors& color)
165 {
166  return moveCollisionObject(convertPose(pose), name, color);
167 }
168 
169 bool MoveItVisualTools::moveCollisionObject(const geometry_msgs::Pose& pose, const std::string& name,
170  const rviz_visual_tools::colors& color)
171 {
172  moveit_msgs::CollisionObject collision_obj;
173  collision_obj.header.stamp = ros::Time::now();
174  collision_obj.header.frame_id = base_frame_;
175  collision_obj.id = name;
176  collision_obj.operation = moveit_msgs::CollisionObject::MOVE;
177 
178  collision_obj.primitive_poses.resize(1);
179  collision_obj.primitive_poses[0] = pose;
180 
181  // ROS_INFO_STREAM_NAMED(name_,"CollisionObject: \n " << collision_obj);
182  // ROS_DEBUG_STREAM_NAMED(name_,"Published collision object " << name);
183  return processCollisionObjectMsg(collision_obj, color);
184 }
185 
187 {
188  // Note in ROS Melodic we've switched to only UPDATE_GEOMETRY
189  // see https://github.com/ros-planning/moveit_visual_tools/pull/29
191  // getPlanningSceneMonitor()->triggerSceneUpdateEvent(planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY);
192 
193  return true;
194 }
195 
197 {
198  // Get robot state
199  if (!shared_robot_state_)
200  {
201  // Check if a robot model was passed in
202  if (!robot_model_)
203  {
204  // Fall back on using planning scene monitor.
205  robot_model_ = getPlanningSceneMonitor()->getRobotModel();
206  }
207  shared_robot_state_.reset(new robot_state::RobotState(robot_model_));
208 
209  // TODO(davetcoleman): this seems to be a work around for a weird NaN bug
210  shared_robot_state_->setToDefaultValues();
211  shared_robot_state_->update(true);
212 
213  // hidden_robot_state_.reset(new robot_state::RobotState(robot_model_));
214  // hidden_robot_state_->setToDefaultValues();
215  // hidden_robot_state_->update(true);
216 
217  hidden_robot_state_.reset(new robot_state::RobotState(*shared_robot_state_));
218  root_robot_state_.reset(new robot_state::RobotState(*shared_robot_state_));
219  }
220 
221  return !(shared_robot_state_ == nullptr);
222 }
223 
224 moveit::core::RobotStatePtr& MoveItVisualTools::getSharedRobotState()
225 {
226  // Always load the robot state before using
228  return shared_robot_state_;
229 }
230 
231 moveit::core::RobotModelConstPtr MoveItVisualTools::getRobotModel()
232 {
233  // Always load the robot state before using
235  return shared_robot_state_->getRobotModel();
236 }
237 
238 bool MoveItVisualTools::loadEEMarker(const robot_model::JointModelGroup* ee_jmg,
239  const std::vector<double>& ee_joint_pos)
240 {
241  // Get joint state group
242  if (ee_jmg == NULL) // make sure EE_GROUP exists
243  {
244  ROS_ERROR_STREAM_NAMED(name_, "Unable to find joint model group with address" << ee_jmg);
245  return false;
246  }
247 
248  // Always load the robot state before using
250  shared_robot_state_->setToDefaultValues();
251  shared_robot_state_->update();
252 
253  if (ee_joint_pos.size() > 0)
254  {
255  if (ee_joint_pos.size() != ee_jmg->getActiveJointModels().size())
256  {
257  ROS_ERROR_STREAM_NAMED(name_, "The number of joint positions given ("
258  << ee_joint_pos.size() << ") does not match the number of active joints in "
259  << ee_jmg->getName() << "(" << ee_jmg->getActiveJointModels().size() << ")");
260  return false;
261  }
262  shared_robot_state_->setJointGroupPositions(ee_jmg, ee_joint_pos);
263  shared_robot_state_->update(true);
264  }
265 
266  // Clear old EE markers and EE poses
267  ee_markers_map_[ee_jmg].markers.clear();
268  ee_poses_map_[ee_jmg].clear();
269 
270  // Remember joint state
271  ee_joint_pos_map_[ee_jmg] = ee_joint_pos;
272 
273  // Keep track of how many unique markers we have between different EEs
274  static std::size_t marker_id_offset = 0;
275 
276  // -----------------------------------------------------------------------------------------------
277  // Get end effector group
278 
279  // Create color to use for EE markers
280  std_msgs::ColorRGBA marker_color = getColor(rviz_visual_tools::GREY);
281 
282  // Get link names that are in end effector
283  const std::vector<std::string>& ee_link_names = ee_jmg->getLinkModelNames();
284 
285  // -----------------------------------------------------------------------------------------------
286  // Get EE link markers for Rviz
287 
288  shared_robot_state_->getRobotMarkers(ee_markers_map_[ee_jmg], ee_link_names, marker_color, ee_jmg->getName(),
289  ros::Duration());
290  ROS_DEBUG_STREAM_NAMED(name_, "Number of rviz markers in end effector: " << ee_markers_map_[ee_jmg].markers.size());
291 
292  const std::string& ee_parent_link_name = ee_jmg->getEndEffectorParentGroup().second;
293  // ROS_DEBUG_STREAM_NAMED(name_,"EE Parent link: " << ee_parent_link_name);
294  const moveit::core::LinkModel* ee_parent_link = robot_model_->getLinkModel(ee_parent_link_name);
295 
296  Eigen::Affine3d ee_marker_global_transform = shared_robot_state_->getGlobalLinkTransform(ee_parent_link);
297  Eigen::Affine3d ee_marker_pose;
298 
299  // Process each link of the end effector
300  for (std::size_t i = 0; i < ee_markers_map_[ee_jmg].markers.size(); ++i)
301  {
302  // Header
303  ee_markers_map_[ee_jmg].markers[i].header.frame_id = base_frame_;
304 
305  // Options for meshes
306  if (ee_markers_map_[ee_jmg].markers[i].type == visualization_msgs::Marker::MESH_RESOURCE)
307  {
308  ee_markers_map_[ee_jmg].markers[i].mesh_use_embedded_materials = true;
309  }
310 
311  // Unique id
312  ee_markers_map_[ee_jmg].markers[i].id += marker_id_offset;
313 
314  // Copy original marker poses to a vector
315  ee_marker_pose = ee_marker_global_transform.inverse() * convertPose(ee_markers_map_[ee_jmg].markers[i].pose);
316  ee_poses_map_[ee_jmg].push_back(ee_marker_pose);
317  }
318 
319  marker_id_offset += ee_markers_map_[ee_jmg].markers.size();
320 
321  return true;
322 }
323 
324 void MoveItVisualTools::loadTrajectoryPub(const std::string& display_planned_path_topic, bool blocking)
325 {
326  if (pub_display_path_)
327  return;
328 
329  // Trajectory paths
330  pub_display_path_ = nh_.advertise<moveit_msgs::DisplayTrajectory>(display_planned_path_topic, 10, false);
331  ROS_DEBUG_STREAM_NAMED(name_, "Publishing MoveIt! trajectory on topic " << pub_display_path_.getTopic());
332 
333  // Wait for topic to be ready
334  if (blocking)
336 }
337 
338 void MoveItVisualTools::loadRobotStatePub(const std::string& robot_state_topic, bool blocking)
339 {
340  if (pub_robot_state_)
341  return;
342 
343  // Update global var if new topic was passed in
344  if (!robot_state_topic.empty())
345  robot_state_topic_ = robot_state_topic;
346 
347  // RobotState Message
348  pub_robot_state_ = nh_.advertise<moveit_msgs::DisplayRobotState>(robot_state_topic_, 1);
349  ROS_DEBUG_STREAM_NAMED(name_, "Publishing MoveIt! robot state on topic " << pub_robot_state_.getTopic());
350 
351  // Wait for topic to be ready
352  if (blocking)
354 }
355 
356 bool MoveItVisualTools::publishEEMarkers(const geometry_msgs::Pose& pose, const robot_model::JointModelGroup* ee_jmg,
357  const std::vector<double>& ee_joint_pos,
358  const rviz_visual_tools::colors& color, const std::string& ns)
359 {
360  // Check if we have not loaded the EE markers
361  if (ee_markers_map_[ee_jmg].markers.empty() || ee_poses_map_[ee_jmg].empty() ||
362  ee_joint_pos_map_[ee_jmg] != ee_joint_pos)
363  {
364  if (!loadEEMarker(ee_jmg, ee_joint_pos))
365  {
366  ROS_ERROR_STREAM_NAMED(name_, "Unable to publish EE marker, unable to load EE markers");
367  return false;
368  }
369  }
370 
371  Eigen::Affine3d eigen_goal_ee_pose = convertPose(pose);
372  Eigen::Affine3d eigen_this_marker;
373 
374  // publishArrow( pose, rviz_visual_tools::RED, rviz_visual_tools::LARGE );
375 
376  // -----------------------------------------------------------------------------------------------
377  // Process each link of the end effector
378  for (std::size_t i = 0; i < ee_markers_map_[ee_jmg].markers.size(); ++i)
379  {
380  // Make sure ROS is still spinning
381  if (!ros::ok())
382  break;
383 
384  // Header
385  ee_markers_map_[ee_jmg].markers[i].header.stamp = ros::Time::now();
386 
387  // Namespace
388  ee_markers_map_[ee_jmg].markers[i].ns = ns;
389 
390  // Lifetime
391  ee_markers_map_[ee_jmg].markers[i].lifetime = marker_lifetime_;
392 
393  // Color
394  ee_markers_map_[ee_jmg].markers[i].color = getColor(color);
395 
396  // Convert pose
397  eigen_this_marker = eigen_goal_ee_pose * ee_poses_map_[ee_jmg][i];
398  ee_markers_map_[ee_jmg].markers[i].pose = convertPose(eigen_this_marker);
399  }
400 
401  // Helper for publishing rviz markers
402  if (!publishMarkers(ee_markers_map_[ee_jmg]))
403  return false;
404 
405  return true;
406 }
407 
408 bool MoveItVisualTools::publishGrasps(const std::vector<moveit_msgs::Grasp>& possible_grasps,
409  const robot_model::JointModelGroup* ee_jmg, double animate_speed)
410 {
411  ROS_DEBUG_STREAM_NAMED(name_, "Visualizing " << possible_grasps.size() << " grasps with EE joint model group "
412  << ee_jmg->getName());
413 
414  // Loop through all grasps
415  for (std::size_t i = 0; i < possible_grasps.size(); ++i)
416  {
417  if (!ros::ok()) // Check that ROS is still ok and that user isn't trying to quit
418  break;
419 
420  publishEEMarkers(possible_grasps[i].grasp_pose.pose, ee_jmg);
421 
422  ros::Duration(animate_speed).sleep();
423  }
424 
425  return true;
426 }
427 
428 bool MoveItVisualTools::publishAnimatedGrasps(const std::vector<moveit_msgs::Grasp>& possible_grasps,
429  const robot_model::JointModelGroup* ee_jmg, double animate_speed)
430 {
431  ROS_DEBUG_STREAM_NAMED(name_, "Visualizing " << possible_grasps.size() << " grasps with joint model group "
432  << ee_jmg->getName() << " at speed " << animate_speed);
433 
434  // Loop through all grasps
435  for (std::size_t i = 0; i < possible_grasps.size(); ++i)
436  {
437  if (!ros::ok()) // Check that ROS is still ok and that user isn't trying to quit
438  break;
439 
440  publishAnimatedGrasp(possible_grasps[i], ee_jmg, animate_speed);
441  ros::Duration(animate_speed).sleep();
442  }
443 
444  return true;
445 }
446 
447 bool MoveItVisualTools::publishAnimatedGrasp(const moveit_msgs::Grasp& grasp,
448  const robot_model::JointModelGroup* ee_jmg, double animate_speed)
449 {
450  // Grasp Pose Variables
451  geometry_msgs::Pose grasp_pose = grasp.grasp_pose.pose;
452 
453  // Debug
454  if (false)
455  {
457  publishEEMarkers(grasp_pose, ee_jmg);
458  ros::Duration(0.5).sleep();
459  }
460 
461  Eigen::Affine3d grasp_pose_eigen;
462  tf::poseMsgToEigen(grasp_pose, grasp_pose_eigen);
463 
464  // Pre-grasp pose variables
465  geometry_msgs::Pose pre_grasp_pose;
466  Eigen::Affine3d pre_grasp_pose_eigen;
467 
468  // Approach direction variables
469  Eigen::Vector3d pre_grasp_approach_direction_local;
470 
471  // Display Grasp Score
472  // std::string text = "Grasp Quality: " +
473  // boost::lexical_cast<std::string>(int(grasp.grasp_quality*100)) + "%";
474  // publishText(grasp_pose, text);
475 
476  // Convert the grasp pose into the frame of reference of the approach/retreat frame_id
477 
478  // Animate the movement - for ee approach direction
479  double animation_resulution = 0.1; // the lower the better the resolution
480  for (double percent = 0; percent < 1; percent += animation_resulution)
481  {
482  if (!ros::ok()) // Check that ROS is still ok and that user isn't trying to quit
483  break;
484 
485  // Copy original grasp pose to pre-grasp pose
486  pre_grasp_pose_eigen = grasp_pose_eigen;
487 
488  // The direction of the pre-grasp
489  // Calculate the current animation position based on the percent
490  Eigen::Vector3d pre_grasp_approach_direction = Eigen::Vector3d(
491  -1 * grasp.pre_grasp_approach.direction.vector.x * grasp.pre_grasp_approach.min_distance * (1 - percent),
492  -1 * grasp.pre_grasp_approach.direction.vector.y * grasp.pre_grasp_approach.min_distance * (1 - percent),
493  -1 * grasp.pre_grasp_approach.direction.vector.z * grasp.pre_grasp_approach.min_distance * (1 - percent));
494 
495  // Decide if we need to change the approach_direction to the local frame of the end effector
496  // orientation
497  const std::string& ee_parent_link_name = ee_jmg->getEndEffectorParentGroup().second;
498 
499  if (grasp.pre_grasp_approach.direction.header.frame_id == ee_parent_link_name)
500  {
501  // Apply/compute the approach_direction vector in the local frame of the grasp_pose
502  // orientation
503  pre_grasp_approach_direction_local = grasp_pose_eigen.rotation() * pre_grasp_approach_direction;
504  }
505  else
506  {
507  pre_grasp_approach_direction_local = pre_grasp_approach_direction; // grasp_pose_eigen.rotation() *
508  // pre_grasp_approach_direction;
509  }
510 
511  // Update the grasp pose usign the new locally-framed approach_direction
512  pre_grasp_pose_eigen.translation() += pre_grasp_approach_direction_local;
513 
514  // Convert eigen pre-grasp position back to regular message
515  tf::poseEigenToMsg(pre_grasp_pose_eigen, pre_grasp_pose);
516 
517  // publishArrow(pre_grasp_pose, moveit_visual_tools::BLUE);
518  publishEEMarkers(pre_grasp_pose, ee_jmg);
520  trigger();
521  ros::Duration(animate_speed).sleep();
522 
523  // Pause more at initial pose for debugging purposes
524  if (percent == 0)
525  ros::Duration(animate_speed * 2).sleep();
526  }
527  return true;
528 }
529 
530 bool MoveItVisualTools::publishIKSolutions(const std::vector<trajectory_msgs::JointTrajectoryPoint>& ik_solutions,
531  const robot_model::JointModelGroup* arm_jmg, double display_time)
532 {
533  if (ik_solutions.empty())
534  {
535  ROS_WARN_STREAM_NAMED(name_, "Empty ik_solutions vector passed into publishIKSolutions()");
536  return false;
537  }
538 
540 
541  ROS_DEBUG_STREAM_NAMED(name_, "Visualizing " << ik_solutions.size() << " inverse kinematic solutions");
542 
543  // Apply the time to the trajectory
544  trajectory_msgs::JointTrajectoryPoint trajectory_pt_timed;
545 
546  // Create a trajectory with one point
547  moveit_msgs::RobotTrajectory trajectory_msg;
548  trajectory_msg.joint_trajectory.header.frame_id = base_frame_;
549  trajectory_msg.joint_trajectory.joint_names = arm_jmg->getActiveJointModelNames();
550 
551  // Overall length of trajectory
552  double running_time = 0;
553 
554  // Loop through all inverse kinematic solutions
555  for (std::size_t i = 0; i < ik_solutions.size(); ++i)
556  {
557  trajectory_pt_timed = ik_solutions[i];
558  trajectory_pt_timed.time_from_start = ros::Duration(running_time);
559  trajectory_msg.joint_trajectory.points.push_back(trajectory_pt_timed);
560 
561  running_time += display_time;
562  }
563 
564  // Re-add final position so the last point is displayed fully
565  trajectory_pt_timed = trajectory_msg.joint_trajectory.points.back();
566  trajectory_pt_timed.time_from_start = ros::Duration(running_time);
567  trajectory_msg.joint_trajectory.points.push_back(trajectory_pt_timed);
568 
569  return publishTrajectoryPath(trajectory_msg, shared_robot_state_, true);
570 }
571 
573 {
574  // Apply command directly to planning scene to avoid a ROS msg call
575  {
577  scene->removeAllCollisionObjects();
578  }
579 
580  return true;
581 }
582 
583 bool MoveItVisualTools::cleanupCO(const std::string& name)
584 {
585  // Clean up old collision objects
586  moveit_msgs::CollisionObject co;
587  co.header.stamp = ros::Time::now();
588  co.header.frame_id = base_frame_;
589  co.id = name;
590  co.operation = moveit_msgs::CollisionObject::REMOVE;
591 
592  return processCollisionObjectMsg(co);
593 }
594 
595 bool MoveItVisualTools::cleanupACO(const std::string& name)
596 {
597  // Clean up old attached collision object
598  moveit_msgs::AttachedCollisionObject aco;
599  aco.object.header.stamp = ros::Time::now();
600  aco.object.header.frame_id = base_frame_;
601 
602  // aco.object.id = name;
603  aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
604 
606 }
607 
608 bool MoveItVisualTools::attachCO(const std::string& name, const std::string& ee_parent_link)
609 {
610  // Attach a collision object
611  moveit_msgs::AttachedCollisionObject aco;
612  aco.object.header.stamp = ros::Time::now();
613  aco.object.header.frame_id = base_frame_;
614 
615  aco.object.id = name;
616  aco.object.operation = moveit_msgs::CollisionObject::ADD;
617 
618  // Link to attach the object to
619  aco.link_name = ee_parent_link;
620 
622 }
623 
624 bool MoveItVisualTools::publishCollisionBlock(const geometry_msgs::Pose& block_pose, const std::string& name,
625  double block_size, const rviz_visual_tools::colors& color)
626 {
627  moveit_msgs::CollisionObject collision_obj;
628  collision_obj.header.stamp = ros::Time::now();
629  collision_obj.header.frame_id = base_frame_;
630  collision_obj.id = name;
631  collision_obj.operation = moveit_msgs::CollisionObject::ADD;
632 
633  collision_obj.primitives.resize(1);
634  collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
635  collision_obj.primitives[0].dimensions.resize(
637  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = block_size;
638  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = block_size;
639  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = block_size;
640  collision_obj.primitive_poses.resize(1);
641  collision_obj.primitive_poses[0] = block_pose;
642 
643  // ROS_INFO_STREAM_NAMED(name_,"CollisionObject: \n " << collision_obj);
644  // ROS_DEBUG_STREAM_NAMED(name_,"Published collision object " << name);
645  return processCollisionObjectMsg(collision_obj, color);
646 }
647 
648 bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2,
649  const std::string& name, const rviz_visual_tools::colors& color)
650 {
651  return publishCollisionCuboid(convertPoint(point1), convertPoint(point2), name, color);
652 }
653 
654 bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2,
655  const std::string& name, const rviz_visual_tools::colors& color)
656 {
657  moveit_msgs::CollisionObject collision_obj;
658  collision_obj.header.stamp = ros::Time::now();
659  collision_obj.header.frame_id = base_frame_;
660  collision_obj.id = name;
661  collision_obj.operation = moveit_msgs::CollisionObject::ADD;
662 
663  // Calculate center pose
664  collision_obj.primitive_poses.resize(1);
665  collision_obj.primitive_poses[0].position.x = (point1.x - point2.x) / 2.0 + point2.x;
666  collision_obj.primitive_poses[0].position.y = (point1.y - point2.y) / 2.0 + point2.y;
667  collision_obj.primitive_poses[0].position.z = (point1.z - point2.z) / 2.0 + point2.z;
668 
669  // Calculate scale
670  collision_obj.primitives.resize(1);
671  collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
672  collision_obj.primitives[0].dimensions.resize(
674  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = fabs(point1.x - point2.x);
675  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = fabs(point1.y - point2.y);
676  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = fabs(point1.z - point2.z);
677 
678  // Prevent scale from being zero
679  if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X])
680  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = rviz_visual_tools::SMALL_SCALE;
681  if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y])
682  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = rviz_visual_tools::SMALL_SCALE;
683  if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z])
684  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = rviz_visual_tools::SMALL_SCALE;
685 
686  // ROS_INFO_STREAM_NAMED(name_,"CollisionObject: \n " << collision_obj);
687  return processCollisionObjectMsg(collision_obj, color);
688 }
689 
690 bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Affine3d& pose, double width, double depth, double height,
691  const std::string& name, const rviz_visual_tools::colors& color)
692 {
693  geometry_msgs::Pose pose_msg;
694  tf::poseEigenToMsg(pose, pose_msg);
695  return publishCollisionCuboid(pose_msg, width, depth, height, name, color);
696 }
697 
698 bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Pose& pose, double width, double depth,
699  double height, const std::string& name,
700  const rviz_visual_tools::colors& color)
701 {
702  moveit_msgs::CollisionObject collision_obj;
703  collision_obj.header.stamp = ros::Time::now();
704  collision_obj.header.frame_id = base_frame_;
705  collision_obj.id = name;
706  collision_obj.operation = moveit_msgs::CollisionObject::ADD;
707 
708  // Calculate center pose
709  collision_obj.primitive_poses.resize(1);
710  collision_obj.primitive_poses[0] = pose;
711 
712  // Calculate scale
713  collision_obj.primitives.resize(1);
714  collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
715  collision_obj.primitives[0].dimensions.resize(
717  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = width;
718  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = depth;
719  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = height;
720 
721  // Prevent scale from being zero
722  if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X])
723  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = rviz_visual_tools::SMALL_SCALE;
724  if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y])
725  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = rviz_visual_tools::SMALL_SCALE;
726  if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z])
727  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = rviz_visual_tools::SMALL_SCALE;
728 
729  return processCollisionObjectMsg(collision_obj, color);
730 }
731 
732 bool MoveItVisualTools::publishCollisionFloor(double z, const std::string& plane_name,
733  const rviz_visual_tools::colors& color)
734 {
735  // Instead just generate a rectangle
736  geometry_msgs::Point point1;
737  geometry_msgs::Point point2;
738 
741  point1.z = z;
742 
743  point2.x = -rviz_visual_tools::LARGE_SCALE;
744  point2.y = -rviz_visual_tools::LARGE_SCALE;
745  point2.z = z - rviz_visual_tools::SMALL_SCALE;
746 
747  return publishCollisionCuboid(point1, point2, plane_name, color);
748 }
749 
750 bool MoveItVisualTools::publishCollisionCylinder(const geometry_msgs::Point& a, const geometry_msgs::Point& b,
751  const std::string& object_name, double radius,
752  const rviz_visual_tools::colors& color)
753 {
754  return publishCollisionCylinder(convertPoint(a), convertPoint(b), object_name, radius, color);
755 }
756 
757 bool MoveItVisualTools::publishCollisionCylinder(const Eigen::Vector3d& a, const Eigen::Vector3d& b,
758  const std::string& object_name, double radius,
759  const rviz_visual_tools::colors& color)
760 {
761  // Distance between two points
762  double height = (a - b).lpNorm<2>();
763 
764  // Find center point
765  Eigen::Vector3d pt_center = getCenterPoint(a, b);
766 
767  // Create vector
768  Eigen::Affine3d pose;
769  pose = getVectorBetweenPoints(pt_center, b);
770 
771  // Convert pose to be normal to cylindar axis
772  Eigen::Affine3d rotation;
773  rotation = Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY());
774  pose = pose * rotation;
775 
776  return publishCollisionCylinder(pose, object_name, radius, height, color);
777 }
778 
779 bool MoveItVisualTools::publishCollisionCylinder(const Eigen::Affine3d& object_pose, const std::string& object_name,
780  double radius, double height, const rviz_visual_tools::colors& color)
781 {
782  return publishCollisionCylinder(convertPose(object_pose), object_name, radius, height, color);
783 }
784 
785 bool MoveItVisualTools::publishCollisionCylinder(const geometry_msgs::Pose& object_pose, const std::string& object_name,
786  double radius, double height, const rviz_visual_tools::colors& color)
787 {
788  moveit_msgs::CollisionObject collision_obj;
789  collision_obj.header.stamp = ros::Time::now();
790  collision_obj.header.frame_id = base_frame_;
791  collision_obj.id = object_name;
792  collision_obj.operation = moveit_msgs::CollisionObject::ADD;
793  collision_obj.primitives.resize(1);
794  collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER;
795  collision_obj.primitives[0].dimensions.resize(
797  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT] = height;
798  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] = radius;
799  collision_obj.primitive_poses.resize(1);
800  collision_obj.primitive_poses[0] = object_pose;
801 
802  // ROS_INFO_STREAM_NAMED(name_,"CollisionObject: \n " << collision_obj);
803  return processCollisionObjectMsg(collision_obj, color);
804 }
805 
806 bool MoveItVisualTools::publishCollisionMesh(const Eigen::Affine3d& object_pose, const std::string& object_name,
807  const std::string& mesh_path, const rviz_visual_tools::colors& color)
808 {
809  return publishCollisionMesh(convertPose(object_pose), object_name, mesh_path, color);
810 }
811 
812 bool MoveItVisualTools::publishCollisionMesh(const geometry_msgs::Pose& object_pose, const std::string& object_name,
813  const std::string& mesh_path, const rviz_visual_tools::colors& color)
814 {
815  shapes::Shape* mesh = shapes::createMeshFromResource(mesh_path); // make sure its prepended by file://
816  shapes::ShapeMsg shape_msg; // this is a boost::variant type from shape_messages.h
817  if (!mesh || !shapes::constructMsgFromShape(mesh, shape_msg))
818  {
819  ROS_ERROR_STREAM_NAMED(name_, "Unable to create mesh shape message from resource " << mesh_path);
820  return false;
821  }
822 
823  if (!publishCollisionMesh(object_pose, object_name, boost::get<shape_msgs::Mesh>(shape_msg), color))
824  return false;
825 
826  ROS_DEBUG_NAMED(name_, "Loaded mesh from '%s'", mesh_path.c_str());
827  return true;
828 }
829 
830 bool MoveItVisualTools::publishCollisionMesh(const Eigen::Affine3d& object_pose, const std::string& object_name,
831  const shape_msgs::Mesh& mesh_msg, const rviz_visual_tools::colors& color)
832 {
833  return publishCollisionMesh(convertPose(object_pose), object_name, mesh_msg, color);
834 }
835 
836 bool MoveItVisualTools::publishCollisionMesh(const geometry_msgs::Pose& object_pose, const std::string& object_name,
837  const shape_msgs::Mesh& mesh_msg, const rviz_visual_tools::colors& color)
838 {
839  // Create collision message
840  moveit_msgs::CollisionObject collision_obj;
841  collision_obj.header.stamp = ros::Time::now();
842  collision_obj.header.frame_id = base_frame_;
843  collision_obj.id = object_name;
844  collision_obj.operation = moveit_msgs::CollisionObject::ADD;
845  collision_obj.mesh_poses.resize(1);
846  collision_obj.mesh_poses[0] = object_pose;
847  collision_obj.meshes.resize(1);
848  collision_obj.meshes[0] = mesh_msg;
849 
850  return processCollisionObjectMsg(collision_obj, color);
851 }
852 
853 bool MoveItVisualTools::publishCollisionGraph(const graph_msgs::GeometryGraph& graph, const std::string& object_name,
854  double radius, const rviz_visual_tools::colors& color)
855 {
856  ROS_INFO_STREAM_NAMED("publishCollisionGraph", "Preparing to create collision graph");
857 
858  // The graph is one collision object with many primitives
859  moveit_msgs::CollisionObject collision_obj;
860  collision_obj.header.stamp = ros::Time::now();
861  collision_obj.header.frame_id = base_frame_;
862  collision_obj.id = object_name;
863  collision_obj.operation = moveit_msgs::CollisionObject::ADD;
864 
865  // Track which pairs of nodes we've already connected since graph is bi-directional
866  typedef std::pair<std::size_t, std::size_t> node_ids;
867  std::set<node_ids> added_edges;
868  std::pair<std::set<node_ids>::iterator, bool> return_value;
869 
870  Eigen::Vector3d a, b;
871  for (std::size_t i = 0; i < graph.nodes.size(); ++i)
872  {
873  for (std::size_t j = 0; j < graph.edges[i].node_ids.size(); ++j)
874  {
875  // Check if we've already added this pair of nodes (edge)
876  return_value = added_edges.insert(node_ids(i, j));
877  if (return_value.second == false)
878  {
879  // Element already existed in set, so don't add a new collision object
880  }
881  else
882  {
883  // Create a cylinder from two points
884  a = convertPoint(graph.nodes[i]);
885  b = convertPoint(graph.nodes[graph.edges[i].node_ids[j]]);
886 
887  // add other direction of edge
888  added_edges.insert(node_ids(j, i));
889 
890  // Distance between two points
891  double height = (a - b).lpNorm<2>();
892 
893  // Find center point
894  Eigen::Vector3d pt_center = getCenterPoint(a, b);
895 
896  // Create vector
897  Eigen::Affine3d pose;
898  pose = getVectorBetweenPoints(pt_center, b);
899 
900  // Convert pose to be normal to cylindar axis
901  Eigen::Affine3d rotation;
902  rotation = Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY());
903  pose = pose * rotation;
904 
905  // Create the solid primitive
906  shape_msgs::SolidPrimitive cylinder;
907  cylinder.type = shape_msgs::SolidPrimitive::CYLINDER;
908  cylinder.dimensions.resize(
910  cylinder.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT] = height;
911  cylinder.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] = radius;
912 
913  // Add to the collision object
914  collision_obj.primitives.push_back(cylinder);
915 
916  // Add the pose
917  collision_obj.primitive_poses.push_back(convertPose(pose));
918  }
919  }
920  }
921 
922  return processCollisionObjectMsg(collision_obj, color);
923 }
924 
925 void MoveItVisualTools::getCollisionWallMsg(double x, double y, double z, double angle, double width, double height,
926  const std::string name, moveit_msgs::CollisionObject& collision_obj)
927 {
928  collision_obj.header.stamp = ros::Time::now();
929  collision_obj.header.frame_id = base_frame_;
930  collision_obj.operation = moveit_msgs::CollisionObject::ADD;
931  collision_obj.primitives.resize(1);
932  collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
933  collision_obj.primitives[0].dimensions.resize(
935 
936  geometry_msgs::Pose rec_pose;
937 
938  // ----------------------------------------------------------------------------------
939  // Name
940  collision_obj.id = name;
941 
942  double depth = 0.1;
943 
944  // Position
945  rec_pose.position.x = x;
946  rec_pose.position.y = y;
947  rec_pose.position.z = height / 2 + z;
948 
949  // Size
950  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = depth;
951  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = width;
952  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = height;
953  // ----------------------------------------------------------------------------------
954 
955  Eigen::Quaterniond quat(Eigen::AngleAxis<double>(static_cast<double>(angle), Eigen::Vector3d::UnitZ()));
956  rec_pose.orientation.x = quat.x();
957  rec_pose.orientation.y = quat.y();
958  rec_pose.orientation.z = quat.z();
959  rec_pose.orientation.w = quat.w();
960 
961  collision_obj.primitive_poses.resize(1);
962  collision_obj.primitive_poses[0] = rec_pose;
963 }
964 
965 bool MoveItVisualTools::publishCollisionWall(double x, double y, double angle, double width, double height,
966  const std::string name, const rviz_visual_tools::colors& color)
967 {
968  return publishCollisionWall(x, y, 0.0, angle, width, height, name, color);
969 }
970 
971 bool MoveItVisualTools::publishCollisionWall(double x, double y, double z, double angle, double width, double height,
972  const std::string name, const rviz_visual_tools::colors& color)
973 {
974  moveit_msgs::CollisionObject collision_obj;
975  getCollisionWallMsg(x, y, z, angle, width, height, name, collision_obj);
976 
977  return processCollisionObjectMsg(collision_obj, color);
978 }
979 
980 bool MoveItVisualTools::publishCollisionTable(double x, double y, double z, double angle, double width, double height,
981  double depth, const std::string name,
982  const rviz_visual_tools::colors& color)
983 {
984  geometry_msgs::Pose table_pose;
985 
986  // Center of table
987  table_pose.position.x = x;
988  table_pose.position.y = y;
989  table_pose.position.z = z + height / 2.0;
990 
991  // Orientation
992  Eigen::Quaterniond quat(Eigen::AngleAxis<double>(static_cast<double>(angle), Eigen::Vector3d::UnitZ()));
993  table_pose.orientation.x = quat.x();
994  table_pose.orientation.y = quat.y();
995  table_pose.orientation.z = quat.z();
996  table_pose.orientation.w = quat.w();
997 
998  moveit_msgs::CollisionObject collision_obj;
999  collision_obj.header.stamp = ros::Time::now();
1000  collision_obj.header.frame_id = base_frame_;
1001  collision_obj.id = name;
1002  collision_obj.operation = moveit_msgs::CollisionObject::ADD;
1003  collision_obj.primitives.resize(1);
1004  collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
1005  collision_obj.primitives[0].dimensions.resize(
1007 
1008  // Size
1009  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = depth;
1010  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = width;
1011  collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = height;
1012 
1013  collision_obj.primitive_poses.resize(1);
1014  collision_obj.primitive_poses[0] = table_pose;
1015  return processCollisionObjectMsg(collision_obj, color);
1016 }
1017 
1019 {
1020  return loadCollisionSceneFromFile(path, Eigen::Affine3d::Identity());
1021 }
1022 
1023 bool MoveItVisualTools::loadCollisionSceneFromFile(const std::string& path, const Eigen::Affine3d& offset)
1024 {
1025  // Open file
1026  std::ifstream fin(path.c_str());
1027  if (fin.good())
1028  {
1029  // Load directly to the planning scene
1031  {
1032  if (scene)
1033  {
1034  scene->loadGeometryFromStream(fin, offset);
1035  }
1036  else
1037  {
1038  ROS_WARN_STREAM_NAMED(name_, "Unable to get locked planning scene RW");
1039  return false;
1040  }
1041  }
1042  ROS_INFO_NAMED(name_, "Loaded scene geometry from '%s'", path.c_str());
1043  }
1044  else
1045  ROS_WARN_NAMED(name_, "Unable to load scene geometry from '%s'", path.c_str());
1046 
1047  fin.close();
1048 
1049  return triggerPlanningSceneUpdate();
1050 }
1051 
1052 bool MoveItVisualTools::publishWorkspaceParameters(const moveit_msgs::WorkspaceParameters& params)
1053 {
1054  return publishCuboid(convertPoint(params.min_corner), convertPoint(params.max_corner), rviz_visual_tools::TRANSLUCENT,
1055  "Planning_Workspace", 1);
1056 }
1057 
1060  const rviz_visual_tools::colors& color)
1061 {
1062  // Compute the contacts if any
1065  c_req.contacts = true;
1066  c_req.max_contacts = 10;
1067  c_req.max_contacts_per_pair = 3;
1068  c_req.verbose = true;
1069 
1070  // Check for collisions
1071  planning_scene->checkCollision(c_req, c_res, robot_state);
1072 
1073  // Display
1074  if (c_res.contact_count > 0)
1075  {
1076  visualization_msgs::MarkerArray arr;
1078  ROS_INFO_STREAM_NAMED(name_, "Completed listing of explanations for invalid states.");
1079 
1080  // Check for markers
1081  if (arr.markers.empty())
1082  return true;
1083 
1084  // Convert markers to same namespace and other custom stuff
1085  for (std::size_t i = 0; i < arr.markers.size(); ++i)
1086  {
1087  arr.markers[i].ns = "Collision";
1088  arr.markers[i].scale.x = 0.04;
1089  arr.markers[i].scale.y = 0.04;
1090  arr.markers[i].scale.z = 0.04;
1091  arr.markers[i].color = getColor(color);
1092  }
1093 
1094  return publishMarkers(arr);
1095  }
1096  return true;
1097 }
1098 
1099 bool MoveItVisualTools::publishTrajectoryPoint(const trajectory_msgs::JointTrajectoryPoint& trajectory_pt,
1100  const std::string& planning_group, double display_time)
1101 {
1102  // Get joint state group
1103  const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group);
1104 
1105  if (jmg == NULL) // not found
1106  {
1107  ROS_ERROR_STREAM_NAMED("publishTrajectoryPoint", "Could not find joint model group " << planning_group);
1108  return false;
1109  }
1110 
1111  // Apply the time to the trajectory
1112  trajectory_msgs::JointTrajectoryPoint trajectory_pt_timed = trajectory_pt;
1113  trajectory_pt_timed.time_from_start = ros::Duration(display_time);
1114 
1115  // Create a trajectory with one point
1116  moveit_msgs::RobotTrajectory trajectory_msg;
1117  trajectory_msg.joint_trajectory.header.frame_id = base_frame_;
1118  trajectory_msg.joint_trajectory.joint_names = jmg->getJointModelNames();
1119  trajectory_msg.joint_trajectory.points.push_back(trajectory_pt);
1120  trajectory_msg.joint_trajectory.points.push_back(trajectory_pt_timed);
1121 
1122  return publishTrajectoryPath(trajectory_msg, shared_robot_state_, true);
1123 }
1124 
1125 bool MoveItVisualTools::publishTrajectoryPath(const std::vector<robot_state::RobotStatePtr>& trajectory,
1126  const moveit::core::JointModelGroup* jmg, double speed, bool blocking)
1127 {
1128  // Copy the vector of RobotStates to a RobotTrajectory
1129  robot_trajectory::RobotTrajectoryPtr robot_trajectory(
1131 
1132  double duration_from_previous = 0;
1133  for (std::size_t k = 0; k < trajectory.size(); ++k)
1134  {
1135  duration_from_previous += speed;
1136  robot_trajectory->addSuffixWayPoint(trajectory[k], duration_from_previous);
1137  }
1138 
1139  // Convert trajectory to a message
1140  moveit_msgs::RobotTrajectory trajectory_msg;
1141  robot_trajectory->getRobotTrajectoryMsg(trajectory_msg);
1142 
1143  return publishTrajectoryPath(trajectory_msg, shared_robot_state_, blocking);
1144 }
1145 
1146 bool MoveItVisualTools::publishTrajectoryPath(const robot_trajectory::RobotTrajectoryPtr& trajectory, bool blocking)
1147 {
1148  return publishTrajectoryPath(*trajectory, blocking);
1149 }
1150 
1152 {
1153  moveit_msgs::RobotTrajectory trajectory_msg;
1154  trajectory.getRobotTrajectoryMsg(trajectory_msg);
1155 
1156  // Add time from start if none specified
1157  if (trajectory_msg.joint_trajectory.points.size() > 1)
1158  {
1159  if (trajectory_msg.joint_trajectory.points[1].time_from_start == ros::Duration(0)) // assume no timestamps exist
1160  {
1161  for (std::size_t i = 0; i < trajectory_msg.joint_trajectory.points.size(); ++i)
1162  {
1163  trajectory_msg.joint_trajectory.points[i].time_from_start = ros::Duration(i * 2); // 1 hz
1164  }
1165  }
1166  }
1167 
1168  return publishTrajectoryPath(trajectory_msg, shared_robot_state_, blocking);
1169 }
1170 
1171 bool MoveItVisualTools::publishTrajectoryPath(const moveit_msgs::RobotTrajectory& trajectory_msg,
1172  const robot_state::RobotStateConstPtr robot_state, bool blocking)
1173 {
1174  return publishTrajectoryPath(trajectory_msg, *robot_state, blocking);
1175 }
1176 
1177 bool MoveItVisualTools::publishTrajectoryPath(const moveit_msgs::RobotTrajectory& trajectory_msg,
1178  const moveit::core::RobotState& robot_state, bool blocking)
1179 {
1180  // Convert the robot state to a ROS message
1181  moveit_msgs::RobotState robot_state_msg;
1182  robot_state::robotStateToRobotStateMsg(robot_state, robot_state_msg);
1183  return publishTrajectoryPath(trajectory_msg, robot_state_msg, blocking);
1184 }
1185 
1186 bool MoveItVisualTools::publishTrajectoryPath(const moveit_msgs::RobotTrajectory& trajectory_msg,
1187  const moveit_msgs::RobotState& robot_state, bool blocking)
1188 {
1189  // Check if we have enough points
1190  if (!trajectory_msg.joint_trajectory.points.size())
1191  {
1192  ROS_WARN_STREAM_NAMED(name_, "Unable to publish trajectory path because trajectory has zero points");
1193  return false;
1194  }
1195 
1196  // Create the message
1197  moveit_msgs::DisplayTrajectory display_trajectory_msg;
1198  display_trajectory_msg.model_id = robot_model_->getName();
1199  display_trajectory_msg.trajectory.resize(1);
1200  display_trajectory_msg.trajectory[0] = trajectory_msg;
1201  display_trajectory_msg.trajectory_start = robot_state;
1202 
1203  // Publish message
1204  loadTrajectoryPub(); // always call this before publishing
1205  pub_display_path_.publish(display_trajectory_msg);
1206  ros::spinOnce();
1207 
1208  // Wait the duration of the trajectory
1209  if (blocking)
1210  {
1211  double duration = trajectory_msg.joint_trajectory.points.back().time_from_start.toSec();
1212 
1213  // If trajectory has not been parameterized, assume each waypoint takes 0.05 seconds (based on Rviz)
1214  if (duration < std::numeric_limits<double>::epsilon())
1215  {
1216  duration = 0.05 * trajectory_msg.joint_trajectory.points.size();
1217  }
1218  ROS_DEBUG_STREAM_NAMED(name_, "Waiting for trajectory animation " << duration << " seconds");
1219 
1220  // Check if ROS is ok in intervals
1221  double counter = 0;
1222  static const double CHECK_TIME_INTERVAL = 0.25; // check every fourth second
1223  while (ros::ok() && counter <= duration)
1224  {
1225  counter += CHECK_TIME_INTERVAL;
1226  ros::Duration(CHECK_TIME_INTERVAL).sleep();
1227  }
1228  }
1229 
1230  return true;
1231 }
1232 
1233 bool MoveItVisualTools::publishTrajectoryLine(const moveit_msgs::RobotTrajectory& trajectory_msg,
1234  const moveit::core::LinkModel* ee_parent_link,
1235  const robot_model::JointModelGroup* arm_jmg,
1236  const rviz_visual_tools::colors& color)
1237 {
1238  // Error check
1239  if (!arm_jmg)
1240  {
1241  ROS_FATAL_STREAM_NAMED(name_, "arm_jmg is NULL");
1242  return false;
1243  }
1244 
1245  // Always load the robot state before using
1247 
1248  // Convert trajectory into a series of RobotStates
1249  robot_trajectory::RobotTrajectoryPtr robot_trajectory(
1250  new robot_trajectory::RobotTrajectory(robot_model_, arm_jmg->getName()));
1251  robot_trajectory->setRobotTrajectoryMsg(*shared_robot_state_, trajectory_msg);
1252 
1253  return publishTrajectoryLine(robot_trajectory, ee_parent_link, color);
1254 }
1255 
1256 bool MoveItVisualTools::publishTrajectoryLine(const robot_trajectory::RobotTrajectoryPtr robot_trajectory,
1257  const moveit::core::LinkModel* ee_parent_link,
1258  const rviz_visual_tools::colors& color)
1259 {
1260  return publishTrajectoryLine(*robot_trajectory, ee_parent_link, color);
1261 }
1262 
1264  const moveit::core::LinkModel* ee_parent_link,
1265  const rviz_visual_tools::colors& color)
1266 {
1267  // Error check
1268  if (!ee_parent_link)
1269  {
1270  ROS_FATAL_STREAM_NAMED(name_, "ee_parent_link is NULL");
1271  return false;
1272  }
1273 
1274  // Point location datastructure
1276 
1277  // Visualize end effector position of cartesian path
1278  for (std::size_t i = 0; i < robot_trajectory.getWayPointCount(); ++i)
1279  {
1280  const Eigen::Affine3d& tip_pose = robot_trajectory.getWayPoint(i).getGlobalLinkTransform(ee_parent_link);
1281 
1282  // Error Check
1283  if (tip_pose.translation().x() != tip_pose.translation().x())
1284  {
1285  ROS_ERROR_STREAM_NAMED(name_, "NAN DETECTED AT TRAJECTORY POINT i=" << i);
1286  return false;
1287  }
1288 
1289  path.push_back(tip_pose.translation());
1290  publishSphere(tip_pose, color, rviz_visual_tools::MEDIUM);
1291  }
1292 
1293  const double radius = 0.005;
1294  publishPath(path, color, radius);
1295 
1296  return true;
1297 }
1298 
1299 bool MoveItVisualTools::publishTrajectoryLine(const moveit_msgs::RobotTrajectory& trajectory_msg,
1300  const robot_model::JointModelGroup* arm_jmg,
1301  const rviz_visual_tools::colors& color)
1302 {
1303  std::vector<const moveit::core::LinkModel*> tips;
1304  if (!arm_jmg->getEndEffectorTips(tips))
1305  {
1306  ROS_ERROR_STREAM_NAMED(name_, "Unable to get end effector tips from jmg");
1307  return false;
1308  }
1309 
1310  // For each end effector
1311  for (const moveit::core::LinkModel* ee_parent_link : tips)
1312  {
1313  if (!publishTrajectoryLine(trajectory_msg, ee_parent_link, arm_jmg, color))
1314  return false;
1315  }
1316 
1317  return true;
1318 }
1319 
1320 bool MoveItVisualTools::publishTrajectoryLine(const robot_trajectory::RobotTrajectoryPtr robot_trajectory,
1321  const robot_model::JointModelGroup* arm_jmg,
1322  const rviz_visual_tools::colors& color)
1323 {
1324  return publishTrajectoryLine(*robot_trajectory, arm_jmg, color);
1325 }
1326 
1328  const robot_model::JointModelGroup* arm_jmg,
1329  const rviz_visual_tools::colors& color)
1330 {
1331  std::vector<const moveit::core::LinkModel*> tips;
1332  if (!arm_jmg->getEndEffectorTips(tips))
1333  {
1334  ROS_ERROR_STREAM_NAMED(name_, "Unable to get end effector tips from jmg");
1335  return false;
1336  }
1337 
1338  // For each end effector
1339  for (const moveit::core::LinkModel* ee_parent_link : tips)
1340  {
1341  if (!publishTrajectoryLine(robot_trajectory, ee_parent_link, color))
1342  return false;
1343  }
1344 
1345  return true;
1346 }
1347 
1348 bool MoveItVisualTools::publishTrajectoryPoints(const std::vector<robot_state::RobotStatePtr>& robot_state_trajectory,
1349  const moveit::core::LinkModel* ee_parent_link,
1350  const rviz_visual_tools::colors& color)
1351 {
1352  // Visualize end effector position of cartesian path
1353  for (std::size_t i = 0; i < robot_state_trajectory.size(); ++i)
1354  {
1355  const Eigen::Affine3d& tip_pose = robot_state_trajectory[i]->getGlobalLinkTransform(ee_parent_link);
1356 
1357  publishSphere(tip_pose, color);
1358  }
1359  return true;
1360 }
1361 
1362 void MoveItVisualTools::enableRobotStateRootOffet(const Eigen::Affine3d& offset)
1363 {
1365  robot_state_root_offset_ = offset;
1366 }
1367 
1369 {
1371 }
1372 
1373 bool MoveItVisualTools::publishRobotState(const trajectory_msgs::JointTrajectoryPoint& trajectory_pt,
1374  const robot_model::JointModelGroup* jmg,
1375  const rviz_visual_tools::colors& color)
1376 {
1377  return publishRobotState(trajectory_pt.positions, jmg, color);
1378 }
1379 
1380 bool MoveItVisualTools::publishRobotState(const std::vector<double> joint_positions,
1381  const robot_model::JointModelGroup* jmg,
1382  const rviz_visual_tools::colors& color)
1383 {
1384  // Always load the robot state before using
1386 
1387  // Set robot state
1388  shared_robot_state_->setToDefaultValues(); // reset the state just in case
1389  shared_robot_state_->setJointGroupPositions(jmg, joint_positions);
1390 
1391  // Publish robot state
1392  return publishRobotState(*shared_robot_state_, color);
1393 }
1394 
1395 bool MoveItVisualTools::publishRobotState(const robot_state::RobotStatePtr& robot_state,
1396  const rviz_visual_tools::colors& color)
1397 {
1398  return publishRobotState(*robot_state.get(), color);
1399 }
1400 
1401 bool MoveItVisualTools::publishRobotState(const robot_state::RobotState& robot_state,
1402  const rviz_visual_tools::colors& color)
1403 {
1404  // Reference to the correctly colored version of message (they are cached)
1405  // May not exist yet but this will create it
1406  moveit_msgs::DisplayRobotState& display_robot_msg = display_robot_msgs_[color];
1407 
1408  // Check if a robot state message already exists for this color
1409  if (display_robot_msg.highlight_links.size() == 0) // has not been colored yet, lets create that
1410  {
1411  if (color != rviz_visual_tools::DEFAULT) // ignore color highlights when set to default
1412  {
1413  // Get links names
1414  const std::vector<const moveit::core::LinkModel*>& link_names =
1415  robot_state.getRobotModel()->getLinkModelsWithCollisionGeometry();
1416  display_robot_msg.highlight_links.resize(link_names.size());
1417 
1418  // Get color
1419  const std_msgs::ColorRGBA& color_rgba = getColor(color);
1420 
1421  // Color every link
1422  for (std::size_t i = 0; i < link_names.size(); ++i)
1423  {
1424  display_robot_msg.highlight_links[i].id = link_names[i]->getName();
1425  display_robot_msg.highlight_links[i].color = color_rgba;
1426  }
1427  }
1428  }
1429 
1430  // Apply the offset
1432  {
1434 
1435  // Copy robot state
1436  *shared_robot_state_ = robot_state;
1437 
1439 
1440  // Convert state to message
1441  robot_state::robotStateToRobotStateMsg(*shared_robot_state_, display_robot_msg.state);
1442  }
1443  else
1444  {
1445  // Convert state to message
1446  robot_state::robotStateToRobotStateMsg(robot_state, display_robot_msg.state);
1447  }
1448 
1449  // Publish
1451  pub_robot_state_.publish(display_robot_msg);
1452  ros::spinOnce();
1453 
1454  return true;
1455 }
1456 
1458 {
1459  static const std::string VJOINT_NAME = "virtual_joint";
1460 
1461  // Always load the robot state before using
1463 
1464  // Apply transform
1465  Eigen::Affine3d offset;
1466  offset.translation().x() = rviz_visual_tools::LARGE_SCALE;
1467  offset.translation().y() = rviz_visual_tools::LARGE_SCALE;
1468  offset.translation().z() = rviz_visual_tools::LARGE_SCALE;
1470 
1471  // Publish
1473 }
1474 
1475 void MoveItVisualTools::showJointLimits(robot_state::RobotStatePtr robot_state)
1476 {
1477  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
1478 
1479  // Loop through joints
1480  for (std::size_t i = 0; i < joints.size(); ++i)
1481  {
1482  // Assume all joints have only one variable
1483  if (joints[i]->getVariableCount() > 1)
1484  {
1485  // ROS_WARN_STREAM_NAMED(name_, "Unable to handle joints with more than one var, skipping '"
1486  //<< joints[i]->getName() << "'");
1487  continue;
1488  }
1489 
1490  double current_value = robot_state->getVariablePosition(joints[i]->getName());
1491 
1492  // check if bad position
1493  bool out_of_bounds = !robot_state->satisfiesBounds(joints[i]);
1494 
1495  const moveit::core::VariableBounds& bound = joints[i]->getVariableBounds()[0];
1496 
1497  if (out_of_bounds)
1498  std::cout << MOVEIT_CONSOLE_COLOR_RED;
1499 
1500  std::cout << " " << std::fixed << std::setprecision(5) << bound.min_position_ << "\t";
1501  double delta = bound.max_position_ - bound.min_position_;
1502  // std::cout << "delta: " << delta << " ";
1503  double step = delta / 20.0;
1504 
1505  bool marker_shown = false;
1506  for (double value = bound.min_position_; value < bound.max_position_; value += step)
1507  {
1508  // show marker of current value
1509  if (!marker_shown && current_value < value)
1510  {
1511  std::cout << "|";
1512  marker_shown = true;
1513  }
1514  else
1515  std::cout << "-";
1516  }
1517  // show max position
1518  std::cout << " \t" << std::fixed << std::setprecision(5) << bound.max_position_ << " \t" << joints[i]->getName()
1519  << " current: " << std::fixed << std::setprecision(5) << current_value << std::endl;
1520 
1521  if (out_of_bounds)
1522  std::cout << MOVEIT_CONSOLE_COLOR_RESET;
1523  }
1524 }
1525 
1532 planning_scene_monitor::PlanningSceneMonitorPtr MoveItVisualTools::getPlanningSceneMonitor()
1533 {
1534  if (!psm_)
1535  {
1536  ROS_INFO_STREAM_NAMED(name_, "No planning scene passed into moveit_visual_tools, creating one.");
1538  ros::spinOnce();
1539  ros::Duration(1).sleep(); // TODO: is this necessary?
1540  }
1541  return psm_;
1542 }
1543 
1545 {
1546  static const std::string VJOINT_NAME = "virtual_joint";
1547 
1548  // Check if joint exists
1549  if (!robot_state.getRobotModel()->hasJointModel(VJOINT_NAME))
1550  {
1551  ROS_WARN_STREAM_NAMED("moveit_visual_tools", "Joint '" << VJOINT_NAME << "' does not exist.");
1552  const std::vector<std::string>& names = robot_state.getRobotModel()->getJointModelNames();
1553  // ROS_DEBUG_STREAM_NAMED("moveit_visual_tools", "Available names:");
1554  // std::copy(names.begin(), names.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
1555 
1556  return false;
1557  }
1558 
1559  // Check if variables exist
1560  if (!robot_state.getRobotModel()->getJointModel(VJOINT_NAME)->hasVariable(VJOINT_NAME + "/trans_x"))
1561  {
1562  // Debug
1563  ROS_WARN_STREAM_NAMED("moveit_visual_tools", "Variables for joint '" << VJOINT_NAME
1564  << "' do not exist. Try making this vjoint "
1565  "floating");
1566  ROS_WARN_STREAM_NAMED("moveit_visual_tools", "The only available joint variables are:");
1567  const std::vector<std::string>& var_names =
1568  robot_state.getRobotModel()->getJointModel(VJOINT_NAME)->getVariableNames();
1569  std::copy(var_names.begin(), var_names.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
1570  return false;
1571  }
1572 
1573  return true;
1574 }
1575 
1576 bool MoveItVisualTools::applyVirtualJointTransform(moveit::core::RobotState& robot_state, const Eigen::Affine3d& offset)
1577 {
1578  static const std::string VJOINT_NAME = "virtual_joint";
1579 
1580  // Error check
1581  if (!checkForVirtualJoint(robot_state))
1582  {
1583  ROS_WARN_STREAM_NAMED("moveit_visual_tools", "Unable to apply virtual joint transform, hideRobot() functionality "
1584  "is disabled");
1585  return false;
1586  }
1587 
1588  // Apply translation
1589  robot_state.setVariablePosition(VJOINT_NAME + "/trans_x", offset.translation().x());
1590  robot_state.setVariablePosition(VJOINT_NAME + "/trans_y", offset.translation().y());
1591  robot_state.setVariablePosition(VJOINT_NAME + "/trans_z", offset.translation().z());
1592 
1593  // Apply rotation
1594  Eigen::Quaterniond q(offset.rotation());
1595  robot_state.setVariablePosition(VJOINT_NAME + "/rot_x", q.x());
1596  robot_state.setVariablePosition(VJOINT_NAME + "/rot_y", q.y());
1597  robot_state.setVariablePosition(VJOINT_NAME + "/rot_z", q.z());
1598  robot_state.setVariablePosition(VJOINT_NAME + "/rot_w", q.w());
1599 
1600  return true;
1601 }
1602 
1603 } // namespace moveit_visual_tools
#define NULL
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 ROS_INFO_NAMED(name,...)
#define MOVEIT_CONSOLE_COLOR_RESET
#define ROS_DEBUG_STREAM_NAMED(name, args)
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.
void publish(const boost::shared_ptr< M > &message) const
#define ROS_ERROR_STREAM_NAMED(name, args)
bool publishCuboid(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2, colors color=BLUE)
#define ROS_WARN_NAMED(name,...)
planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitor()
Get the planning scene monitor that this class is using.
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
bool waitForSubscriber(const ros::Publisher &pub, double wait_time=0.5, bool blocking=false)
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.
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
MoveItVisualTools(const std::string &base_frame, const std::string &marker_topic, planning_scene_monitor::PlanningSceneMonitorPtr psm)
Constructor.
void getCollisionMarkersFromContacts(visualization_msgs::MarkerArray &arr, const std::string &frame_id, const CollisionResult::ContactMap &con, const std_msgs::ColorRGBA &color, const ros::Duration &lifetime, const double radius=0.035)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
std_msgs::ColorRGBA getColor(colors color) const
const std::string & getName() const
bool sleep() const
moveit::core::RobotModelConstPtr robot_model_
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.
std::string getName(void *handle)
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.
#define ROS_INFO_STREAM_NAMED(name, args)
#define M_PI
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...
planning_scene_monitor::PlanningSceneMonitorPtr psm_
moveit::core::RobotStatePtr hidden_robot_state_
void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory &trajectory) const
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...
#define ROS_DEBUG_NAMED(name,...)
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.
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.
#define ROS_FATAL_STREAM_NAMED(name, args)
static const double SMALL_SCALE
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.
void checkCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
ROSCPP_DECL bool ok()
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.
bool publishPath(const std::vector< geometry_msgs::Pose > &path, colors color=RED, scales scale=MEDIUM, const std::string &ns="Path")
moveit::core::RobotStatePtr & getSharedRobotState()
Allow robot state to be altered.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
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.
std::map< const robot_model::JointModelGroup *, EigenSTL::vector_Affine3d > ee_poses_map_
#define MOVEIT_CONSOLE_COLOR_RED
static const double LARGE_SCALE
const std::string & getPlanningFrame() const
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...
const RobotModelConstPtr & getRobotModel() const
static geometry_msgs::Pose convertPose(const Eigen::Affine3d &pose)
bool cleanupCO(const std::string &name)
Remove a collision object from the planning scene.
const robot_state::RobotState & getWayPoint(std::size_t index) const
unsigned int step
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.
std::size_t getWayPointCount() const
Eigen::Vector3d getCenterPoint(const Eigen::Vector3d &a, const Eigen::Vector3d &b) const
void loadRobotStatePub(const std::string &robot_state_topic="", bool blocking=true)
Eigen::Affine3d getVectorBetweenPoints(const Eigen::Vector3d &a, const Eigen::Vector3d &b) const
moveit::core::RobotStatePtr shared_robot_state_
bool constructMsgFromShape(const Shape *shape, ShapeMsg &shape_msg)
Mesh * createMeshFromResource(const std::string &resource)
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 Eigen::Vector3d convertPoint(const geometry_msgs::Point &point)
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.
static const std::string name_
bool publishArrow(const Eigen::Affine3d &pose, colors color=BLUE, scales scale=MEDIUM, double length=0.0, std::size_t id=0)
static Time now()
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 publishSphere(const Eigen::Affine3d &pose, colors color=BLUE, scales scale=MEDIUM, const std::string &ns="Sphere", std::size_t id=0)
std::string getTopic() const
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 setVariablePosition(const std::string &variable, double value)
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.
bool publishMarkers(visualization_msgs::MarkerArray &markers)
ROSCPP_DECL void spinOnce()
RvizVisualTools(std::string base_frame, std::string marker_topic=RVIZ_MARKER_TOPIC, ros::NodeHandle nh=ros::NodeHandle("~"))
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...
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.
boost::variant< shape_msgs::SolidPrimitive, shape_msgs::Mesh, shape_msgs::Plane > ShapeMsg
#define ROS_WARN_STREAM_NAMED(name, args)
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