00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <cob_hand_bridge/InitFinger.h>
00019 #include <cob_hand_bridge/Status.h>
00020
00021 #include <std_srvs/Trigger.h>
00022
00023 #include <sensor_msgs/JointState.h>
00024 #include <control_msgs/FollowJointTrajectoryAction.h>
00025
00026 #include <diagnostic_updater/publisher.h>
00027
00028 #include <actionlib/server/simple_action_server.h>
00029
00030 #include <ros/ros.h>
00031
00032 #include <boost/thread/mutex.hpp>
00033 #include <boost/shared_ptr.hpp>
00034 #include <boost/scoped_ptr.hpp>
00035
00036 #include <angles/angles.h>
00037
00038 boost::mutex g_mutex;
00039
00040 cob_hand_bridge::Status::ConstPtr g_status;
00041 boost::shared_ptr<diagnostic_updater::TimeStampStatus> g_topic_status;
00042
00043 ros::Publisher g_js_pub;
00044 sensor_msgs::JointState g_js;
00045
00046 ros::ServiceServer g_init_srv;
00047
00048 ros::ServiceClient g_init_finger_client;
00049 ros::ServiceClient g_halt_client;
00050
00051 ros::Publisher g_command_pub;
00052 typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> FollowJointTrajectoryActionServer;
00053 boost::scoped_ptr<FollowJointTrajectoryActionServer> g_as;
00054 control_msgs::FollowJointTrajectoryGoalConstPtr g_goal;
00055 cob_hand_bridge::JointValues g_command;
00056 cob_hand_bridge::JointValues g_default_command;
00057 ros::Timer g_command_timer;
00058 ros::Timer g_deadline_timer;
00059 double g_stopped_velocity;
00060 double g_stopped_current;
00061 bool g_initialized;
00062 bool g_motion_stopped;
00063 bool g_control_stopped;
00064 bool g_motors_moved;
00065 std::vector<double> g_goal_tolerance;
00066
00067 bool isFingerReady_nolock() {
00068 return g_status && (g_status->status & (g_status->MASK_FINGER_READY | g_status->MASK_ERROR)) == g_status->MASK_FINGER_READY && g_status->rc == 0;
00069 }
00070
00071 bool checkAction_nolock(bool deadline_exceeded){
00072 control_msgs::FollowJointTrajectoryResult result;
00073 if(g_as->isActive()){
00074 bool goal_reached = false;
00075 if(g_motion_stopped) {
00076 goal_reached = true;;
00077 for(size_t i = 0; i < g_status->joints.position_cdeg.size(); ++i){
00078 if(fabs(g_status->joints.position_cdeg[i]-g_command.position_cdeg[i]) > g_goal_tolerance[i]){
00079 goal_reached = false;
00080 result.error_code = result.GOAL_TOLERANCE_VIOLATED;
00081 break;
00082 }
00083 }
00084 }
00085 if(!isFingerReady_nolock()) {
00086 g_deadline_timer.stop();
00087 g_as->setAborted();
00088 }else if(g_motion_stopped && (goal_reached || g_motors_moved)) {
00089 g_deadline_timer.stop();
00090 g_as->setSucceeded(result);
00091 }else if (deadline_exceeded) {
00092 g_deadline_timer.stop();
00093 result.error_code = result.GOAL_TOLERANCE_VIOLATED;
00094 g_as->setAborted(result, "goal not reached in time");
00095 return false;
00096 }
00097 }
00098 return true;
00099 }
00100
00101 bool initCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
00102 {
00103 boost::mutex::scoped_lock lock(g_mutex);
00104
00105 ros::NodeHandle nh_priv("~");
00106
00107 if(!g_status) {
00108 res.message = "hand is not yet connected";
00109 }else if(g_status->status == g_status->NOT_INITIALIZED) {
00110 lock.unlock();
00111 cob_hand_bridge::InitFinger srv;
00112 srv.request.port = nh_priv.param<std::string>("sdhx/port", "/dev/ttyACM0");
00113 srv.request.min_pwm0 = nh_priv.param("sdhx/min_pwm0", 0);
00114 srv.request.min_pwm1 = nh_priv.param("sdhx/min_pwm1", 0);
00115 srv.request.max_pwm0 = nh_priv.param("sdhx/max_pwm0", 0);
00116 srv.request.max_pwm1 = nh_priv.param("sdhx/max_pwm1", 0);
00117
00118 if(g_init_finger_client.waitForExistence(ros::Duration(nh_priv.param("sdhx/connect_timeout", 10)))){
00119 if(!g_init_finger_client.call(srv)) return false;
00120 res.success = srv.response.success;
00121 if(res.success) g_as->start();
00122 }else{
00123 res.message = "init_finger service does not exist";
00124 }
00125 lock.lock();
00126 g_initialized = res.success;
00127 }else if(!g_initialized){
00128 g_as->start();
00129 res.success = true;
00130 res.message = "finger already initialized, restarting the controller";
00131 g_initialized = true;
00132 }else{
00133 res.success = true;
00134 res.message = "already initialized";
00135 }
00136
00137 return true;
00138 }
00139
00140 void statusCallback(const cob_hand_bridge::Status::ConstPtr& msg){
00141 boost::mutex::scoped_lock lock(g_mutex);
00142 double dt = g_js.header.stamp.toSec() - msg->stamp.toSec();
00143 bool first = !g_status;
00144 bool calc_vel = !first && dt != 0 ;
00145 g_status = msg;
00146 g_topic_status->tick(msg->stamp);
00147
00148 g_motion_stopped = true;
00149 g_control_stopped = true;
00150
00151 if(msg->status & msg->MASK_FINGER_READY){
00152 for(size_t i=0; i < msg->joints.position_cdeg.size(); ++i){
00153 double new_pos = angles::from_degrees(msg->joints.position_cdeg[i]/100.0);
00154 if(calc_vel){
00155 g_js.velocity[i] = (new_pos - g_js.position[i]) / dt;
00156 }
00157 if(fabs(g_js.velocity[i]) > g_stopped_velocity){
00158 g_motion_stopped = false;
00159 g_motors_moved = true;
00160 }
00161 if(fabs(msg->joints.current_100uA[i]) > g_stopped_current){
00162 g_control_stopped = false;
00163 }
00164 g_js.position[i] = new_pos;
00165 }
00166 g_js.header.stamp = msg->stamp;
00167 g_js_pub.publish(g_js);
00168 }
00169 checkAction_nolock(false);
00170
00171 if(first) g_init_srv = ros::NodeHandle("driver").advertiseService("init", initCallback);
00172 }
00173
00174 void reportDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat){
00175 boost::mutex::scoped_lock lock(g_mutex);
00176
00177 if(!g_status){
00178 stat.summary(stat.ERROR, "not connected");
00179 return;
00180 }
00181
00182 if(g_status->status == g_status->NOT_INITIALIZED){
00183 stat.summary(stat.WARN, "not initialized");
00184 }else if(g_status->status & g_status->MASK_ERROR){
00185 stat.summary(stat.ERROR, "Bridge has error");
00186 }else{
00187 stat.summary(stat.OK, "Connected and running");
00188 }
00189 stat.add("sdhx_ready", bool(g_status->status & g_status->MASK_FINGER_READY));
00190 stat.add("sdhx_rc", uint32_t(g_status->rc));
00191 stat.add("sdhx_motion_stopped", g_motion_stopped);
00192 stat.add("sdhx_control_stopped", g_control_stopped);
00193
00194 if(g_status->rc > 0){
00195 stat.mergeSummary(stat.ERROR, "SDHx has error");
00196 }
00197 }
00198
00199 void callHalt(){
00200 if(g_halt_client.exists()){
00201 std_srvs::Trigger srv;
00202 if(!g_halt_client.call(srv) || !srv.response.success){
00203 ROS_ERROR("Halt service did not succeed");
00204 }
00205 }else{
00206 ROS_ERROR("Halt service is not available");
00207 }
00208 }
00209
00210 void handleDeadline(const ros::TimerEvent &){
00211 boost::mutex::scoped_lock lock(g_mutex);
00212 if(g_as->isActive()){
00213 if(!checkAction_nolock(true)){
00214 g_command.position_cdeg = g_status->joints.position_cdeg;
00215 lock.unlock();
00216 callHalt();
00217 lock.lock();
00218 }
00219 }
00220 }
00221
00222 void goalCB() {
00223
00224 control_msgs::FollowJointTrajectoryGoalConstPtr goal = g_as->acceptNewGoal();
00225
00226 g_deadline_timer.stop();
00227
00228
00229 control_msgs::FollowJointTrajectoryResult result;
00230 result.error_code = result.INVALID_GOAL;
00231 if(goal->trajectory.points.size()!=1 && (goal->trajectory.points.size()!=2 || !goal->trajectory.points[0].time_from_start.isZero() ) ){
00232 g_as->setAborted(result, "goal is not valid");
00233 return;
00234 }
00235
00236 cob_hand_bridge::JointValues new_command = g_default_command;
00237 size_t found = 0;
00238
00239 for(size_t i=0; i < g_js.name.size(); ++i){
00240 for(size_t j=0; j < goal->trajectory.joint_names.size(); ++j){
00241 if(g_js.name[i] == goal->trajectory.joint_names[j]){
00242 new_command.position_cdeg[i]= angles::to_degrees(goal->trajectory.points.back().positions[j])*100;
00243
00244 if(goal->trajectory.points.back().effort.size() > 0){
00245 if(goal->trajectory.points.back().effort.size() == new_command.current_100uA.size()){
00246 new_command.current_100uA[i] = goal->trajectory.points.back().effort[j] * 1000.0;
00247 }else{
00248 g_as->setAborted(result, "Number of effort values mismatch");
00249 return;
00250 }
00251 }
00252
00253 ++found;
00254 break;
00255 }
00256 }
00257 }
00258
00259 if(found != g_js.name.size()){
00260 g_as->setAborted(result, "Joint names mismatch");
00261 return;
00262 }
00263 std::vector<double> goal_tolerance(g_command.position_cdeg.size(), angles::to_degrees(g_stopped_velocity)*100);
00264
00265 for(size_t i = 0; i < goal->goal_tolerance.size(); ++i){
00266 bool missing = true;
00267 for(size_t j=0; j < g_js.name.size(); ++j){
00268 if(goal->goal_tolerance[i].name == g_js.name[j]){
00269 missing = false;
00270 if(goal->goal_tolerance[i].position > 0.0){
00271 goal_tolerance[j] = goal->goal_tolerance[i].position;
00272 }
00273 break;
00274 }
00275 }
00276 if(missing){
00277 g_as->setAborted(result, "Goal tolerance invalid");
00278 return;
00279 }
00280 }
00281
00282 ros::Time now = ros::Time::now();
00283 ros::Time trajectory_deadline = (goal->trajectory.header.stamp.isZero() ? now : goal->trajectory.header.stamp)
00284 + goal->trajectory.points.back().time_from_start + ros::Duration(goal->goal_time_tolerance);
00285 if(trajectory_deadline <= now){
00286 result.error_code = result.OLD_HEADER_TIMESTAMP;
00287 g_as->setAborted(result, "goal is not valid");
00288 return;
00289 }
00290
00291 boost::mutex::scoped_lock lock(g_mutex);
00292
00293 if(!isFingerReady_nolock()) {
00294 g_as->setAborted(result, "SDHx is not ready for commands");
00295 return;
00296 }
00297
00298 g_command = new_command;
00299
00300 g_goal_tolerance = goal_tolerance;
00301 g_motors_moved = false;
00302 g_command_timer.stop();
00303 g_command_pub.publish(g_command);
00304 g_deadline_timer.setPeriod(trajectory_deadline-ros::Time::now());
00305 g_deadline_timer.start();
00306 g_command_timer.start();
00307 }
00308
00309 void cancelCB() {
00310 boost::mutex::scoped_lock lock(g_mutex);
00311 g_command.position_cdeg = g_status->joints.position_cdeg;
00312 g_deadline_timer.stop();
00313 lock.unlock();
00314
00315 callHalt();
00316 g_as->setPreempted();
00317 }
00318
00319 void resendCommand(const ros::TimerEvent &e){
00320 boost::mutex::scoped_lock lock(g_mutex);
00321 if(isFingerReady_nolock()){
00322 if(g_control_stopped || !g_motion_stopped) g_command_pub.publish(g_command);
00323 } else {
00324 g_command_timer.stop();
00325 lock.unlock();
00326 callHalt();
00327 ROS_WARN("finger is not ready, stopped resend timer");
00328 }
00329 }
00330
00331 int main(int argc, char* argv[])
00332 {
00333 ros::init(argc, argv, "cob_hand_bridge_node");
00334
00335 ros::NodeHandle nh;
00336 ros::NodeHandle nh_d("driver");
00337 ros::NodeHandle nh_i("internal");
00338 ros::NodeHandle nh_priv("~");
00339
00340 if(!nh_priv.getParam("sdhx/joint_names", g_js.name)){
00341 ROS_ERROR("Please provide joint names for SDHx");
00342 return 1;
00343 }
00344
00345 if(g_command.position_cdeg.size() != g_js.name.size()){
00346 ROS_ERROR_STREAM("Number of joints does not match " << g_command.position_cdeg.size());
00347 return 1;
00348 }
00349
00350
00351 nh_priv.param("sdhx/stopped_velocity",g_stopped_velocity, 0.05);
00352 if(g_stopped_velocity <= 0.0){
00353 ROS_ERROR_STREAM("stopped_velocity must be a positive number");
00354 return 1;
00355 }
00356
00357 double stopped_current= nh_priv.param("sdhx/stopped_current", 0.1);
00358 if(stopped_current <= 0.0){
00359 ROS_ERROR_STREAM("stopped_current must be a positive number");
00360 return 1;
00361 }
00362 g_stopped_current = stopped_current * 1000.0;
00363
00364 std::vector<double> default_currents;
00365 if(nh_priv.getParam("sdhx/default_currents", default_currents)){
00366 if(default_currents.size() != g_default_command.current_100uA.size()){
00367 ROS_ERROR_STREAM("Number of current values does not match number of joints");
00368 return 1;
00369 }
00370 for(size_t i=0; i< g_default_command.current_100uA.size(); ++i){
00371 g_default_command.current_100uA[i] = default_currents[i] * 1000.0;
00372 }
00373 }else{
00374 g_default_command.current_100uA[0] = 2120;
00375 g_default_command.current_100uA[1] = 1400;
00376 }
00377
00378 g_default_command.velocity_cdeg_s[0] = 1000;
00379 g_default_command.velocity_cdeg_s[1] = 1000;
00380
00381 g_js.position.resize(g_command.position_cdeg.size());
00382 g_js.velocity.resize(g_command.position_cdeg.size());
00383
00384 diagnostic_updater::Updater diag_updater;
00385 diag_updater.setHardwareID(nh_priv.param("hardware_id", std::string("none")));
00386 diag_updater.add("bridge", reportDiagnostics);
00387
00388 diagnostic_updater::TimeStampStatusParam param(
00389 nh_priv.param("status/min_duration", -1.0),
00390 nh_priv.param("status/max_duration", 0.1)
00391 );
00392 g_topic_status.reset ( new diagnostic_updater::TimeStampStatus(param) );
00393 diag_updater.add("connection", boost::bind(&diagnostic_updater::TimeStampStatus::run, g_topic_status, _1));
00394
00395 ros::Timer diag_timer = nh.createTimer(ros::Duration(diag_updater.getPeriod()/2.0),boost::bind(&diagnostic_updater::Updater::update, &diag_updater));
00396
00397 double resend_period = nh_priv.param("sdhx/resend_period", 0.1);
00398 if(resend_period > 0.0) g_command_timer = nh.createTimer(ros::Duration(resend_period), resendCommand, false, false);
00399
00400 g_deadline_timer = nh.createTimer(ros::Duration(1.0), handleDeadline, true, false);
00401
00402 g_js_pub = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
00403
00404 ros::Subscriber status_sub = nh_i.subscribe("status", 1, statusCallback);
00405 g_init_finger_client = nh_i.serviceClient<cob_hand_bridge::InitFinger>("init_finger");
00406 g_command_pub = nh_i.advertise<cob_hand_bridge::JointValues>("command", 1);
00407 g_halt_client = nh_d.serviceClient<std_srvs::Trigger>("halt");
00408
00409 g_as.reset(new FollowJointTrajectoryActionServer(ros::NodeHandle("joint_trajectory_controller"), "follow_joint_trajectory",false));
00410 g_as->registerPreemptCallback(cancelCB);
00411 g_as->registerGoalCallback(goalCB);
00412
00413 ros::spin();
00414 return 0;
00415 }