/** \brief Reads gripper responses in auto_update mode. The gripper pushes state messages in regular intervals. */
void read_thread(int interval_ms)
{
ROS_INFO("Thread started");
status_t status;
int res;
bool pub_state = false;
// Prepare messages
wsg_50_common::Status status_msg;
status_msg.status = "";
sensor_msgs::JointState joint_states;
// joint_states.header.frame_id = "wsg_50/palm_link";
joint_states.name.push_back("wsg_50/palm_joint_gripper_left");
joint_states.name.push_back("wsg_50/palm_joint_gripper_right");
joint_states.position.resize(2);
joint_states.velocity.resize(2);
joint_states.effort.resize(2);
// Request automatic updates (error checking is done below)
getOpening(interval_ms);
getSpeed(interval_ms);
getForce(interval_ms);
msg_t msg; msg.id = 0;
msg.data = 0;
msg.len = 0;
int cnt[3] = {0,0,0};
auto time_start = std::chrono::system_clock::now();
// Prepare FMF data reading
std::vector<float> finger_data; finger_data.push_back(0.0);
finger_data.push_back(0.0);
int index = 0;
bool waiting_fmf = false;
unsigned char vResult[4];
while (g_thread)
{
// Receive gripper response
msg_free(&msg);
res = msg_receive( &msg );
if (res < 0 ) //|| msg.len < 2)
{
ROS_ERROR("Gripper response failure");
continue;
}
float val = 0.0;
status = cmd_get_response_status(msg.data);
// Decode float for opening/speed/force
if (msg.id >= 0x43 && msg.id <= 0x45 && msg.len == 6)
{
if (status != E_SUCCESS)
{
ROS_ERROR("Gripper response failure for opening/speed/force\n");
continue;
}
val = convert(&msg.data[2]);
}
// Request force measurement
// if (use_fmf && !waiting_fmf)
// {
// waiting_fmf = true;
// getFingerData_serial(index);
// }
if ((info_fingers[index].type == 1) && !waiting_fmf)
{
waiting_fmf = true;
getFingerData(index, true);
}
// Handle response types
int motion = -1;
switch (msg.id)
{
/*** Opening ***/
case 0x43:
status_msg.width = val;
pub_state = true;
cnt[0]++;
break;
/*** Speed ***/
case 0x44:
status_msg.speed = val;
cnt[1]++;
break;
/*** Force ***/
case 0x45:
status_msg.force = val;
cnt[2]++;
break;
/*** Home ***/
case 0x20:
// Homing command; nothing to do
break;
case 0x22:
// Stop command; nothing to do
break;
/*** Move ***/
case 0x21:
// Move commands are sent from outside this thread
case 0x25:
// Grasping object
case 0x26:
// Releasing object
if (status == E_SUCCESS)
{
status_msg.status = std::string(status_to_str(status));
ROS_INFO("Position reached");
motion = 0;
}
else if (status == E_AXIS_BLOCKED)
{
status_msg.status = std::string(status_to_str(status));
ROS_INFO("Axis blocked");
motion = 0;
}
else if (status == E_CMD_PENDING)
{
status_msg.status = std::string(status_to_str(status));
ROS_INFO("Movement started");
motion = 1;
}
else if (status == E_ALREADY_RUNNING)
{
status_msg.status = std::string(status_to_str(status));
ROS_INFO("Movement error: already running");
}
else if (status == E_CMD_ABORTED)
{
status_msg.status = std::string(status_to_str(status));
ROS_INFO("Movement aborted");
motion = 0;
}
else
{
status_msg.status = std::string(status_to_str(status));
ROS_INFO("Movement error");
motion = 0;
}
break;
/*** Finger Data ***/
case 0x63:
if (status == E_SUCCESS)
{
vResult[0] = msg.data[2];
vResult[1] = msg.data[3];
vResult[2] = msg.data[4];
vResult[3] = msg.data[5];
finger_data[index] = convert(vResult);
}
else
finger_data[index] = 0.0;
waiting_fmf = false;
index = abs(index - 1);
break;
/*** Soft Limits ***/
case 0x34:
if (status == E_SUCCESS) ROS_INFO("New Soft Limits");
break;
case 0x35:
if (status == E_SUCCESS && msg.len == 10)
{
unsigned char limit_minus[4];
unsigned char limit_plus[4];
limit_minus[0] = msg.data[2];
limit_minus[1] = msg.data[3];
limit_minus[2] = msg.data[4];
limit_minus[3] = msg.data[5];
limit_plus[0] = msg.data[6];
limit_plus[1] = msg.data[7];
limit_plus[2] = msg.data[8];
limit_plus[3] = msg.data[9];
ROS_INFO("Soft limits: \nLIMIT MINUS %f\nLIMIT PLUS %f\n", convert(limit_minus), convert(limit_plus));
}
else ROS_INFO("Failed to read soft limits, len %d", msg.len);
break;
case 0x36:
if (status == E_SUCCESS) ROS_INFO("Soft Limits Cleared");
break;
default:
ROS_INFO("Received response 0x%02x (%2dB)\n", msg.id, msg.len);
}
// ***** PUBLISH motion message
if (motion == 0 || motion == 1)
{
std_msgs::Bool moving_msg;
moving_msg.data = motion;
g_pub_moving.publish(moving_msg);
g_ismoving = motion;
}
// ***** PUBLISH state message & joint message
if (pub_state)
{
pub_state = false;
if (info_fingers[0].type == 1)
status_msg.force_finger0 = finger_data[0];
else
status_msg.force_finger0 = status_msg.force/2;
if (info_fingers[1].type == 1)
status_msg.force_finger1 = finger_data[1];
else
status_msg.force_finger1 = status_msg.force/2;
g_pub_state.publish(status_msg);
joint_states.header.stamp = ros::Time::now();;
joint_states.position[0] = -status_msg.width/2000.0;
joint_states.position[1] = status_msg.width/2000.0;
joint_states.velocity[0] = status_msg.speed/1000.0;
joint_states.velocity[1] = status_msg.speed/1000.0;
joint_states.effort[0] = status_msg.force_finger0;
joint_states.effort[1] = status_msg.force_finger1;
g_pub_joint.publish(joint_states);
}
// Check # of received messages regularly
double rate_exp = 1000.0 / (double)interval_ms;
std::string names[3] = { "opening", "speed", "force" };
std::chrono::duration<float> t = std::chrono::system_clock::now() - time_start;
double t_ = t.count();
if (t_ > 5.0)
{
time_start = std::chrono::system_clock::now();
//printf("Infos for %5.1fHz, %5.1fHz, %5.1fHz\n", (double)cnt[0]/t_, (double)cnt[1]/t_, (double)cnt[2]/t_);
std::string info = "Rates for ";
for (int i=0; i<3; i++)
{
double rate_is = (double)cnt[i]/t_;
info += names[i] + ": " + std::to_string((int)rate_is) + "Hz, ";
if (rate_is == 0.0)
ROS_ERROR("Did not receive data for %s", names[i].c_str());
}
ROS_DEBUG_STREAM((info + " expected: " + std::to_string((int)rate_exp) + "Hz").c_str());
cnt[0] = 0; cnt[1] = 0; cnt[2] = 0;
}
}
// Disable automatic updates
getOpening(0);
getSpeed(0);
getForce(0);
ROS_INFO("Thread ended");
}
/** \brief Reads gripper responses in auto_update mode. The gripper pushes state messages in regular intervals. */
void read_thread(int interval_ms)
{
ROS_INFO("Thread started");
status_t status;
int res;
bool pub_state = false;
double rate_exp = 1000.0 / (double)interval_ms;
std::string names[3] = { "opening", "speed", "force" };
// Prepare messages
wsg_50_common::Status status_msg;
status_msg.status = "UNKNOWN";
sensor_msgs::JointState joint_states;
joint_states.header.frame_id = "wsg50_base_link";
joint_states.name.push_back("wsg50_finger_left_joint");
joint_states.name.push_back("wsg50_finger_right_joint");
joint_states.position.resize(2);
joint_states.velocity.resize(2);
joint_states.effort.resize(2);
// Request automatic updates (error checking is done below)
getOpening(interval_ms);
getSpeed(interval_ms);
getForce(interval_ms);
msg_t msg; msg.id = 0; msg.data = 0; msg.len = 0;
int cnt[3] = {0,0,0};
auto time_start = std::chrono::system_clock::now();
while (g_mode_periodic) {
// Receive gripper response
msg_free(&msg);
res = msg_receive( &msg );
if (res < 0 || msg.len < 2) {
ROS_ERROR("Gripper response failure");
continue;
}
float val = 0.0;
status = cmd_get_response_status(msg.data);
// Decode float for opening/speed/force
if (msg.id >= 0x43 && msg.id <= 0x45 && msg.len == 6) {
if (status != E_SUCCESS) {
ROS_ERROR("Gripper response failure for opening/speed/force\n");
continue;
}
val = convert(&msg.data[2]);
}
// Handle response types
int motion = -1;
switch (msg.id) {
/*** Opening ***/
case 0x43:
status_msg.width = val;
pub_state = true;
cnt[0]++;
break;
/*** Speed ***/
case 0x44:
status_msg.speed = val;
cnt[1]++;
break;
/*** Force ***/
case 0x45:
status_msg.force = val;
cnt[2]++;
break;
/*** Move ***/
// Move commands are sent from outside this thread
case 0x21:
if (status == E_SUCCESS) {
ROS_INFO("Position reached");
motion = 0;
} else if (status == E_AXIS_BLOCKED) {
ROS_INFO("Axis blocked");
motion = 0;
} else if (status == E_CMD_PENDING) {
ROS_INFO("Movement started");
motion = 1;
} else if (status == E_ALREADY_RUNNING) {
ROS_INFO("Movement error: already running");
} else if (status == E_CMD_ABORTED) {
ROS_INFO("Movement aborted");
motion = 0;
} else {
ROS_INFO("Movement error");
motion = 0;
}
break;
/*** Stop ***/
// Stop commands are sent from outside this thread
case 0x22:
// Stop command; nothing to do
break;
default:
ROS_INFO("Received unknown respone 0x%02x (%2dB)\n", msg.id, msg.len);
}
// ***** PUBLISH motion message
if (motion == 0 || motion == 1) {
std_msgs::Bool moving_msg;
moving_msg.data = motion;
g_pub_moving.publish(moving_msg);
g_ismoving = motion;
}
// ***** PUBLISH state message & joint message
if (pub_state) {
pub_state = false;
g_pub_state.publish(status_msg);
joint_states.header.stamp = ros::Time::now();;
joint_states.position[0] = -status_msg.width/2000.0;
joint_states.position[1] = status_msg.width/2000.0;
joint_states.velocity[0] = status_msg.speed/1000.0;
joint_states.velocity[1] = status_msg.speed/1000.0;
joint_states.effort[0] = status_msg.force;
joint_states.effort[1] = status_msg.force;
g_pub_joint.publish(joint_states);
}
// Check # of received messages regularly
std::chrono::duration<float> t = std::chrono::system_clock::now() - time_start;
double t_ = t.count();
if (t_ > 5.0) {
time_start = std::chrono::system_clock::now();
//printf("Infos for %5.1fHz, %5.1fHz, %5.1fHz\n", (double)cnt[0]/t_, (double)cnt[1]/t_, (double)cnt[2]/t_);
std::string info = "Rates for ";
for (int i=0; i<3; i++) {
double rate_is = (double)cnt[i]/t_;
info += names[i] + ": " + std::to_string((int)rate_is) + "Hz, ";
if (rate_is == 0.0)
ROS_ERROR("Did not receive data for %s", names[i].c_str());
}
ROS_DEBUG_STREAM((info + " expected: " + std::to_string((int)rate_exp) + "Hz").c_str());
cnt[0] = 0; cnt[1] = 0; cnt[2] = 0;
}
}
// Disable automatic updates
// TODO: The functions will receive an unexpected response
getOpening(0);
getSpeed(0);
getForce(0);
ROS_INFO("Thread ended");
}
