/**
* \brief Publishes the joint angles for the visualization
*
* \param device0 the robot and its state
*
* \ingroup ROS
*
*/
void publish_joints(struct robot_device* device0){
static int count=0;
static ros::Time t1;
static ros::Time t2;
static ros::Duration d;
if (count == 0){
t1 = t1.now();
}
count ++;
t2 = t2.now();
d = t2-t1;
if (d.toSec()<0.030)
return;
t1=t2;
publish_marker(device0);
sensor_msgs::JointState joint_state;
//update joint_state
joint_state.header.stamp = ros::Time::now();
joint_state.name.resize(28);
joint_state.position.resize(28);
// joint_state.name.resize(14);
// joint_state.position.resize(14);
int left, right;
if (device0->mech[0].type == GOLD_ARM)
{
left = 0;
right = 1;
}
else
{
left = 1;
right = 0;
}
//======================LEFT ARM===========================
joint_state.name[0] ="shoulder_L";
joint_state.position[0] = device0->mech[left].joint[0].jpos + offsets_l.shoulder_off;
joint_state.name[1] ="elbow_L";
joint_state.position[1] = device0->mech[left].joint[1].jpos + offsets_l.elbow_off;
joint_state.name[2] ="insertion_L";
joint_state.position[2] = device0->mech[left].joint[2].jpos + d4 + offsets_l.insertion_off;
joint_state.name[3] ="tool_roll_L";
joint_state.position[3] = device0->mech[left].joint[4].jpos - 45 * d2r + offsets_l.roll_off;
joint_state.name[4] ="wrist_joint_L";
joint_state.position[4] = device0->mech[left].joint[5].jpos + offsets_l.wrist_off;
joint_state.name[5] ="grasper_joint_1_L";
joint_state.position[5] = device0->mech[left].joint[6].jpos + offsets_l.grasp1_off;
joint_state.name[6] ="grasper_joint_2_L";
joint_state.position[6] = device0->mech[left].joint[7].jpos * -1 + offsets_l.grasp2_off;
//======================RIGHT ARM===========================
joint_state.name[7] ="shoulder_R";
joint_state.position[7] = device0->mech[right].joint[0].jpos + offsets_r.shoulder_off;
joint_state.name[8] ="elbow_R";
joint_state.position[8] = device0->mech[right].joint[1].jpos + offsets_r.elbow_off;
joint_state.name[9] ="insertion_R";
joint_state.position[9] = device0->mech[right].joint[2].jpos + d4 + offsets_r.insertion_off;
joint_state.name[10] ="tool_roll_R";
joint_state.position[10] = device0->mech[right].joint[4].jpos + 45 * d2r + offsets_r.roll_off;
joint_state.name[11] ="wrist_joint_R";
joint_state.position[11] = device0->mech[right].joint[5].jpos * -1 + offsets_r.wrist_off;
joint_state.name[12] ="grasper_joint_1_R";
joint_state.position[12] = device0->mech[right].joint[6].jpos + offsets_r.grasp1_off;
joint_state.name[13] ="grasper_joint_2_R";
joint_state.position[13] = device0->mech[right].joint[7].jpos * -1 + offsets_r.grasp2_off;
//======================LEFT ARM===========================
joint_state.name[14] ="shoulder_L2";
joint_state.position[14] = device0->mech[left].joint[0].jpos_d + offsets_l.shoulder_off;
joint_state.name[15] ="elbow_L2";
joint_state.position[15] = device0->mech[left].joint[1].jpos_d + offsets_l.elbow_off;
joint_state.name[16] ="insertion_L2";
joint_state.position[16] = device0->mech[left].joint[2].jpos_d + d4 + offsets_l.insertion_off;
joint_state.name[17] ="tool_roll_L2";
joint_state.position[17] = device0->mech[left].joint[4].jpos_d - 45 * d2r + offsets_l.roll_off;
joint_state.name[18] ="wrist_joint_L2";
joint_state.position[18] = device0->mech[left].joint[5].jpos_d + offsets_l.wrist_off;
joint_state.name[19] ="grasper_joint_1_L2";
joint_state.position[19] = device0->mech[left].joint[6].jpos_d + offsets_l.grasp1_off;
joint_state.name[20] ="grasper_joint_2_L2";
joint_state.position[20] = device0->mech[left].joint[7].jpos_d * -1 + offsets_l.grasp2_off;
//======================RIGHT ARM===========================
joint_state.name[21] ="shoulder_R2";
joint_state.position[21] = device0->mech[right].joint[0].jpos_d + offsets_r.shoulder_off;
joint_state.name[22] ="elbow_R2";
joint_state.position[22] = device0->mech[right].joint[1].jpos_d + offsets_r.elbow_off;
joint_state.name[23] ="insertion_R2";
joint_state.position[23] = device0->mech[right].joint[2].jpos_d + d4 + offsets_r.insertion_off;
joint_state.name[24] ="tool_roll_R2";
joint_state.position[24] = device0->mech[right].joint[4].jpos_d + 45 * d2r + offsets_r.roll_off;
joint_state.name[25] ="wrist_joint_R2";
joint_state.position[25] = device0->mech[right].joint[5].jpos_d * -1 + offsets_r.wrist_off;
joint_state.name[26] ="grasper_joint_1_R2";
joint_state.position[26] = device0->mech[right].joint[6].jpos_d + offsets_r.grasp1_off;
joint_state.name[27] ="grasper_joint_2_R2";
joint_state.position[27] = device0->mech[right].joint[7].jpos_d * -1 + offsets_r.grasp2_off;
//Publish the joint states
joint_publisher.publish(joint_state);
}
/**
* \brief Publishes the raven_state message from the robot and currParams structures
*
* \param dev robot device structure with the current state of the robot
* \param currParams the parameters being passed from the interfaces
* \ingroup ROS
*/
void publish_ravenstate_ros(struct robot_device *dev,struct param_pass *currParams){
static int count=0;
static raven_state msg_ravenstate; // satic variables to minimize memory allocation calls
static ros::Time t1;
static ros::Time t2;
static ros::Duration d;
msg_ravenstate.last_seq = currParams->last_sequence;
if (count == 0){
t1 = t1.now();
}
count ++;
t2 = t2.now();
d = t2-t1;
// if (d.toSec()<0.01)
// return;
msg_ravenstate.dt=d;
t1=t2;
publish_joints(dev);
// Copy the robot state to the output datastructure.
int numdof=8;
int j;
for (int i=0; i<NUM_MECH; i++){
j = dev->mech[i].type == GREEN_ARM ? 1 : 0;
msg_ravenstate.type[j] = dev->mech[j].type;
msg_ravenstate.pos[j*3] = dev->mech[j].pos.x;
msg_ravenstate.pos[j*3+1] = dev->mech[j].pos.y;
msg_ravenstate.pos[j*3+2] = dev->mech[j].pos.z;
msg_ravenstate.pos_d[j*3] = dev->mech[j].pos_d.x;
msg_ravenstate.pos_d[j*3+1] = dev->mech[j].pos_d.y;
msg_ravenstate.pos_d[j*3+2] = dev->mech[j].pos_d.z;
msg_ravenstate.grasp_d[j] = (float)dev->mech[j].ori_d.grasp/1000;
for (int orii=0; orii<3; orii++)
{
for (int orij=0; orij<3; orij++)
{
msg_ravenstate.ori[j*9 + orii*3+orij] = dev->mech[j].ori.R[orii][orij];
msg_ravenstate.ori_d[j*9 + orii*3+orij] = dev->mech[j].ori_d.R[orii][orij];
}
}
for (int m=0; m<numdof; m++){
int jtype = dev->mech[j].joint[m].type;
msg_ravenstate.encVals[jtype] = dev->mech[j].joint[m].enc_val;
msg_ravenstate.tau[jtype] = dev->mech[j].joint[m].tau_d;
msg_ravenstate.mpos[jtype] = dev->mech[j].joint[m].mpos RAD2DEG;
msg_ravenstate.jpos[jtype] = dev->mech[j].joint[m].jpos RAD2DEG;
msg_ravenstate.mvel[jtype] = dev->mech[j].joint[m].mvel RAD2DEG;
msg_ravenstate.jvel[jtype] = dev->mech[j].joint[m].jvel RAD2DEG;
msg_ravenstate.jpos_d[jtype] = dev->mech[j].joint[m].jpos_d RAD2DEG;
msg_ravenstate.mpos_d[jtype] = dev->mech[j].joint[m].mpos_d RAD2DEG;
msg_ravenstate.encoffsets[jtype] = dev->mech[j].joint[m].enc_offset;
msg_ravenstate.dac_val[jtype] = dev->mech[j].joint[m].current_cmd;
}
//grab jacobian velocities and forces
float vel[6];
float f[6];
j = dev->mech[i].type == GREEN_ARM ? 1 : 0;
dev->mech[j].r2_jac.get_vel(vel);
dev->mech[j].r2_jac.get_vel(f);
for (int k=0; k<6; k++){
msg_ravenstate.jac_vel[j*6+k] = vel[k];
msg_ravenstate.jac_f[j*6+k] = f[k];
}
}
// msg_ravenstate.f_secs = d.toSec();
msg_ravenstate.hdr.stamp = msg_ravenstate.hdr.stamp.now();
msg_ravenstate.runlevel=currParams->runlevel;
msg_ravenstate.sublevel=currParams->sublevel;
// Publish the raven data to ROS
pub_ravenstate.publish(msg_ravenstate);
}
