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