41 #include <moveit_msgs/CollisionObject.h>
68 const std::string
LOGNAME =
"visual_tools";
75 loadSharedRobotState();
76 setBaseFrame(robot_model_->getModelFrame());
80 planning_scene_monitor::PlanningSceneMonitorPtr psm)
81 : RvizVisualTools::RvizVisualTools(base_frame, marker_topic)
82 , psm_(
std::move(psm))
89 moveit::core::RobotModelConstPtr robot_model)
90 : RvizVisualTools::RvizVisualTools(base_frame, marker_topic), robot_model_(
std::move(robot_model))
100 "already been set for Visual Tools");
106 std::shared_ptr<tf2_ros::Buffer>
tf_buffer = std::make_shared<tf2_ros::Buffer>();
107 std::shared_ptr<tf2_ros::TransformListener> tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);
116 if (
psm_->getPlanningScene())
145 scene->getCurrentStateNonConst().update();
146 scene->processCollisionObjectMsg(msg);
147 scene->setObjectColor(msg.id,
getColor(color));
164 scene->processAttachedCollisionObjectMsg(msg);
185 moveit_msgs::CollisionObject collision_obj;
188 collision_obj.id =
name;
189 collision_obj.operation = moveit_msgs::CollisionObject::MOVE;
191 collision_obj.primitive_poses.resize(1);
192 collision_obj.primitive_poses[0] = pose;
250 const std::vector<double>& ee_joint_pos)
253 if (ee_jmg ==
nullptr)
264 if (!ee_joint_pos.empty())
269 << ee_joint_pos.size() <<
") does not match the number of active joints in "
285 static std::size_t marker_id_offset = 0;
304 "No links found to visualize in end effector for joint model group: " << ee_jmg->getName());
308 const std::string& ee_parent_link_name = ee_jmg->getEndEffectorParentGroup().second;
312 Eigen::Isometry3d ee_marker_global_transform =
shared_robot_state_->getGlobalLinkTransform(ee_parent_link);
313 Eigen::Isometry3d ee_marker_pose;
316 for (std::size_t i = 0; i <
ee_markers_map_[ee_jmg].markers.size(); ++i)
322 if (
ee_markers_map_[ee_jmg].markers[i].type == visualization_msgs::Marker::MESH_RESOURCE)
360 if (!robot_state_topic.empty())
373 const std::vector<double>& ee_joint_pos,
387 Eigen::Isometry3d eigen_goal_ee_pose =
convertPose(pose);
388 Eigen::Isometry3d eigen_this_marker;
391 for (std::size_t i = 0; i <
ee_markers_map_[ee_jmg].markers.size(); ++i)
411 eigen_this_marker = eigen_goal_ee_pose *
ee_poses_map_[ee_jmg][i];
433 for (std::size_t i = 0; i < possible_grasps.size(); ++i)
450 << ee_jmg->
getName() <<
" at speed " << animate_speed);
453 for (std::size_t i = 0; i < possible_grasps.size(); ++i)
469 geometry_msgs::Pose grasp_pose = grasp.grasp_pose.pose;
477 Eigen::Isometry3d grasp_pose_eigen;
481 geometry_msgs::Pose pre_grasp_pose;
482 Eigen::Isometry3d pre_grasp_pose_eigen;
485 Eigen::Vector3d pre_grasp_approach_direction_local;
495 double animation_resulution = 0.1;
496 for (
double percent = 0; percent < 1; percent += animation_resulution)
502 pre_grasp_pose_eigen = grasp_pose_eigen;
506 Eigen::Vector3d pre_grasp_approach_direction = Eigen::Vector3d(
507 -1 * grasp.pre_grasp_approach.direction.vector.x * grasp.pre_grasp_approach.min_distance * (1 - percent),
508 -1 * grasp.pre_grasp_approach.direction.vector.y * grasp.pre_grasp_approach.min_distance * (1 - percent),
509 -1 * grasp.pre_grasp_approach.direction.vector.z * grasp.pre_grasp_approach.min_distance * (1 - percent));
515 if (grasp.pre_grasp_approach.direction.header.frame_id == ee_parent_link_name)
519 pre_grasp_approach_direction_local = grasp_pose_eigen.rotation() * pre_grasp_approach_direction;
523 pre_grasp_approach_direction_local = pre_grasp_approach_direction;
528 pre_grasp_pose_eigen.translation() += pre_grasp_approach_direction_local;
531 pre_grasp_pose =
tf2::toMsg(pre_grasp_pose_eigen);
547 if (ik_solutions.empty())
558 trajectory_msgs::JointTrajectoryPoint trajectory_pt_timed;
561 moveit_msgs::RobotTrajectory trajectory_msg;
562 trajectory_msg.joint_trajectory.header.frame_id =
base_frame_;
566 double running_time = 0;
569 for (std::size_t i = 0; i < ik_solutions.size(); ++i)
571 trajectory_pt_timed = ik_solutions[i];
572 trajectory_pt_timed.time_from_start =
ros::Duration(running_time);
573 trajectory_msg.joint_trajectory.points.push_back(trajectory_pt_timed);
575 running_time += display_time;
579 trajectory_pt_timed = trajectory_msg.joint_trajectory.points.back();
580 trajectory_pt_timed.time_from_start =
ros::Duration(running_time);
581 trajectory_msg.joint_trajectory.points.push_back(trajectory_pt_timed);
591 scene->removeAllCollisionObjects();
600 moveit_msgs::CollisionObject co;
604 co.operation = moveit_msgs::CollisionObject::REMOVE;
612 moveit_msgs::AttachedCollisionObject aco;
617 aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
625 moveit_msgs::AttachedCollisionObject aco;
630 aco.object.operation = moveit_msgs::CollisionObject::ADD;
633 aco.link_name = ee_parent_link;
641 moveit_msgs::CollisionObject collision_obj;
644 collision_obj.id =
name;
645 collision_obj.operation = moveit_msgs::CollisionObject::ADD;
647 collision_obj.primitives.resize(1);
648 collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
649 collision_obj.primitives[0].dimensions.resize(
650 geometric_shapes::solidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>());
651 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = block_size;
652 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = block_size;
653 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = block_size;
654 collision_obj.primitive_poses.resize(1);
655 collision_obj.primitive_poses[0] = block_pose;
671 moveit_msgs::CollisionObject collision_obj;
674 collision_obj.id =
name;
675 collision_obj.operation = moveit_msgs::CollisionObject::ADD;
678 collision_obj.primitive_poses.resize(1);
679 collision_obj.primitive_poses[0].position.x = (point1.x - point2.x) / 2.0 + point2.x;
680 collision_obj.primitive_poses[0].position.y = (point1.y - point2.y) / 2.0 + point2.y;
681 collision_obj.primitive_poses[0].position.z = (point1.z - point2.z) / 2.0 + point2.z;
684 collision_obj.primitives.resize(1);
685 collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
686 collision_obj.primitives[0].dimensions.resize(
687 geometric_shapes::solidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>());
688 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = fabs(point1.x - point2.x);
689 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = fabs(point1.y - point2.y);
690 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = fabs(point1.z - point2.z);
693 if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X])
695 if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y])
697 if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z])
707 geometry_msgs::Pose pose_msg =
tf2::toMsg(pose);
712 double height,
const std::string& name,
715 moveit_msgs::CollisionObject collision_obj;
718 collision_obj.id =
name;
719 collision_obj.operation = moveit_msgs::CollisionObject::ADD;
722 collision_obj.primitive_poses.resize(1);
723 collision_obj.primitive_poses[0] = pose;
726 collision_obj.primitives.resize(1);
727 collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
728 collision_obj.primitives[0].dimensions.resize(
729 geometric_shapes::solidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>());
730 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = width;
731 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = depth;
732 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = height;
735 if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X])
737 if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y])
739 if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z])
749 geometry_msgs::Point point1;
750 geometry_msgs::Point point2;
764 const std::string& object_name,
double radius,
771 const std::string& object_name,
double radius,
775 double height = (a - b).lpNorm<2>();
781 Eigen::Isometry3d pose;
785 Eigen::Isometry3d rotation;
786 rotation = Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY());
787 pose = pose * rotation;
801 moveit_msgs::CollisionObject collision_obj;
804 collision_obj.id = object_name;
805 collision_obj.operation = moveit_msgs::CollisionObject::ADD;
806 collision_obj.primitives.resize(1);
807 collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER;
808 collision_obj.primitives[0].dimensions.resize(
809 geometric_shapes::solidPrimitiveDimCount<shape_msgs::SolidPrimitive::CYLINDER>());
810 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT] = height;
811 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] = radius;
812 collision_obj.primitive_poses.resize(1);
813 collision_obj.primitive_poses[0] = object_pose;
836 if (!
publishCollisionMesh(object_pose, object_name, boost::get<shape_msgs::Mesh>(shape_msg), color))
853 moveit_msgs::CollisionObject collision_obj;
856 collision_obj.id = object_name;
857 collision_obj.operation = moveit_msgs::CollisionObject::ADD;
858 collision_obj.mesh_poses.resize(1);
859 collision_obj.mesh_poses[0] = object_pose;
860 collision_obj.meshes.resize(1);
861 collision_obj.meshes[0] = mesh_msg;
872 moveit_msgs::CollisionObject collision_obj;
875 collision_obj.id = object_name;
876 collision_obj.operation = moveit_msgs::CollisionObject::ADD;
879 typedef std::pair<std::size_t, std::size_t> node_ids;
880 std::set<node_ids> added_edges;
881 std::pair<std::set<node_ids>::iterator,
bool> return_value;
883 Eigen::Vector3d a, b;
884 for (std::size_t i = 0; i < graph.nodes.size(); ++i)
886 for (std::size_t j = 0; j < graph.edges[i].node_ids.size(); ++j)
889 return_value = added_edges.insert(node_ids(i, j));
890 if (!return_value.second)
901 added_edges.insert(node_ids(j, i));
904 double height = (a - b).lpNorm<2>();
910 Eigen::Isometry3d pose;
914 Eigen::Isometry3d rotation;
915 rotation = Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY());
916 pose = pose * rotation;
919 shape_msgs::SolidPrimitive cylinder;
920 cylinder.type = shape_msgs::SolidPrimitive::CYLINDER;
921 cylinder.dimensions.resize(geometric_shapes::solidPrimitiveDimCount<shape_msgs::SolidPrimitive::CYLINDER>());
922 cylinder.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT] = height;
923 cylinder.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] = radius;
926 collision_obj.primitives.push_back(cylinder);
929 collision_obj.primitive_poses.push_back(
convertPose(pose));
938 const std::string& name, moveit_msgs::CollisionObject& collision_obj)
942 collision_obj.operation = moveit_msgs::CollisionObject::ADD;
943 collision_obj.primitives.resize(1);
944 collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
945 collision_obj.primitives[0].dimensions.resize(
946 geometric_shapes::solidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>());
948 geometry_msgs::Pose rec_pose;
952 collision_obj.id =
name;
957 rec_pose.position.x =
x;
958 rec_pose.position.y =
y;
959 rec_pose.position.z = height / 2 +
z;
962 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = depth;
963 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = width;
964 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = height;
967 Eigen::Quaterniond quat(Eigen::AngleAxis<double>(
static_cast<double>(angle), Eigen::Vector3d::UnitZ()));
968 rec_pose.orientation.x = quat.x();
969 rec_pose.orientation.y = quat.y();
970 rec_pose.orientation.z = quat.z();
971 rec_pose.orientation.w = quat.w();
973 collision_obj.primitive_poses.resize(1);
974 collision_obj.primitive_poses[0] = rec_pose;
986 moveit_msgs::CollisionObject collision_obj;
993 double depth,
const std::string& name,
996 geometry_msgs::Pose table_pose;
999 table_pose.position.x =
x;
1000 table_pose.position.y =
y;
1001 table_pose.position.z =
z + height / 2.0;
1004 Eigen::Quaterniond quat(Eigen::AngleAxis<double>(
static_cast<double>(angle), Eigen::Vector3d::UnitZ()));
1005 table_pose.orientation.x = quat.x();
1006 table_pose.orientation.y = quat.y();
1007 table_pose.orientation.z = quat.z();
1008 table_pose.orientation.w = quat.w();
1010 moveit_msgs::CollisionObject collision_obj;
1013 collision_obj.id =
name;
1014 collision_obj.operation = moveit_msgs::CollisionObject::ADD;
1015 collision_obj.primitives.resize(1);
1016 collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
1017 collision_obj.primitives[0].dimensions.resize(
1018 geometric_shapes::solidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>());
1021 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = depth;
1022 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = width;
1023 collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = height;
1025 collision_obj.primitive_poses.resize(1);
1026 collision_obj.primitive_poses[0] = table_pose;
1038 std::ifstream fin(path.c_str());
1046 scene->loadGeometryFromStream(fin, offset);
1067 "Planning_Workspace", 1);
1085 std::vector<std::string> highlight_links;
1086 for (
const auto& contact : c_res.
contacts)
1088 highlight_links.push_back(contact.first.first);
1089 highlight_links.push_back(contact.first.second);
1119 if (!contacts.empty())
1121 visualization_msgs::MarkerArray arr;
1126 if (arr.markers.empty())
1130 for (std::size_t i = 0; i < arr.markers.size(); ++i)
1132 arr.markers[i].ns =
"Collision";
1133 arr.markers[i].id = i;
1134 arr.markers[i].scale.x = 0.04;
1135 arr.markers[i].scale.y = 0.04;
1136 arr.markers[i].scale.z = 0.04;
1137 arr.markers[i].color =
getColor(color);
1146 const std::string& planning_group,
double display_time)
1161 trajectory_msgs::JointTrajectoryPoint trajectory_pt_timed = trajectory_pt;
1162 trajectory_pt_timed.time_from_start =
ros::Duration(display_time);
1165 moveit_msgs::RobotTrajectory trajectory_msg;
1166 trajectory_msg.joint_trajectory.header.frame_id =
base_frame_;
1168 trajectory_msg.joint_trajectory.points.push_back(trajectory_pt);
1169 trajectory_msg.joint_trajectory.points.push_back(trajectory_pt_timed);
1184 double duration_from_previous = 0;
1185 for (std::size_t k = 0; k < trajectory.size(); ++k)
1187 duration_from_previous += speed;
1188 robot_trajectory->addSuffixWayPoint(trajectory[k], duration_from_previous);
1192 moveit_msgs::RobotTrajectory trajectory_msg;
1196 moveit_msgs::RobotState robot_state_msg;
1197 if (!trajectory.empty())
1210 moveit_msgs::RobotTrajectory trajectory_msg;
1214 if (trajectory_msg.joint_trajectory.points.size() > 1)
1216 if (trajectory_msg.joint_trajectory.points[1].time_from_start ==
ros::Duration(0))
1218 for (std::size_t i = 0; i < trajectory_msg.joint_trajectory.points.size(); ++i)
1220 trajectory_msg.joint_trajectory.points[i].time_from_start =
ros::Duration(i * 2);
1226 moveit_msgs::RobotState robot_state_msg;
1227 if (!trajectory.
empty())
1234 const moveit::core::RobotStateConstPtr& robot_state,
bool blocking)
1243 moveit_msgs::RobotState robot_state_msg;
1249 const moveit_msgs::RobotState& robot_state,
bool blocking)
1252 if (trajectory_msg.joint_trajectory.points.empty())
1262 moveit_msgs::DisplayTrajectory display_trajectory_msg;
1263 display_trajectory_msg.model_id =
robot_model_->getName();
1264 display_trajectory_msg.trajectory.resize(1);
1265 display_trajectory_msg.trajectory[0] = trajectory_msg;
1266 display_trajectory_msg.trajectory_start = robot_state;
1273 double duration = trajectory_msg.joint_trajectory.points.back().time_from_start.toSec();
1276 if (
duration < std::numeric_limits<double>::epsilon())
1278 duration = 0.05 * trajectory_msg.joint_trajectory.points.size();
1284 static const double CHECK_TIME_INTERVAL = 0.25;
1287 counter += CHECK_TIME_INTERVAL;
1338 if (!ee_parent_link)
1350 const Eigen::Isometry3d& tip_pose =
robot_trajectory.getWayPoint(i).getGlobalLinkTransform(ee_parent_link);
1353 if (tip_pose.translation().x() != tip_pose.translation().x())
1359 path.push_back(tip_pose.translation());
1363 const double radius = 0.005;
1379 std::vector<const moveit::core::LinkModel*> tips;
1413 std::vector<const moveit::core::LinkModel*> tips;
1435 for (std::size_t i = 0; i < robot_state_trajectory.size(); ++i)
1437 const Eigen::Isometry3d& tip_pose = robot_state_trajectory[i]->getGlobalLinkTransform(ee_parent_link);
1479 const std::vector<std::string>& highlight_links)
1486 const std::vector<std::string>& highlight_links)
1491 if (!highlight_links.empty())
1499 if (display_robot_msg.highlight_links.empty())
1504 const std::vector<std::string>& link_names =
1505 highlight_links.empty() ? robot_state.
getRobotModel()->getLinkModelNamesWithCollisionGeometry() :
1507 display_robot_msg.highlight_links.resize(link_names.size());
1510 const std_msgs::ColorRGBA& color_rgba =
getColor(color);
1513 for (std::size_t i = 0; i < link_names.size(); ++i)
1515 display_robot_msg.highlight_links[i].id = link_names[i];
1516 display_robot_msg.highlight_links[i].color = color_rgba;
1544 if (!highlight_links.empty())
1545 display_robot_msg.highlight_links.clear();
1559 moveit_msgs::DisplayRobotState display_robot_state_msg;
1561 display_robot_state_msg.hide =
true;
1570 const std::vector<const moveit::core::JointModel*>& joints = robot_state->getRobotModel()->getActiveJointModels();
1573 for (std::size_t i = 0; i < joints.size(); ++i)
1576 if (joints[i]->getVariableCount() > 1)
1583 double current_value = robot_state->getVariablePosition(joints[i]->
getName());
1586 bool out_of_bounds = !robot_state->satisfiesBounds(joints[i]);
1593 std::cout <<
" " << std::fixed << std::setprecision(5) <<
bound.min_position_ <<
"\t";
1594 double delta =
bound.max_position_ -
bound.min_position_;
1596 double step = delta / 20.0;
1598 bool marker_shown =
false;
1599 for (
double value =
bound.min_position_; value <
bound.max_position_; value += step)
1602 if (!marker_shown && current_value < value)
1605 marker_shown =
true;
1611 std::cout <<
" \t" << std::fixed << std::setprecision(5) <<
bound.max_position_ <<
" \t" << joints[i]->getName()
1612 <<
" current: " << std::fixed << std::setprecision(5) << current_value << std::endl;
1639 static const std::string VJOINT_NAME =
"virtual_joint";
1642 if (!robot_state.
getRobotModel()->hasJointModel(VJOINT_NAME))
1649 if (!robot_state.
getRobotModel()->getJointModel(VJOINT_NAME)->hasVariable(VJOINT_NAME +
"/trans_x"))
1653 <<
"' do not exist. Try making this vjoint "
1656 const std::vector<std::string>& var_names =
1658 std::copy(var_names.begin(), var_names.end(), std::ostream_iterator<std::string>(std::cout,
"\n"));
1666 const Eigen::Isometry3d& offset)
1668 static const std::string VJOINT_NAME =
"virtual_joint";
1673 ROS_WARN_STREAM_NAMED(
"moveit_visual_tools",
"Unable to apply virtual joint transform, hideRobot() functionality "
1684 Eigen::Quaterniond q(offset.rotation());