/** \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"); }