Go to the documentation of this file.
40 #ifndef MOVEIT_VISUAL_TOOLS_MOVEIT_VISUAL_TOOLS_H
41 #define MOVEIT_VISUAL_TOOLS_MOVEIT_VISUAL_TOOLS_H
52 #include <moveit_msgs/Grasp.h>
53 #include <moveit_msgs/DisplayRobotState.h>
54 #include <moveit_msgs/WorkspaceParameters.h>
55 #include <moveit_msgs/DisplayTrajectory.h>
58 #include <trajectory_msgs/JointTrajectory.h>
71 "/move_group/display_planned_path";
73 "display_robot_state";
95 planning_scene_monitor::PlanningSceneMonitorPtr psm);
105 moveit::core::RobotModelConstPtr robot_model = moveit::core::RobotModelConstPtr());
203 bool blocking =
true);
204 void loadRobotStatePub(
const std::string& robot_state_topic =
"",
bool blocking =
true);
233 const std::vector<double>& ee_joint_pos,
235 const std::string& ns =
"end_effector")
241 const std::string& ns =
"end_effector")
247 const std::string& ns =
"end_effector")
252 const std::vector<double>& ee_joint_pos,
254 const std::string& ns =
"end_effector");
262 bool publishGrasps(
const std::vector<moveit_msgs::Grasp>& possible_grasps,
284 double animate_speed);
293 bool publishIKSolutions(
const std::vector<trajectory_msgs::JointTrajectoryPoint>& ik_solutions,
324 bool attachCO(
const std::string& name,
const std::string& ee_parent_link);
344 bool publishCollisionBlock(
const geometry_msgs::Pose& block_pose,
const std::string& block_name =
"block",
345 double block_size = 0.1,
356 bool publishCollisionCuboid(
const Eigen::Vector3d& point1,
const Eigen::Vector3d& point2,
const std::string& name,
359 const std::string& name,
386 bool publishCollisionCuboid(
const Eigen::Isometry3d& pose,
const Eigen::Vector3d& size,
const std::string& name,
407 const std::string& object_name,
double radius,
434 bool publishCollisionMesh(
const geometry_msgs::Pose& object_pose,
const std::string& object_name,
435 const std::string& mesh_path,
438 const std::string& mesh_path,
441 const shape_msgs::Mesh& mesh_msg,
443 bool publishCollisionMesh(
const geometry_msgs::Pose& object_pose,
const std::string& object_name,
444 const shape_msgs::Mesh& mesh_msg,
454 bool publishCollisionGraph(
const graph_msgs::GeometryGraph& graph,
const std::string& object_name,
double radius,
460 void getCollisionWallMsg(
double x,
double y,
double z,
double angle,
double width,
double height,
461 const std::string& name, moveit_msgs::CollisionObject& collision_obj);
474 bool publishCollisionWall(
double x,
double y,
double angle = 0.0,
double width = 2.0,
double height = 1.5,
475 const std::string& name =
"wall",
477 bool publishCollisionWall(
double x,
double y,
double z,
double angle = 0.0,
double width = 2.0,
double height = 1.5,
478 const std::string& name =
"wall",
493 bool publishCollisionTable(
double x,
double y,
double z,
double angle,
double width,
double height,
double depth,
494 const std::string& name,
562 const std::string& planning_group,
double display_time = 0.1);
574 bool publishTrajectoryPath(
const robot_trajectory::RobotTrajectoryPtr& trajectory,
bool blocking =
false);
577 const moveit::core::RobotStateConstPtr& robot_state,
bool blocking =
false);
581 const moveit_msgs::RobotState& robot_state,
bool blocking =
false);
644 bool publishRobotState(
const trajectory_msgs::JointTrajectoryPoint& trajectory_pt,
668 const std::vector<std::string>& highlight_links = {});
671 const std::vector<std::string>& highlight_links = {});
672 void publishRobotState(
const moveit_msgs::DisplayRobotState& display_robot_state_msg);
704 planning_scene_monitor::PlanningSceneMonitorPtr
psm_;
717 robot_model_loader::RobotModelLoaderPtr
rm_loader_;
720 std::map<const moveit::core::JointModelGroup*, visualization_msgs::MarkerArray>
ee_markers_map_;
721 std::map<const moveit::core::JointModelGroup*, EigenSTL::vector_Isometry3d>
ee_poses_map_;
722 std::map<const moveit::core::JointModelGroup*, std::vector<double> >
ee_joint_pos_map_;
747 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
755 #endif // MOVEIT_VISUAL_TOOLS_MOVEIT_VISUAL_TOOLS_H
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap