moveit 机械臂 读取CSV文件 轨迹跟踪

 

 

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <nav_msgs/Path.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <geometry_msgs/PoseArray.h>
#include <fstream>
#include <string>
#include <cstdlib>
#include <ros/package.h>
#include "tf/tf.h"
#include <iostream>

void getPose(std::string s, double *v)
{
    int p = 0;
    int q = 0;
    for (int i = 0; i < s.size(); i++)
    {
        if (s[i] == ',' || i == s.size() - 1)
        {
            char tab2[16];
            strcpy(tab2, s.substr(p, i - p).c_str());
            v[q] = std::strtod(tab2, NULL);
            p = i + 1;
            q++;
        }
    }
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "moveit_cartesian_demo");
    ros::NodeHandle n;
    ros::Publisher path_pub = n.advertise<geometry_msgs::PoseArray>("trajectory", 10, true);
    ros::AsyncSpinner spinner(1);
    spinner.start();
    moveit::planning_interface::MoveGroupInterface arm("ur5_planning_group");
    //获取终端link的名称
    std::string end_effector_link = arm.getEndEffectorLink();
    //设置目标位置所使用的参考坐标系
    std::string reference_frame = "base_link";
    arm.setPoseReferenceFrame(reference_frame);
    //当运动规划失败后，允许重新规划
    arm.allowReplanning(true);
    //设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.setGoalPositionTolerance(0.001);
    arm.setGoalOrientationTolerance(0.01);
    //设置允许的最大速度和加速度
    arm.setMaxAccelerationScalingFactor(0.2);
    arm.setMaxVelocityScalingFactor(0.2);
    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("home1");
    arm.move();
    sleep(1);
    std::ifstream f(ros::package::getPath("probot_demo") + "/paths/" + "cubeOri.csv");
    //f.open(ros::package::find(plan_pkg)+"/paths/path.csv"); //ros::package::find(plan_pkg)
    if (!f.is_open())
    {
        ROS_ERROR("failed to open file");
        return 0;
    }
    std::string line;
    std::vector<geometry_msgs::Pose> waypoints;
    double scale = 100;
    int count = -1;
    geometry_msgs::PoseArray track;
    track.header.stamp = ros::Time::now();
    track.header.frame_id = "/base_link";
    while (std::getline(f, line))
    {
        std::cout << "Hello world!!!" << std::endl;

        count++;
        double pose[6];
        getPose(line, pose);
        tf::Quaternion q = tf::createQuaternionFromRPY(pose[3], pose[4], pose[5]);
        geometry_msgs::Pose pose1;
        pose1.orientation.x =0;
        pose1.orientation.y = 0;
        pose1.orientation.z = 0;
        pose1.orientation.w = 1;
        pose1.position.x = pose[0] / scale+0.5;
        pose1.position.y = pose[1] / scale+0.5;
        pose1.position.z = pose[2] / scale+0.5;
        std::cout << "path_pose1.position.x" << pose1.position.x <<std::endl;

        ROS_INFO("Adding waypoint x,y,z =  [%.2f, %0.2f, %0.2f]", pose1.position.x, pose1.position.y, pose1.position.z);
        if (count % 2 == 0)
        {
            waypoints.push_back(pose1);
            track.poses.push_back(pose1);
        }
    }
    moveit_msgs::RobotTrajectory trajectory;
    const double jump_threshold = 0.0;
    const double eef_step = 0.01;
    double fraction = 0.0;
    int maxtries = 100; //max fail times
    int attempts = 0;   //attemots times

    arm.setPlanningTime(10.0);
    while (fraction < 1.0 && attempts < maxtries)
    {
        fraction = arm.computeCartesianPath(waypoints,
                                            eef_step,       // eef_step
                                            jump_threshold, // jump_threshold
                                            trajectory);
        attempts++;
        if (attempts % 10 == 0)
            ROS_INFO("Still trying after %d attempts...", attempts);
    }

    robot_trajectory::RobotTrajectory rt(arm.getCurrentState()->getRobotModel(), "ur5_planning_group");

    // Second get a RobotTrajectory from trajectory
    rt.setRobotTrajectoryMsg(*arm.getCurrentState(), trajectory);

    // Thrid create a IterativeParabolicTimeParameterization object
    trajectory_processing::IterativeParabolicTimeParameterization iptp;

    // Fourth compute computeTimeStamps
    bool success = iptp.computeTimeStamps(rt);
    //ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");

    // Get RobotTrajectory_msg from RobotTrajectory
    rt.getRobotTrajectoryMsg(trajectory);
    ROS_INFO("Path computed: (%.2f%% of pablovo)",
             fraction * 100.0);

    moveit::planning_interface::MoveGroupInterface::Plan plan;

    plan.trajectory_ = trajectory;

    arm.execute(plan);

    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("home0");
    arm.move();
    sleep(1);
    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        path_pub.publish(track);
    }
    return 0;
}