.. _program_listing_file__tmp_ws_src_moveit_visual_tools_include_moveit_visual_tools_moveit_visual_tools.h: Program Listing for File moveit_visual_tools.h ============================================== |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/moveit_visual_tools/include/moveit_visual_tools/moveit_visual_tools.h``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp // Copyright 2015 University of Colorado, Boulder // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the names of its // contributors may be used to endorse or promote products derived from // this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. /* \author Dave Coleman * \desc Helper functions for displaying and debugging MoveIt data in Rviz via published markers * and MoveIt collision objects. Very useful for debugging complex software */ #ifndef MOVEIT_VISUAL_TOOLS_MOVEIT_VISUAL_TOOLS_H #define MOVEIT_VISUAL_TOOLS_MOVEIT_VISUAL_TOOLS_H // Rviz Visualization Tool #include // MoveIt #include // MoveIt Messages #include #include #include #include #include // ROS Messages #include #include // C++ #include #include #include #include namespace moveit_visual_tools { // Default constants static const std::string ROBOT_DESCRIPTION = "robot_description"; // this is the default used in ROS static const std::string DISPLAY_PLANNED_PATH_TOPIC = "/move_group/display_planned_path"; // this is the default when adding the Rviz plugin static const std::string DISPLAY_ROBOT_STATE_TOPIC = "display_robot_state"; // this is the default when adding the Rviz plugin static const std::string PLANNING_SCENE_TOPIC = "planning_scene"; // this is the default when adding the Rviz plugin class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools { public: MoveItVisualTools(const rclcpp::Node::SharedPtr& node); MoveItVisualTools(const rclcpp::Node::SharedPtr& node, const std::string& base_frame, const std::string& marker_topic, planning_scene_monitor::PlanningSceneMonitorPtr psm); MoveItVisualTools(const rclcpp::Node::SharedPtr& node, const std::string& base_frame, const std::string& marker_topic = rviz_visual_tools::RVIZ_MARKER_TOPIC, moveit::core::RobotModelConstPtr robot_model = moveit::core::RobotModelConstPtr()); void setRobotStateTopic(const std::string& robot_state_topic) { robot_state_topic_ = robot_state_topic; } void setPlanningSceneTopic(const std::string& planning_scene_topic) { planning_scene_topic_ = planning_scene_topic; } bool loadPlanningSceneMonitor(); bool processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject& msg, const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); bool processAttachedCollisionObjectMsg(const moveit_msgs::msg::AttachedCollisionObject& msg); bool moveCollisionObject(const Eigen::Isometry3d& pose, const std::string& name, const rviz_visual_tools::Colors& color); bool moveCollisionObject(const geometry_msgs::msg::Pose& pose, const std::string& name, const rviz_visual_tools::Colors& color); bool triggerPlanningSceneUpdate(); bool loadSharedRobotState(); moveit::core::RobotStatePtr& getSharedRobotState(); moveit::core::RobotStatePtr& getRootRobotState() { return root_robot_state_; } moveit::core::RobotModelConstPtr getRobotModel(); bool loadEEMarker(const moveit::core::JointModelGroup* ee_jmg, const std::vector& ee_joint_pos = {}); void loadTrajectoryPub(const std::string& display_planned_path_topic = DISPLAY_PLANNED_PATH_TOPIC, bool blocking = true); void loadRobotStatePub(const std::string& robot_state_topic = "", bool blocking = true); void setPlanningSceneMonitor(planning_scene_monitor::PlanningSceneMonitorPtr psm) { psm_ = std::move(psm); } void setManualSceneUpdating(bool enable_manual = true) { manual_trigger_update_ = enable_manual; } bool publishEEMarkers(const Eigen::Isometry3d& pose, const moveit::core::JointModelGroup* ee_jmg, const std::vector& ee_joint_pos, const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT, const std::string& ns = "end_effector") { return publishEEMarkers(convertPose(pose), ee_jmg, ee_joint_pos, color, ns); } bool publishEEMarkers(const Eigen::Isometry3d& pose, const moveit::core::JointModelGroup* ee_jmg, const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT, const std::string& ns = "end_effector") { return publishEEMarkers(convertPose(pose), ee_jmg, {}, color, ns); } bool publishEEMarkers(const geometry_msgs::msg::Pose& pose, const moveit::core::JointModelGroup* ee_jmg, const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT, const std::string& ns = "end_effector") { return publishEEMarkers(pose, ee_jmg, {}, color, ns); } bool publishEEMarkers(const geometry_msgs::msg::Pose& pose, const moveit::core::JointModelGroup* ee_jmg, const std::vector& ee_joint_pos, const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT, const std::string& ns = "end_effector"); bool publishGrasps(const std::vector& possible_grasps, const moveit::core::JointModelGroup* ee_jmg, double animate_speed = 0.1 /* seconds */); bool publishAnimatedGrasps(const std::vector& possible_grasps, const moveit::core::JointModelGroup* ee_jmg, double animate_speed = 0.01 /* seconds */); bool publishAnimatedGrasp(const moveit_msgs::msg::Grasp& grasp, const moveit::core::JointModelGroup* ee_jmg, double animate_speed /* seconds */); bool publishIKSolutions(const std::vector& ik_solutions, const moveit::core::JointModelGroup* arm_jmg, double display_time = 0.4); bool removeAllCollisionObjects(); bool cleanupCO(const std::string& name); bool cleanupACO(const std::string& name); bool attachCO(const std::string& name, const std::string& ee_parent_link); bool publishCollisionFloor(double z = 0.0, const std::string& plane_name = "Floor", const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); bool publishCollisionBlock(const geometry_msgs::msg::Pose& block_pose, const std::string& block_name = "block", double block_size = 0.1, const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); bool publishCollisionCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std::string& name, const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); bool publishCollisionCuboid(const geometry_msgs::msg::Point& point1, const geometry_msgs::msg::Point& point2, const std::string& name, const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); bool publishCollisionCuboid(const Eigen::Isometry3d& pose, double width, double depth, double height, const std::string& name, const rviz_visual_tools::Colors& color); bool publishCollisionCuboid(const geometry_msgs::msg::Pose& pose, double width, double depth, double height, const std::string& name, const rviz_visual_tools::Colors& color); bool publishCollisionCuboid(const Eigen::Isometry3d& pose, const Eigen::Vector3d& size, const std::string& name, const rviz_visual_tools::Colors& color) { return publishCollisionCuboid(pose, size.x(), size.y(), size.z(), name, color); } bool publishCollisionCuboid(const geometry_msgs::msg::Pose& pose, const geometry_msgs::msg::Vector3& size, const std::string& name, const rviz_visual_tools::Colors& color) { return publishCollisionCuboid(pose, size.x, size.y, size.z, name, color); } bool publishCollisionCylinder(const geometry_msgs::msg::Point& a, const geometry_msgs::msg::Point& b, const std::string& object_name, double radius, const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); bool publishCollisionCylinder(const Eigen::Vector3d& a, const Eigen::Vector3d& b, const std::string& object_name, double radius, const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); bool publishCollisionCylinder(const Eigen::Isometry3d& object_pose, const std::string& object_name, double radius, double height, const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); bool publishCollisionCylinder(const geometry_msgs::msg::Pose& object_pose, const std::string& object_name, double radius, double height, const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); bool publishCollisionMesh(const geometry_msgs::msg::Pose& object_pose, const std::string& object_name, const std::string& mesh_path, const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); bool publishCollisionMesh(const Eigen::Isometry3d& object_pose, const std::string& object_name, const std::string& mesh_path, const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); bool publishCollisionMesh(const Eigen::Isometry3d& object_pose, const std::string& object_name, const shape_msgs::msg::Mesh& mesh_msg, const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); bool publishCollisionMesh(const geometry_msgs::msg::Pose& object_pose, const std::string& object_name, const shape_msgs::msg::Mesh& mesh_msg, const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); bool publishCollisionGraph(const graph_msgs::msg::GeometryGraph& graph, const std::string& object_name, double radius, const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); void getCollisionWallMsg(double x, double y, double z, double angle, double width, double height, const std::string& name, moveit_msgs::msg::CollisionObject& collision_obj); 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); bool publishCollisionWall(double x, double y, double z, double angle = 0.0, double width = 2.0, double height = 1.5, const std::string& name = "wall", const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); bool publishCollisionTable(double x, double y, double z, double angle, double width, double height, double depth, const std::string& name, const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); bool loadCollisionSceneFromFile(const std::string& path); bool loadCollisionSceneFromFile(const std::string& path, const Eigen::Isometry3d& offset); bool publishWorkspaceParameters(const moveit_msgs::msg::WorkspaceParameters& params); bool checkAndPublishCollision(const moveit::core::RobotState& robot_state, const planning_scene::PlanningScene* planning_scene, const rviz_visual_tools::Colors& highlight_link_color = rviz_visual_tools::RED, const rviz_visual_tools::Colors& contact_point_color = rviz_visual_tools::PURPLE); bool publishContactPoints(const moveit::core::RobotState& robot_state, const planning_scene::PlanningScene* planning_scene, const rviz_visual_tools::Colors& color = rviz_visual_tools::RED); bool publishContactPoints(const collision_detection::CollisionResult::ContactMap& contacts, const planning_scene::PlanningScene* planning_scene, const rviz_visual_tools::Colors& color = rviz_visual_tools::RED); bool publishTrajectoryPoint(const trajectory_msgs::msg::JointTrajectoryPoint& trajectory_pt, const std::string& planning_group, double display_time = 0.1); bool publishTrajectoryPath(const std::vector& trajectory, const moveit::core::JointModelGroup* jmg, double speed = 0.01, bool blocking = false); bool publishTrajectoryPath(const robot_trajectory::RobotTrajectoryPtr& trajectory, bool blocking = false); bool publishTrajectoryPath(const robot_trajectory::RobotTrajectory& trajectory, bool blocking = false); bool publishTrajectoryPath(const moveit_msgs::msg::RobotTrajectory& trajectory_msg, const moveit::core::RobotStateConstPtr& robot_state, bool blocking = false); bool publishTrajectoryPath(const moveit_msgs::msg::RobotTrajectory& trajectory_msg, const moveit::core::RobotState& robot_state, bool blocking = false); bool publishTrajectoryPath(const moveit_msgs::msg::RobotTrajectory& trajectory_msg, const moveit_msgs::msg::RobotState& robot_state, bool blocking = false); void publishTrajectoryPath(const moveit_msgs::msg::DisplayTrajectory& display_trajectory_msg); bool publishTrajectoryLine(const moveit_msgs::msg::RobotTrajectory& trajectory_msg, const moveit::core::LinkModel* ee_parent_link, const moveit::core::JointModelGroup* arm_jmg, const rviz_visual_tools::Colors& color = rviz_visual_tools::LIME_GREEN); bool publishTrajectoryLine(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, const moveit::core::LinkModel* ee_parent_link, const rviz_visual_tools::Colors& color = rviz_visual_tools::LIME_GREEN); bool publishTrajectoryLine(const robot_trajectory::RobotTrajectory& robot_trajectory, const moveit::core::LinkModel* ee_parent_link, const rviz_visual_tools::Colors& color = rviz_visual_tools::LIME_GREEN); bool publishTrajectoryLine(const moveit_msgs::msg::RobotTrajectory& trajectory_msg, const moveit::core::JointModelGroup* arm_jmg, const rviz_visual_tools::Colors& color = rviz_visual_tools::LIME_GREEN); bool publishTrajectoryLine(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, const moveit::core::JointModelGroup* arm_jmg, const rviz_visual_tools::Colors& color = rviz_visual_tools::LIME_GREEN); bool publishTrajectoryLine(const robot_trajectory::RobotTrajectory& robot_trajectory, const moveit::core::JointModelGroup* arm_jmg, const rviz_visual_tools::Colors& color = rviz_visual_tools::LIME_GREEN); bool publishTrajectoryPoints(const std::vector& robot_state_trajectory, const moveit::core::LinkModel* ee_parent_link, const rviz_visual_tools::Colors& color = rviz_visual_tools::YELLOW); void enableRobotStateRootOffet(const Eigen::Isometry3d& offset); void disableRobotStateRootOffet(); bool publishRobotState(const trajectory_msgs::msg::JointTrajectoryPoint& trajectory_pt, const moveit::core::JointModelGroup* jmg, const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT); bool publishRobotState(const std::vector& joint_positions, const moveit::core::JointModelGroup* jmg, const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT); bool publishRobotState(const moveit::core::RobotState& robot_state, const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT, const std::vector& highlight_links = {}); bool publishRobotState(const moveit::core::RobotStatePtr& robot_state, const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT, const std::vector& highlight_links = {}); void publishRobotState(const moveit_msgs::msg::DisplayRobotState& display_robot_state_msg); bool hideRobot(); static bool applyVirtualJointTransform(moveit::core::RobotState& robot_state, const Eigen::Isometry3d& offset); void showJointLimits(const moveit::core::RobotStatePtr& robot_state); planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitor(); private: static bool checkForVirtualJoint(const moveit::core::RobotState& robot_state); protected: // Pointer to a Planning Scene Monitor planning_scene_monitor::PlanningSceneMonitorPtr psm_; // Prevent the planning scene from always auto-pushing, but rather do it manually bool manual_trigger_update_ = false; // Pointer to the robot model moveit::core::RobotModelConstPtr robot_model_; // ROS topic names to use when starting publishers std::string robot_state_topic_; std::string planning_scene_topic_; // ROS publishers rclcpp::Publisher::SharedPtr pub_display_path_; // for MoveIt trajectories rclcpp::Publisher::SharedPtr pub_robot_state_; // publish a RobotState message // ROS Node rclcpp::Node::SharedPtr node_; robot_model_loader::RobotModelLoaderPtr rm_loader_; // so that we can specify our own options // End Effector Markers std::map ee_markers_map_; std::map ee_poses_map_; std::map > ee_joint_pos_map_; // Cached robot state marker - cache the colored links. // Note: Only allows colors provided in rviz_visual_tools to prevent too many robot state messages from being loaded // and ensuring efficiency std::map display_robot_msgs_; // Note: call loadSharedRobotState() before using this moveit::core::RobotStatePtr shared_robot_state_; // Note: call loadSharedRobotState() before using this. Use only for hiding the robot moveit::core::RobotStatePtr hidden_robot_state_; // A robot state whose virtual_joint remains at identity so that getGlobalLinkTransform() isn't tainted // Note: call loadSharedRobotState() before using this moveit::core::RobotStatePtr root_robot_state_; // Optional offset that can be applied to all outgoing/published robot states bool robot_state_root_offset_enabled_ = false; Eigen::Isometry3d robot_state_root_offset_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW // http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html }; // class typedef std::shared_ptr MoveItVisualToolsPtr; typedef std::shared_ptr MoveItVisualToolsConstPtr; } // namespace moveit_visual_tools #endif // MOVEIT_VISUAL_TOOLS_MOVEIT_VISUAL_TOOLS_H