UR_modern_driver ROS与UR控制器的实时接口

    RosWrapper(std::string host, int reverse_port) :
            as_(nh_, "follow_joint_trajectory",
                    boost::bind(&RosWrapper::goalCB, this, _1),
                    boost::bind(&RosWrapper::cancelCB, this, _1), false), robot_(
                    rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300), io_flag_delay_(0.05), joint_offsets_(
                    6, 0.0) {   ...

}

//boost::bind(&AuboDriver::goalCB, this,_1)：boost这个是固定用法，就不深究了，它绑定了一个名为AuboDriver::goalCB的函数，AuboDriver是as_所属的类的名称，goalCB（）是其中的一个成员函数，这个函数写在了as_构造函数的第三个参数位，表示它是一个接受action目标的回调函数（函数名可以自己更改），当action客户端请求一个动作时，这个动作被认为是一个目标，传递给action服务端，此时就moveit的action客户端而言，这个目标就是机械臂运动轨迹，服务端接收到目标后，会自动回调goalCB这个函数，而目标（轨迹）会作为参数传递进来。

 void goalCB(
            actionlib::ServerGoalHandle<
                    control_msgs::FollowJointTrajectoryAction> gh) {
        std::string buf;
        print_info("on_goal");
        if (!robot_.sec_interface_->robot_state_->isReady()) {
            result_.error_code = -100; //nothing is defined for this...?

            if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) {
                result_.error_string =
                        "Cannot accept new trajectories: Robot arm is not powered on";
                gh.setRejected(result_, result_.error_string);
                print_error(result_.error_string);
                return;
            }
            if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) {
                result_.error_string =
                        "Cannot accept new trajectories: Robot is not enabled";
                gh.setRejected(result_, result_.error_string);
                print_error(result_.error_string);
                return;
            }
            result_.error_string =
                    "Cannot accept new trajectories. (Debug: Robot mode is "
                            + std::to_string(
                                    robot_.sec_interface_->robot_state_->getRobotMode())
                            + ")";
            gh.setRejected(result_, result_.error_string);
            print_error(result_.error_string);
            return;
        }
        if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) {
            result_.error_code = -100; //nothing is defined for this...?
            result_.error_string =
                    "Cannot accept new trajectories: Robot is emergency stopped";
            gh.setRejected(result_, result_.error_string);
            print_error(result_.error_string);
            return;
        }
        if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) {
            result_.error_code = -100; //nothing is defined for this...?
            result_.error_string =
                    "Cannot accept new trajectories: Robot is protective stopped";
            gh.setRejected(result_, result_.error_string);
            print_error(result_.error_string);
            return;
        }

        actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
                *gh.getGoal(); //make a copy that we can modify
        if (has_goal_) {
            print_warning(
                    "Received new goal while still executing previous trajectory. Canceling previous trajectory");
            has_goal_ = false;
            robot_.stopTraj();
            result_.error_code = -100; //nothing is defined for this...?
            result_.error_string = "Received another trajectory";
            goal_handle_.setAborted(result_, result_.error_string);
            std::this_thread::sleep_for(std::chrono::milliseconds(250));
        }
        goal_handle_ = gh;
        if (!validateJointNames()) {
            std::string outp_joint_names = "";
            for (unsigned int i = 0; i < goal.trajectory.joint_names.size();
                    i++) {
                outp_joint_names += goal.trajectory.joint_names[i] + " ";
            }
            result_.error_code = result_.INVALID_JOINTS;
            result_.error_string =
                    "Received a goal with incorrect joint names: "
                            + outp_joint_names;
            gh.setRejected(result_, result_.error_string);
            print_error(result_.error_string);
            return;
        }
        if (!has_positions()) {
            result_.error_code = result_.INVALID_GOAL;
            result_.error_string = "Received a goal without positions";
            gh.setRejected(result_, result_.error_string);
            print_error(result_.error_string);
            return;
        }
        
        if (!has_velocities()) {
            result_.error_code = result_.INVALID_GOAL;
            result_.error_string = "Received a goal without velocities";
            gh.setRejected(result_, result_.error_string);
            print_error(result_.error_string);
            return;
        }

        if (!traj_is_finite()) {
            result_.error_string = "Received a goal with infinities or NaNs";
            result_.error_code = result_.INVALID_GOAL;
            gh.setRejected(result_, result_.error_string);
            print_error(result_.error_string);
            return;
        }

        if (!has_limited_velocities()) {
            result_.error_code = result_.INVALID_GOAL;
            result_.error_string =
                    "Received a goal with velocities that are higher than "
                            + std::to_string(max_velocity_);
            gh.setRejected(result_, result_.error_string);
            print_error(result_.error_string);
            return;
        }

        reorder_traj_joints(goal.trajectory);
        
        if (!start_positions_match(goal.trajectory, 0.01)) {
            result_.error_code = result_.INVALID_GOAL;
            result_.error_string = "Goal start doesn't match current pose";
            gh.setRejected(result_, result_.error_string);
            print_error(result_.error_string);
            return;
        }

        std::vector<double> timestamps;
        std::vector<std::vector<double> > positions, velocities;
        if (goal.trajectory.points[0].time_from_start.toSec() != 0.) {
            print_warning(
                    "Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory");
            timestamps.push_back(0.0);
            positions.push_back(
                    robot_.rt_interface_->robot_state_->getQActual());
            velocities.push_back(
                    robot_.rt_interface_->robot_state_->getQdActual());
        }
        for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
            timestamps.push_back(
                    goal.trajectory.points[i].time_from_start.toSec());
            positions.push_back(goal.trajectory.points[i].positions);
            velocities.push_back(goal.trajectory.points[i].velocities);

        }

        goal_handle_.setAccepted();
        has_goal_ = true;
        std::thread(&RosWrapper::trajThread, this, timestamps, positions,
                velocities).detach();
    }

 

private:
    void trajThread(std::vector<double> timestamps,
            std::vector<std::vector<double> > positions,
            std::vector<std::vector<double> > velocities) {

        robot_.doTraj(timestamps, positions, velocities);
        if (has_goal_) {
            result_.error_code = result_.SUCCESSFUL;
            goal_handle_.setSucceeded(result_);
            has_goal_ = false;
        }
    }

 

bool UrDriver::doTraj(std::vector<double> inp_timestamps,
        std::vector<std::vector<double> > inp_positions,
        std::vector<std::vector<double> > inp_velocities) {
    std::chrono::high_resolution_clock::time_point t0, t;
    std::vector<double> positions;
    unsigned int j;

    if (!UrDriver::uploadProg()) {
        return false;
    }//初始化下发python脚本
    executing_traj_ = true;
    t0 = std::chrono::high_resolution_clock::now();
    t = t0;
    j = 0;
    while ((inp_timestamps[inp_timestamps.size() - 1]
            >= std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count())
            and executing_traj_) {
        while (inp_timestamps[j]
                <= std::chrono::duration_cast<std::chrono::duration<double>>(
                        t - t0).count() && j < inp_timestamps.size() - 1) {
            j += 1;
        }
        positions = UrDriver::interp_cubic(
                std::chrono::duration_cast<std::chrono::duration<double>>(
                        t - t0).count() - inp_timestamps[j - 1],
                inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1],
                inp_positions[j], inp_velocities[j - 1], inp_velocities[j]);
        UrDriver::servoj(positions);

        // oversample with 4 * sample_time
        std::this_thread::sleep_for(
                std::chrono::milliseconds((int) ((servoj_time_ * 1000) / 4.))); //休眠servoj_time_/4毫秒
        t = std::chrono::high_resolution_clock::now();
    }
    executing_traj_ = false;
    //Signal robot to stop driverProg()
    UrDriver::closeServo(positions);
    return true;
}

 

bool UrDriver::uploadProg() {
    std::string cmd_str;
    char buf[128];
    cmd_str = "def driverProg():\n";

    sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_);
    cmd_str += buf;

    cmd_str += "\tSERVO_IDLE = 0\n";
    cmd_str += "\tSERVO_RUNNING = 1\n";
    cmd_str += "\tcmd_servo_state = SERVO_IDLE\n";
    cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n";
    cmd_str += "\tdef set_servo_setpoint(q):\n";
    cmd_str += "\t\tenter_critical\n";
    cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n";
    cmd_str += "\t\tcmd_servo_q = q\n";
    cmd_str += "\t\texit_critical\n";
    cmd_str += "\tend\n";
    cmd_str += "\tthread servoThread():\n";
    cmd_str += "\t\tstate = SERVO_IDLE\n";
    cmd_str += "\t\twhile True:\n";
    cmd_str += "\t\t\tenter_critical\n";
    cmd_str += "\t\t\tq = cmd_servo_q\n";
    cmd_str += "\t\t\tdo_brake = False\n";
    cmd_str += "\t\t\tif (state == SERVO_RUNNING) and ";
    cmd_str += "(cmd_servo_state == SERVO_IDLE):\n";
    cmd_str += "\t\t\t\tdo_brake = True\n";
    cmd_str += "\t\t\tend\n";
    cmd_str += "\t\t\tstate = cmd_servo_state\n";
    cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n";
    cmd_str += "\t\t\texit_critical\n";
    cmd_str += "\t\t\tif do_brake:\n";
    cmd_str += "\t\t\t\tstopj(1.0)\n";
    cmd_str += "\t\t\t\tsync()\n";
    cmd_str += "\t\t\telif state == SERVO_RUNNING:\n";

    if (sec_interface_->robot_state_->getVersion() >= 3.1)
        sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n",
                servoj_time_, servoj_lookahead_time_, servoj_gain_);
    else
        sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_);
    cmd_str += buf;

    cmd_str += "\t\t\telse:\n";
    cmd_str += "\t\t\t\tsync()\n";
    cmd_str += "\t\t\tend\n";
    cmd_str += "\t\tend\n";
    cmd_str += "\tend\n";

    sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(),
            REVERSE_PORT_);
    cmd_str += buf;

    cmd_str += "\tthread_servo = run servoThread()\n";
    cmd_str += "\tkeepalive = 1\n";
    cmd_str += "\twhile keepalive > 0:\n";
    cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n";
    cmd_str += "\t\tif params_mult[0] > 0:\n";
    cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, ";
    cmd_str += "params_mult[2] / MULT_jointstate, ";
    cmd_str += "params_mult[3] / MULT_jointstate, ";
    cmd_str += "params_mult[4] / MULT_jointstate, ";
    cmd_str += "params_mult[5] / MULT_jointstate, ";
    cmd_str += "params_mult[6] / MULT_jointstate]\n";
    cmd_str += "\t\t\tkeepalive = params_mult[7]\n";
    cmd_str += "\t\t\tset_servo_setpoint(q)\n";
    cmd_str += "\t\tend\n";
    cmd_str += "\tend\n";
    cmd_str += "\tsleep(.1)\n";
    cmd_str += "\tsocket_close()\n";
    cmd_str += "\tkill thread_servo\n";
    cmd_str += "end\n";

    rt_interface_->addCommandToQueue(cmd_str);
    return UrDriver::openServo();
}

 

//进行cubic点与点之间的插值

void UrDriver::servoj(std::vector<double> positions, int keepalive) {
    if (!reverse_connected_) {
        print_error(
                "UrDriver::servoj called without a reverse connection present. Keepalive: "
                        + std::to_string(keepalive));
        return;
    }
    unsigned int bytes_written;
    int tmp;
    unsigned char buf[28];
    for (int i = 0; i < 6; i++) {
        tmp = htonl((int) (positions[i] * MULT_JOINTSTATE_));
        buf[i * 4] = tmp & 0xff;
        buf[i * 4 + 1] = (tmp >> 8) & 0xff;
        buf[i * 4 + 2] = (tmp >> 16) & 0xff;
        buf[i * 4 + 3] = (tmp >> 24) & 0xff;
    }
    tmp = htonl((int) keepalive);
    buf[6 * 4] = tmp & 0xff;
    buf[6 * 4 + 1] = (tmp >> 8) & 0xff;
    buf[6 * 4 + 2] = (tmp >> 16) & 0xff;
    buf[6 * 4 + 3] = (tmp >> 24) & 0xff;
    bytes_written = write(new_sockfd_, buf, 28);//写入socket
}

 

std::vector<double> UrDriver::interp_cubic(double t, double T,
        std::vector<double> p0_pos, std::vector<double> p1_pos,
        std::vector<double> p0_vel, std::vector<double> p1_vel) {
    /*Returns positions of the joints at time 't' */
    std::vector<double> positions;
    for (unsigned int i = 0; i < p0_pos.size(); i++) {
        double a = p0_pos[i];
        double b = p0_vel[i];
        double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i]
                - T * p1_vel[i]) / pow(T, 2);
        double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i]
                + T * p1_vel[i]) / pow(T, 3);
        positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3));
    }
    return positions;
}