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;
}