bool incrementSrv(wsg_50_common::Incr::Request &req, wsg_50_common::Incr::Response &res)
{
if (req.direction == "open"){
if (!objectGraspped){
float currentWidth = getOpening();
float nextWidth = currentWidth + req.increment;
if ( (currentWidth < GRIPPER_MAX_OPEN) && nextWidth < GRIPPER_MAX_OPEN ){
//grasp(nextWidth, 1);
move(nextWidth,20, true);
currentWidth = nextWidth;
}else if( nextWidth >= GRIPPER_MAX_OPEN){
//grasp(GRIPPER_MAX_OPEN, 1);
move(GRIPPER_MAX_OPEN,1, true);
currentWidth = GRIPPER_MAX_OPEN;
}
}else{
ROS_INFO("Releasing object...");
release(GRIPPER_MAX_OPEN, 20);
objectGraspped = false;
}
}else if (req.direction == "close"){
if (!objectGraspped){
float currentWidth = getOpening();
float nextWidth = currentWidth - req.increment;
if ( (currentWidth > GRIPPER_MIN_OPEN) && nextWidth > GRIPPER_MIN_OPEN ){
//grasp(nextWidth, 1);
move(nextWidth,20, true);
currentWidth = nextWidth;
}else if( nextWidth <= GRIPPER_MIN_OPEN){
//grasp(GRIPPER_MIN_OPEN, 1);
move(GRIPPER_MIN_OPEN,1, true);
currentWidth = GRIPPER_MIN_OPEN;
}
}
}
return true;
}
/** \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");
}
/** \brief Loop for state polling in modes script and polling. Also sends command in script mode. */
void timer_cb(const ros::TimerEvent& ev)
{
// ==== Get state values by built-in commands ====
gripper_response info;
float acc = 0.0;
info.speed = 0.0;
if (g_mode_polling) {
const char * state = systemState();
if (!state)
return;
info.state_text = std::string(state);
info.position = getOpening();
acc = getAcceleration();
info.f_motor = getForce();//getGraspingForce();
} else if (g_mode_script) {
// ==== Call custom measure-and-move command ====
int res = 0;
if (!std::isnan(g_goal_position)) {
ROS_INFO("Position command: pos=%5.1f, speed=%5.1f", g_goal_position, g_speed);
res = script_measure_move(1, g_goal_position, g_speed, info);
} else if (!std::isnan(g_goal_speed)) {
ROS_INFO("Velocity command: speed=%5.1f", g_goal_speed);
res = script_measure_move(2, 0, g_goal_speed, info);
} else
res = script_measure_move(0, 0, 0, info);
if (!std::isnan(g_goal_position))
g_goal_position = NAN;
if (!std::isnan(g_goal_speed))
g_goal_speed = NAN;
if (!res) {
ROS_ERROR("Measure-and-move command failed");
return;
}
// ==== Moving msg ====
if (g_ismoving != info.ismoving) {
std_msgs::Bool moving_msg;
moving_msg.data = info.ismoving;
g_pub_moving.publish(moving_msg);
g_ismoving = info.ismoving;
}
} else
return;
// ==== Status msg ====
wsg_50_common::Status status_msg;
status_msg.status = info.state_text;
status_msg.width = info.position;
status_msg.speed = info.speed;
status_msg.acc = acc;
status_msg.force = info.f_motor;
status_msg.force_finger0 = info.f_finger0;
status_msg.force_finger1 = info.f_finger1;
g_pub_state.publish(status_msg);
// ==== Joint state msg ====
// \todo Use name of node for joint names
sensor_msgs::JointState joint_states;
joint_states.header.stamp = ros::Time::now();;
joint_states.header.frame_id = "summit_xl_wsg50_base_link";
joint_states.name.push_back("summit_xl_wsg50_finger_left_joint");
joint_states.name.push_back("summit_xl_wsg50_finger_right_joint");
joint_states.position.resize(2);
joint_states.position[0] = -info.position/2000.0;
joint_states.position[1] = info.position/2000.0;
joint_states.velocity.resize(2);
joint_states.velocity[0] = info.speed/1000.0;
joint_states.velocity[1] = info.speed/1000.0;
joint_states.effort.resize(2);
joint_states.effort[0] = info.f_motor;
joint_states.effort[1] = info.f_motor;
g_pub_joint.publish(joint_states);
// printf("Timer, last duration: %6.1f\n", ev.profile.last_duration.toSec() * 1000.0);
}
int main( int argc, char **argv )
{
ros::init(argc, argv, "wsg_50");
ros::NodeHandle nh;
std::string ip;
int port;
ROS_INFO("WSG 50 - ROS NODE");
nh.param("wsg_50_tcp/ip", ip, std::string("192.168.1.20"));
nh.param("wsg_50_tcp/port", port, 1000);
// Connect to device using TCP
if( cmd_connect_tcp( ip.c_str(), port ) == 0 )
{
ROS_INFO("TCP connection stablished");
// Services
ros::ServiceServer moveSS = nh.advertiseService("wsg_50/move", moveSrv);
ros::ServiceServer graspSS = nh.advertiseService("wsg_50/grasp", graspSrv);
ros::ServiceServer releaseSS = nh.advertiseService("wsg_50/release", releaseSrv);
ros::ServiceServer homingSS = nh.advertiseService("wsg_50/homing", homingSrv);
ros::ServiceServer stopSS = nh.advertiseService("wsg_50/stop", stopSrv);
ros::ServiceServer ackSS = nh.advertiseService("wsg_50/ack", ackSrv);
ros::ServiceServer incrementSS = nh.advertiseService("wsg_50/move_incrementally", incrementSrv);
ros::ServiceServer setAccSS = nh.advertiseService("wsg_50/set_acceleration", setAccSrv);
ros::ServiceServer setForceSS = nh.advertiseService("wsg_50/set_force", setForceSrv);
// Publisher
ros::Publisher state_pub = nh.advertise<wsg_50_common::Status>("wsg_50/status", 1000);
ros::Publisher joint_states_pub = nh.advertise<sensor_msgs::JointState>("/joint_states", 10);
ROS_INFO("Ready to use.");
homing();
ros::Rate loop_rate(1); // loop at 1Hz
while( ros::ok() ){
//Loop waiting for orders and updating the state
//Create the msg to send
wsg_50_common::Status status_msg;
//Get state values
const char * aux;
aux = systemState();
int op = getOpening();
int acc = getAcceleration();
int force = getGraspingForceLimit();
std::stringstream ss;
ss << aux;
status_msg.status = ss.str();
status_msg.width = op;
status_msg.acc = acc;
status_msg.force = force;
state_pub.publish(status_msg);
sensor_msgs::JointState joint_states;
joint_states.header.stamp = ros::Time::now();;
joint_states.header.frame_id = "wsg_50_gripper_base_link";
joint_states.name.push_back("wsg_50_gripper_base_joint_gripper_left");
joint_states.name.push_back("wsg_50_gripper_base_joint_gripper_right");
joint_states.position.resize(2);
joint_states.position[0] = -op/2000.0;
joint_states.position[1] = op/2000.0;
//joint_states.velocity.resize(2);
//joint_states.velocity[0];
//joint_states.velocity[1];
joint_states.effort.resize(2);
joint_states.effort[0] = force;
joint_states.effort[1] = force;
joint_states_pub.publish(joint_states);
loop_rate.sleep();
ros::spinOnce();
}
}else{
ROS_ERROR("Unable to connect via TCP, please check the port and address used.");
}
// Disconnect - won't be executed atm. as the endless loop in test()
// will never return.
cmd_disconnect();
return 0;
}
