joint_trajectory_handler.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2011, Southwest Research Institute
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are met:
00009  *
00010  *      * Redistributions of source code must retain the above copyright
00011  *      notice, this list of conditions and the following disclaimer.
00012  *      * Redistributions in binary form must reproduce the above copyright
00013  *      notice, this list of conditions and the following disclaimer in the
00014  *      documentation and/or other materials provided with the distribution.
00015  *      * Neither the name of the Southwest Research Institute, nor the names
00016  *      of its contributors may be used to endorse or promote products derived
00017  *      from this software without specific prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029  * POSSIBILITY OF SUCH DAMAGE.
00030  */
00031 
00032 #include <dx100/joint_trajectory_handler.h>
00033 #include "simple_message/messages/joint_message.h"
00034 #include "simple_message/smpl_msg_connection.h"
00035 
00036 using namespace industrial::smpl_msg_connection;
00037 using namespace industrial::joint_message;
00038 using namespace industrial::simple_message;
00039 
00040 namespace motoman
00041 {
00042 namespace joint_trajectory_handler
00043 {
00044 JointTrajectoryHandler::JointTrajectoryHandler()
00045 {
00046 }
00047 
00048 JointTrajectoryHandler::JointTrajectoryHandler(ros::NodeHandle &n, SmplMsgConnection* robotConnecton) :
00049     node_(n)
00050 {
00051   ROS_INFO("Constructor joint trajectory handler node");
00052 
00053   this->mutex_.lock();
00054   this->sub_joint_tranectory_ = this->node_.subscribe("command", 0, &JointTrajectoryHandler::jointTrajectoryCB,
00055                                                       this);
00056   this->robot_ = robotConnecton;
00057   this->currentPoint = 0;
00058   this->state_ = JointTrajectoryStates::IDLE;
00059   this->trajectoryHandler_ =
00060       new boost::thread(boost::bind(&JointTrajectoryHandler::trajectoryHandler, this));
00061   ROS_INFO("Unlocking mutex");
00062   this->mutex_.unlock();
00063 }
00064 
00065 JointTrajectoryHandler::~JointTrajectoryHandler()
00066 {  
00067   trajectoryStop();
00068   this->sub_joint_tranectory_.shutdown();
00069   delete this->trajectoryHandler_;
00070 }
00071 
00072 void JointTrajectoryHandler::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
00073 {
00074   ROS_INFO("Receiving joint trajctory message");
00075   this->mutex_.lock();
00076   ROS_INFO("Processing joint trajctory message (mutex acquired)");
00077   ROS_DEBUG("Current state is: %d", this->state_);
00078   if (JointTrajectoryStates::IDLE != this->state_)
00079   {
00080     if (msg->points.empty())
00081     {
00082       ROS_INFO("Empty trajectory received, canceling current trajectory");
00083     }
00084     else
00085     {
00086       ROS_ERROR("Trajectory splicing not yet implemented, stopping current motion.");
00087     }
00088     trajectoryStop();
00089   }
00090   if (msg->points.empty())
00091   {
00092     ROS_INFO("Empty trajectory received while in IDLE state, nothing is done");
00093   }
00094   else
00095   {
00096     ROS_INFO("Loading trajectory, setting state to streaming");
00097     this->current_traj_ = *msg;
00098     
00099     // TODO: This section pads any trajectory below the minimum size with the same
00100     // end point.  This is required because the motoman side requires a minimum buffer
00101     // size before it start motion.
00102     while (current_traj_.points.size() <= 6)
00103     {
00104       ROS_DEBUG("Padding trajectory, current size: %d", current_traj_.points.size());
00105       current_traj_.points.push_back(current_traj_.points[current_traj_.points.size()-1]);
00106     };
00107     
00108     ROS_INFO("Executing trajectory of size: %d", this->current_traj_.points.size());
00109     this->currentPoint = 0;
00110     this->state_ = JointTrajectoryStates::STREAMING;
00111     streaming_start_ = ros::Time::now();
00112   }
00113   this->mutex_.unlock();
00114 }
00115 
00116 void JointTrajectoryHandler::trajectoryHandler()
00117 {
00118   JointMessage jMsg;
00119   SimpleMessage msg;
00120   SimpleMessage reply;
00121   trajectory_msgs::JointTrajectoryPoint pt;
00122   ROS_INFO("Starting joint trajectory handler state");
00123   while (ros::ok())
00124   {
00125     this->mutex_.lock();
00126 
00127     if (this->robot_->isConnected())
00128     {
00129       
00130       //TODO: These variables should be moved outside of this loop
00131       //so that we aren't contantly reinitializing them.
00132       JointMessage jMsg;
00133       SimpleMessage msg;
00134       SimpleMessage reply;
00135       trajectory_msgs::JointTrajectoryPoint pt;
00136       switch (this->state_)
00137       {
00138         case JointTrajectoryStates::IDLE:
00139           ros::Duration(0.250).sleep();
00140           break;
00141 
00142 
00143         case JointTrajectoryStates::STREAMING:
00144           if (this->currentPoint < this->current_traj_.points.size())
00145           {
00146             // unsigned int lastCurrentPoint = currentPoint;
00147             // currentPoint = getNextTrajectoryPoint(current_traj_,
00148             //                                       streaming_start_,
00149             //                                       ros::Time::now());
00150             //ROS_INFO("Skipping from point[%d] to point[%d]", lastCurrentPoint, currentPoint);
00151             pt = this->current_traj_.points[this->currentPoint];
00152             
00153             jMsg.setSequence(this->currentPoint);
00154 
00155             for (int i = 0; i < this->current_traj_.joint_names.size(); i++)
00156             {
00157               jMsg.getJoints().setJoint(i, pt.positions[i]);
00158             }
00159             
00160             ROS_DEBUG("Sending joint point");
00161             jMsg.toRequest(msg);
00162             if (this->robot_->sendAndReceiveMsg(msg, reply, false))
00163             //jMsg.toTopic(msg);
00164             //if (this->robot_->sendMsg(msg))
00165             {
00166               ROS_INFO("Point[%d of %d] sent to controller", 
00167                        this->currentPoint, this->current_traj_.points.size());
00168               this->currentPoint++;
00169             }
00170             else
00171             {
00172               ROS_WARN("Failed sent joint point, will try again");
00173             }
00174             //ROS_INFO_STREAM("Time taken to stream single point is " << (ros::WallTime::now()-start));
00175           }
00176           else
00177           {
00178             ROS_INFO("Trajectory streaming complete, setting state to IDLE");
00179             this->state_ = JointTrajectoryStates::IDLE;
00180           }
00181           break;
00182 
00183         default:
00184           ROS_ERROR("Joint trajectory handler: unknown state");
00185           this->state_ = JointTrajectoryStates::IDLE;
00186           break;
00187       }
00188 
00189     }
00190     else
00191     {
00192       ROS_INFO("Connecting to robot motion server");
00193       this->robot_->makeConnect();
00194     }
00195 
00196     this->mutex_.unlock();
00197     ros::Duration(0.005).sleep();
00198   }
00199 
00200   ROS_WARN("Exiting trajectory handler thread");
00201 }
00202 
00203 unsigned int JointTrajectoryHandler::getNextTrajectoryPoint(const trajectory_msgs::JointTrajectory& traj,
00204                                                             const ros::Time& start,
00205                                                             const ros::Time& cur)
00206 {
00207   for(unsigned int i = 0; i < traj.points.size(); i++) {
00208     ros::Duration dur(cur-start);
00209     if(dur < traj.points[i].time_from_start) {
00210       return i;
00211     }
00212   }
00213   return traj.points.size()-1;
00214 }
00215 
00216 
00217 void JointTrajectoryHandler::trajectoryStop()
00218 {
00219   JointMessage jMsg;
00220   SimpleMessage msg;
00221   SimpleMessage reply;
00222 
00223   ROS_INFO("Joint trajectory handler: entering stopping state");
00224   jMsg.setSequence(SpecialSeqValues::STOP_TRAJECTORY);
00225   jMsg.toRequest(msg);
00226   ROS_DEBUG("Sending stop command");
00227   this->robot_->sendAndReceiveMsg(msg, reply);
00228   ROS_DEBUG("Stop command sent, entring idle mode");
00229   this->state_ = JointTrajectoryStates::IDLE;
00230 }
00231 
00232 
00233 
00234 
00235 
00236 } //joint_trajectory_handler
00237 } //motoman
00238 


dx100
Author(s): Shaun Edwards (Southwest Research Institute)
autogenerated on Mon Oct 6 2014 02:25:33