// ------------------------------Auto Thread-------------------------------------
void* auto_thread(void*notused)
{
int homed;
homed = 0;
while(1)
{
//printf("home: %i\n",homed);
if ((last_input == BASECW) || (last_input == BASECCW) ||
(last_input == SHOULDERUP) || (last_input == SHOULDERDW) ||
(last_input == ELBOWUP) || (last_input == ELBOWDW) ||
(last_input == PITCHUP) || (last_input == PITCHDW) ||
(last_input == GRIPPEROP) || (last_input == GRIPPERCLOSE) ||
(last_input == ROLLCW) || (last_input == ROLLCCW))
homed = 0;
if (memData.autoModeOn == ON)
{
if (last_input == HOMEROBOT)
{
printf("homing!\n");
if (homed != 1)
{
homed = homing();
last_input = MANUALON;
writeMsg(last_input);
}
printf("home done\n");
flushall();
last_input = MANUALON;
writeMsg(last_input);
}
if (last_input == MANUALOFF)
{
if (readLowerByte.lowIpReserved1 == 1)
{
printf("No block in the loader\n");
flushall();
}
else
{
if (homed != 1)
homing();
homed = 0;
auto_mode();
}
last_input = MANUALON;
writeMsg(last_input);
printf("auto done\n");
flushall();
}
}
delay(50);
}
}
bool homingSrv(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res)
{
ROS_INFO("Homing...");
homing();
ROS_INFO("Home position reached.");
return true;
}
/**
* raven_homing()
*
* This function is called 1000 times per second during INIT mode
*
* \param device0 Which (top level) device to home (usually only one device per system)
* \param currParams Current parameters (for Run Level)
* \param begin_homing Flag to start the homing process
* \ingroup Control
*
* \brief Move to hard stops in controled way, "zero" the joint value, and then move to "home" position
*
* This function operates in two phases:
* -# Discover joint position by running to hard stop. Using PD control (I term zero'd) we move the joint at a smooth rate until
* current increases which indicates hitting hard mechanical stop.
* -# Move joints to "home" position. In this phase the robot moves from the joint limits to a designated pose in the center of the workspace.
* \todo Homing limits should be Amps not DAC units (see homing() ).
* \todo Eliminate or comment out Enders Game code!!
*/
int raven_homing(struct device *device0, struct param_pass *currParams, int begin_homing)
{
static int homing_inited = 0;
static unsigned long int delay, delay2;
struct DOF *_joint = NULL;
struct mechanism* _mech = NULL;
int i=0,j=0;
// Only run in init mode
if ( ! (currParams->runlevel == RL_INIT && currParams->sublevel == SL_AUTO_INIT ))
{
homing_inited = 0;
delay = gTime;
return 0; // return if we are in the WRONG run level (PLC state)
}
// Wait a short time for amps to turn on
if (gTime - delay < 1000)
{
return 0;
}
// Initialize the homing sequence.
if (begin_homing || !homing_inited) // only do this the first time through
{
// Zero out joint torques, and control inputs. Set joint.state=not_ready.
_mech = NULL; _joint = NULL;
while (loop_over_joints(device0, _mech, _joint, i,j) ) // foreach (joint)
{
_joint->tau_d = 0;
_joint->mpos_d = _joint->mpos;
_joint->jpos_d = _joint->jpos;
_joint->jvel_d = 0;
_joint->state = jstate_not_ready;
if (is_toolDOF(_joint))
jvel_PI_control(_joint, 1); // reset PI control integral term
homing_inited = 1;
}
log_msg("Homing sequence initialized");
}
// Specify motion commands
_mech = NULL; _joint = NULL;
while ( loop_over_joints(device0, _mech, _joint, i,j) )
{
// Initialize tools first.
if ( is_toolDOF(_joint) || tools_ready( &(device0->mech[i]) ) )
{
homing(_joint);
}
}
//Inverse Cable Coupling
invCableCoupling(device0, currParams->runlevel);
// Do PD control on all joints
_mech = NULL; _joint = NULL;
while ( loop_over_joints(device0, _mech, _joint, i,j) )
{
mpos_PD_control( _joint );
}
// Calculate output DAC values
TorqueToDAC(device0);
// Check homing conditions and set joint angles appropriately.
_mech = NULL; _joint = NULL;
while ( loop_over_joints(device0, _mech, _joint, i,j) )
{
struct DOF * _joint = &(_mech->joint[j]); ///\todo is this line necessary?
// Check to see if we've reached the joint limit.
if( check_homing_condition(_joint) )
{
log_msg("Found limit on joint %d cmd: %d \t", _joint->type, _joint->current_cmd, DOF_types[_joint->type].DAC_max);
_joint->state = jstate_hard_stop;
_joint->current_cmd = 0;
stop_trajectory(_joint);
log_msg("joint %d checked ",j);
}
// For each mechanism, check to see if the mech is finished homing.
if ( j == (MAX_DOF_PER_MECH-1) )
{
/// if we're homing tools, wait for tools to be finished
if (( !tools_ready(_mech) &&
_mech->joint[TOOL_ROT].state==jstate_hard_stop &&
_mech->joint[WRIST ].state==jstate_hard_stop &&
_mech->joint[GRASP1 ].state==jstate_hard_stop )
||
( tools_ready( _mech ) &&
_mech->joint[SHOULDER].state==jstate_hard_stop &&
_mech->joint[ELBOW ].state==jstate_hard_stop &&
_mech->joint[Z_INS ].state==jstate_hard_stop ))
{
if (delay2==0)
delay2=gTime;
if (gTime > delay2 + 200) // wait 200 ticks for cables to settle down
{
set_joints_known_pos(_mech, !tools_ready(_mech) ); // perform second phase
delay2 = 0;
}
}
}
}
return 0;
}
int main( int argc, char **argv )
{
ros::init(argc, argv, "wsg_50");
ros::NodeHandle nh("~");
signal(SIGINT, sigint_handler);
double rate, grasping_force;
std::string device;
int bitrate;
nh.param("read_rate", rate, 30.0);
nh.param("grasping_force", grasping_force, 80.0);
nh.param("device", device, std::string("/dev/ttyUSB3"));
nh.param("bitrate", bitrate, 38400);
nh.param("use_fmf", use_fmf, true);
// Connect to device
int res_con;
ROS_INFO("Connecting to %s: (rs232) at %d ...", device.c_str(), bitrate);
res_con = cmd_connect_serial(device.c_str(), bitrate);
if (res_con == 0 ) {
ROS_INFO("Gripper connection stablished");
// Services
ros::ServiceServer moveSS, graspSS, releaseSS, homingSS, stopSS, ackSS, incrementSS, setAccSS, setForceSS;
ros::ServiceServer move_no_waitingSS, grasp_no_waitingSS, release_no_waitingSS,
homing_no_waitingSS, stop_no_waitingSS, setAcc_no_waitingSS,
setForce_no_waitingSS, setLimitsSS, clearLimitsSS, getLimitsSS;
moveSS = nh.advertiseService("move", moveSrv);
graspSS = nh.advertiseService("grasp", graspSrv);
releaseSS = nh.advertiseService("release", releaseSrv);
homingSS = nh.advertiseService("homing", homingSrv);
stopSS = nh.advertiseService("stop", stopSrv);
ackSS = nh.advertiseService("ack", ackSrv);
setAccSS = nh.advertiseService("set_acceleration", setAccSrv);
setForceSS = nh.advertiseService("set_force", setForceSrv);
move_no_waitingSS = nh.advertiseService("move_no_waiting", moveSrv_no_waiting);
grasp_no_waitingSS = nh.advertiseService("grasp_no_waiting", graspSrv_no_waiting);
release_no_waitingSS = nh.advertiseService("release_no_waiting", releaseSrv_no_waiting);
homing_no_waitingSS = nh.advertiseService("homing_no_waiting", homingSrv_no_waiting);
stop_no_waitingSS = nh.advertiseService("stop_no_waiting", stopSrv_no_waiting);
setAcc_no_waitingSS = nh.advertiseService("set_acceleration_no_waiting", setAccSrv_no_waiting);
setForce_no_waitingSS = nh.advertiseService("set_force_no_waiting", setForceSrv_no_waiting);
setLimitsSS = nh.advertiseService("set_soft_limits", setLimitsSrv);
clearLimitsSS = nh.advertiseService("clear_soft_limits", clearLimitsSrv);
getLimitsSS = nh.advertiseService("view_soft_limits", getLimitsSrv);
// Publisher
g_pub_state = nh.advertise<wsg_50_common::Status>("status", 1000);
g_pub_joint = nh.advertise<sensor_msgs::JointState>("joint_states", 10);
g_pub_moving = nh.advertise<std_msgs::Bool>("moving", 10);
ROS_INFO("Ready to use, homing now...");
homing();
if (grasping_force > 0.0) {
ROS_INFO("Setting grasping force limit to %5.1f", grasping_force);
setGraspingForceLimit(grasping_force);
}
if (use_fmf)
{
ROS_INFO("Force measure fingers data reading: ON");
ROS_INFO("Reading finger info (type 0 - generic, 1 - WSG-FMF) ...");
getFingerInfo(0, info_fingers[0]);
getFingerInfo(1, info_fingers[1]);
ROS_INFO("Finger 0 type %d: data size %d bytes", info_fingers[0].type, info_fingers[0].data_size);
ROS_INFO("Finger 1 type %d: data size %d bytes", info_fingers[1].type, info_fingers[1].data_size);
if (info_fingers[0].type != info_fingers[1].type)
info_fingers[0].type = info_fingers[1].type = 1;
}
else
{
ROS_INFO("Force measure fingers data reading: OFF");
info_fingers[0].type = info_fingers[1].type = 0;
}
ROS_INFO("Init done. Starting timer/thread with target rate %.1f.", rate);
std::thread th;
g_thread = true;
th = std::thread(read_thread, (int)(1000.0/rate));
ros::spin();
} else {
ROS_ERROR("Unable to connect, please check the port and address used.");
}
return 0;
}
int main( int argc, char **argv )
{
ros::init(argc, argv, "wsg_50");
ros::NodeHandle nh("~");
signal(SIGINT, sigint_handler);
std::string ip, protocol, com_mode;
int port, local_port;
double rate, grasping_force;
bool use_udp = false;
nh.param("ip", ip, std::string("192.168.1.20"));
nh.param("port", port, 1000);
nh.param("local_port", local_port, 1501);
nh.param("protocol", protocol, std::string(""));
nh.param("com_mode", com_mode, std::string(""));
nh.param("rate", rate, 1.0); // With custom script, up to 30Hz are possible
nh.param("grasping_force", grasping_force, 0.0);
if (protocol == "udp")
use_udp = true;
else
protocol = "tcp";
if (com_mode == "script")
g_mode_script = true;
else if (com_mode == "auto_update")
g_mode_periodic = true;
else {
com_mode = "polling";
g_mode_polling = true;
}
ROS_INFO("Connecting to %s:%d (%s); communication mode: %s ...", ip.c_str(), port, protocol.c_str(), com_mode.c_str());
// Connect to device using TCP/USP
int res_con;
if (!use_udp)
res_con = cmd_connect_tcp( ip.c_str(), port );
else
res_con = cmd_connect_udp(local_port, ip.c_str(), port );
if (res_con == 0 ) {
ROS_INFO("Gripper connection stablished");
// Services
ros::ServiceServer moveSS, graspSS, releaseSS, homingSS, stopSS, ackSS, incrementSS, setAccSS, setForceSS;
if (g_mode_script || g_mode_polling) {
moveSS = nh.advertiseService("move", moveSrv);
graspSS = nh.advertiseService("grasp", graspSrv);
releaseSS = nh.advertiseService("release", releaseSrv);
homingSS = nh.advertiseService("homing", homingSrv);
stopSS = nh.advertiseService("stop", stopSrv);
ackSS = nh.advertiseService("ack", ackSrv);
incrementSS = nh.advertiseService("move_incrementally", incrementSrv);
setAccSS = nh.advertiseService("set_acceleration", setAccSrv);
setForceSS = nh.advertiseService("set_force", setForceSrv);
}
// Subscriber
ros::Subscriber sub_position, sub_speed;
if (g_mode_script || g_mode_periodic)
sub_position = nh.subscribe("goal_position", 5, position_cb);
if (g_mode_script)
sub_speed = nh.subscribe("goal_speed", 5, speed_cb);
// Publisher
g_pub_state = nh.advertise<wsg_50_common::Status>("status", 1000);
g_pub_joint = nh.advertise<sensor_msgs::JointState>("joint_states", 10);
if (g_mode_script || g_mode_periodic)
g_pub_moving = nh.advertise<std_msgs::Bool>("moving", 10);
ROS_INFO("Ready to use. Homing and taring now...");
homing();
ros::Duration(0.5).sleep();
doTare();
if (grasping_force > 0.0) {
ROS_INFO("Setting grasping force limit to %5.1f", grasping_force);
setGraspingForceLimit(grasping_force);
}
ROS_INFO("Init done. Starting timer/thread with target rate %.1f.", rate);
std::thread th;
ros::Timer tmr;
if (g_mode_polling || g_mode_script)
tmr = nh.createTimer(ros::Duration(1.0/rate), timer_cb);
if (g_mode_periodic)
th = std::thread(read_thread, (int)(1000.0/rate));
ros::spin();
} else {
ROS_ERROR("Unable to connect, please check the port and address used.");
}
ROS_INFO("Exiting...");
g_mode_periodic = false;
g_mode_script = false;
g_mode_polling = false;
sleep(1);
cmd_disconnect();
return 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;
}
