Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include <iostream>
00021 #include <boost/thread/thread.hpp>
00022 #include <boost/date_time.hpp>
00023 #include <boost/thread/locks.hpp>
00024
00025 #include "ros/ros.h"
00026 #include "ros/service.h"
00027 #include "ros/service_server.h"
00028 #include "sensor_msgs/JointState.h"
00029 #include "std_msgs/String.h"
00030
00031 #include <stdio.h>
00032 #include <iostream>
00033 #include <string>
00034
00035
00036 #include "controlAllegroHand.h"
00037
00038
00039 #define JOINT_STATE_TOPIC "/allegroHand/joint_states"
00040 #define JOINT_CMD_TOPIC "/allegroHand/joint_cmd"
00041 #define EXT_CMD_TOPIC "/allegroHand/external_cmd"
00042
00043 #define JOINT_DESIRED_TOPIC "/allegroHand/joint_desired_states"
00044 #define JOINT_CURRENT_TOPIC "/allegroHand/joint_current_states"
00045
00046
00047 double desired_position[DOF_JOINTS] = {0.0};
00048 double current_position[DOF_JOINTS] = {0.0};
00049 double previous_position[DOF_JOINTS] = {0.0};
00050 double current_position_filtered[DOF_JOINTS] = {0.0};
00051 double previous_position_filtered[DOF_JOINTS] = {0.0};
00052
00053 double desired_velocity[DOF_JOINTS] = {0.0};
00054 double current_velocity[DOF_JOINTS] = {0.0};
00055 double previous_velocity[DOF_JOINTS] = {0.0};
00056 double current_velocity_filtered[DOF_JOINTS] = {0.0};
00057
00058 double desired_torque[DOF_JOINTS] = {0.0};
00059
00060
00061 std::string jointNames[DOF_JOINTS] = { "joint_0.0", "joint_1.0", "joint_2.0", "joint_3.0" ,
00062 "joint_4.0", "joint_5.0", "joint_6.0", "joint_7.0" ,
00063 "joint_8.0", "joint_9.0", "joint_10.0", "joint_11.0",
00064 "joint_12.0", "joint_13.0", "joint_14.0", "joint_15.0" };
00065
00066 int frame = 0;
00067
00068
00069 int lEmergencyStop = 0;
00070
00071 boost::mutex *mutex;
00072
00073
00074 ros::Publisher joint_state_pub;
00075 ros::Subscriber joint_cmd_sub;
00076 ros::Subscriber external_cmd_sub;
00077 sensor_msgs::JointState msgJoint;
00078 std::string ext_cmd;
00079
00080
00081 ros::Time tstart;
00082 ros::Time tnow;
00083 double secs;
00084 double dt;
00085
00086
00087 controlAllegroHand *canDevice;
00088
00089
00090
00091
00092 void SetjointCallback(const sensor_msgs::JointState& msg)
00093 {
00094 mutex->lock();
00095 for(int i=0;i<DOF_JOINTS;i++) desired_position[i] = msg.position[i];
00096 mutex->unlock();
00097 }
00098
00099
00100 void extCmdCallback(const std_msgs::String::ConstPtr& msg)
00101 {
00102 ROS_INFO("CTRL: Processing external command");
00103 ext_cmd = msg->data.c_str();
00104
00105
00106 if (ext_cmd.compare("comparisonString") == 0)
00107 {
00108
00109 }
00110 else
00111 {
00112
00113 }
00114 }
00115
00116
00117
00118
00119 int main(int argc, char** argv)
00120 {
00121 using namespace std;
00122
00123 ros::init(argc, argv, "allegro_hand_core");
00124 ros::Time::init();
00125
00126 ros::NodeHandle nh;
00127
00128
00129
00130 ros::Rate rate(1./ALLEGRO_CONTROL_TIME_INTERVAL);
00131
00132 mutex = new boost::mutex();
00133
00134
00135 joint_state_pub = nh.advertise<sensor_msgs::JointState>(JOINT_STATE_TOPIC, 3);
00136 joint_cmd_sub = nh.subscribe(JOINT_CMD_TOPIC, 3, SetjointCallback);
00137 external_cmd_sub = nh.subscribe(EXT_CMD_TOPIC, 1, extCmdCallback);
00138
00139
00140 msgJoint.position.resize(DOF_JOINTS);
00141 msgJoint.velocity.resize(DOF_JOINTS);
00142 msgJoint.effort.resize(DOF_JOINTS);
00143 msgJoint.name.resize(DOF_JOINTS);
00144
00145
00146 for(int i=0; i<DOF_JOINTS; i++) msgJoint.name[i] = jointNames[i];
00147
00148
00149
00150
00151 string robot_name, whichHand, manufacturer, origin, serial;
00152 double version;
00153 ros::param::get("~hand_info/robot_name",robot_name);
00154 ros::param::get("~hand_info/which_hand",whichHand);
00155 ros::param::get("~hand_info/manufacturer",manufacturer);
00156 ros::param::get("~hand_info/origin",origin);
00157 ros::param::get("~hand_info/serial",serial);
00158 ros::param::get("~hand_info/version",version);
00159
00160
00161
00162 canDevice = new controlAllegroHand();
00163 canDevice->init();
00164 usleep(3000);
00165
00166
00167 cout << endl << endl << robot_name << " v" << version << endl << serial << " (" << whichHand << ")" << endl << manufacturer << endl << origin << endl << endl;
00168
00169
00170 for(int i=0; i<16; i++) desired_torque[i] = 0.0;
00171
00172
00173 tstart = ros::Time::now();
00174
00175 while(ros::ok())
00176 {
00177
00178
00179 tnow = ros::Time::now();
00180 dt = 1e-9*(tnow - tstart).nsec;
00181 tstart = tnow;
00182
00183
00184 for(int i=0; i<DOF_JOINTS; i++) previous_position[i] = current_position[i];
00185
00186
00187
00188
00189
00190
00191
00192 canDevice->setTorque(desired_torque);
00193 lEmergencyStop = canDevice->update();
00194 canDevice->getJointInfo(current_position);
00195
00196
00197
00198
00199
00200 if( lEmergencyStop < 0 )
00201 {
00202 ROS_ERROR("\n\nAllegro Hand Node is Shutting Down! (Emergency Stop)\n");
00203 ros::shutdown();
00204 }
00205
00206
00207
00208
00209
00210 for(int i=0; i<DOF_JOINTS; i++)
00211 {
00212 current_position_filtered[i] = (0.6*current_position_filtered[i]) + (0.198*previous_position[i]) + (0.198*current_position[i]);
00213 current_velocity[i] = (current_position_filtered[i] - previous_position_filtered[i]) / dt;
00214 current_velocity_filtered[i] = (0.6*current_velocity_filtered[i]) + (0.198*previous_velocity[i]) + (0.198*current_velocity[i]);
00215 }
00216
00217
00218
00219 if(frame>100)
00220 {
00221
00222
00223
00224
00225
00226
00227
00228 }
00229
00230
00231
00232
00233
00234
00235
00236 msgJoint.header.stamp = tnow;
00237 for(int i=0; i<DOF_JOINTS; i++)
00238 {
00239 msgJoint.position[i] = current_position_filtered[i];
00240 msgJoint.velocity[i] = current_velocity_filtered[i];
00241 msgJoint.effort[i] = desired_torque[i];
00242 }
00243 joint_state_pub.publish(msgJoint);
00244
00245
00246 frame++;
00247
00248 ros::spinOnce();
00249 rate.sleep();
00250 }
00251
00252
00253 nh.shutdown();
00254 delete canDevice;
00255 printf("\nAllegro Hand Node has been shut down. Bye!\n\n");
00256 return 0;
00257
00258 }
00259
00260