ros_tools.prompt 断点调试

╰半橙微兮° 2023-02-27 10:31 58阅读 0赞

# Rviz虚拟调试工具 #

#include "moveit_visual_tools/moveit_visual_tools.h"
    ...
    //绘制移动轨迹
    string frame = group.getPlanningFrame();
    moveit_visual_tools::MoveItVisualTools tools(frame);
    tools.deleteAllMarkers();
    //添加label
    const moveit::core::JointModelGroup *jointModelGroup = group.getCurrentState()->getJointModelGroup(groupName);
    tools.publishTrajectoryLine(plan.trajectory_, jointModelGroup);
    tools.trigger();

可视化工具，目前只支持c++

## Gui断点调试 ##

// group库
    #include "moveit/move_group_interface/move_group_interface.h"
    // 可视化工具库
    #include "moveit_visual_tools/moveit_visual_tools.h"
    ......
    
    string groupName="manipulator";
    moveit::planning_interface::PlanningSceneInterface scene;
    moveit_visual_tools::MoveItVisualTools tools(group.getPlanningFrame());
    
    tools.loadRemoteControl();
    
    ROS_INFO_STREAM("1");
    tools.prompt("Press 'next' to start demo");
    
    ROS_INFO_STREAM("2");
    tools.prompt("Press 'next' to start demo");
    
    ROS_INFO_STREAM("3");
    tools.prompt("Press 'next' to start demo");
    
    ROS_INFO_STREAM("4");
    tools.prompt("Press 'next' to start demo");

### 开启调试面板 ###

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 1][]

完整代码示例:

#include <iostream>
    #include <ros/ros.h>
    
    //moveit环境引入
    #include <moveit/move_group_interface/move_group_interface.h>
    #include <geometry_msgs/Pose.h>
    
    #include <tf/LinearMath/Quaternion.h>
    
    #include "moveit_visual_tools/moveit_visual_tools.h"
    
    using namespace std;
    
    
    double deg2rad(double deg) {
        return deg * M_PI / 180.0;
    }
    
    
    void moveHome() {
        moveit::planning_interface::MoveGroupInterface group("manipulator_i5");
        group.setNamedTarget("home");
        group.move();
    }
    
    void moveZero() {
        moveit::planning_interface::MoveGroupInterface group("manipulator_i5");
        group.setNamedTarget("zero");
        group.move();
    }
    
    void movePose() {
        moveit::planning_interface::MoveGroupInterface group("manipulator_i5");
        //设置目标
    //    group.setNamedTarget("home");
    
        //去到具体的位置
        geometry_msgs::Pose pose;
        // 位置
        pose.position.x = 0.364878;
        pose.position.y = -0.279625;
        pose.position.z = 0.388561 + 0.502;
        // 姿态(四元素)(欧拉角转四元素)
        tf::Quaternion quat;
        quat.setRPY(deg2rad(-166.969955), deg2rad(2.913350), deg2rad(38.519089));
        pose.orientation.x = quat.x();
        pose.orientation.y = quat.y();
        pose.orientation.z = quat.z();
        pose.orientation.w = quat.w();
    
        group.setPoseTarget(pose);
    
        //移动
        const moveit::planning_interface::MoveItErrorCode &code = group.move();
    
        if (code == moveit::planning_interface::MoveItErrorCode::SUCCESS) {
            // 成功
            ROS_INFO_STREAM("执行成功: Success");
        } else {
            // 失败
            ROS_INFO_STREAM("执行失败: Failed");
        }
    }
    
    
    int main(int argc, char **argv) {
        // 初始化节点
        string nodeName = "ur3_move_node";
        ros::init(argc, argv, nodeName);
    
        ros::NodeHandle node;
    
        ros::AsyncSpinner spinner(1);
        spinner.start();
    
        moveit::planning_interface::MoveGroupInterface group("manipulator_i5");
        moveit_visual_tools::MoveItVisualTools tools(group.getPlanningFrame());
    
        tools.prompt("start move home");
        moveHome();
    
        tools.prompt("start move zero");
        moveZero();
    
        tools.prompt("start move pose");
        movePose();
    
        //阻塞
        ros::waitForShutdown();
    
        return 0;
    }

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70]: /images/20230209/1a6135115266442eba996a1bf9bb8e67.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 1]: https://img-blog.csdnimg.cn/2020071621115378.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5,size_16,color_FFFFFF,t_70