joint_trajectory_downloader.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 "adept_common/joint_trajectory_downloader.h"
00033 #include "simple_message/joint_traj_pt.h"
00034 #include "simple_message/messages/joint_traj_pt_message.h"
00035 #include "simple_message/smpl_msg_connection.h"
00036 
00037 using namespace industrial::smpl_msg_connection;
00038 using namespace industrial::joint_data;
00039 using namespace industrial::joint_traj_pt;
00040 using namespace industrial::joint_traj_pt_message;
00041 using namespace industrial::simple_message;
00042 
00043 
00044 namespace adept
00045 {
00046 namespace joint_trajectory_downloader
00047 {
00048 JointTrajectoryDownloader::JointTrajectoryDownloader()
00049 {
00050 }
00051 
00052 JointTrajectoryDownloader::JointTrajectoryDownloader(ros::NodeHandle &n,
00053                 SmplMsgConnection* robotConnecton) : node_(n)
00054 {
00055         ROS_INFO("Constructor joint trajectory downloader node");
00056 
00057         this->sub_joint_trajectory_ = this->node_.subscribe("command",
00058                         0, &JointTrajectoryDownloader::jointTrajectoryCB, this);
00059         this->robot_ = robotConnecton;
00060         ROS_INFO("Joint trajectory downloader node initialized");
00061 }
00062 
00063 JointTrajectoryDownloader::~JointTrajectoryDownloader()
00064 {  
00065         //trajectoryStop();
00066         this->sub_joint_trajectory_.shutdown();
00067 }
00068 
00069 void JointTrajectoryDownloader::jointTrajectoryCB(
00070                 const trajectory_msgs::JointTrajectoryConstPtr &msg)
00071 {
00072         ROS_INFO("Receiving joint trajectory message");
00073 
00074   /*
00075         if (!checkTrajectory(msg))
00076         {
00077                 ROS_ERROR("Joint trajectory check failed, trajectory not downloaded");
00078                 return;
00079         }
00080 
00081         std::vector<double> joint_velocity_limits;
00082         if (!getVelocityLimits("robot_description", msg->joint_names, joint_velocity_limits))
00083         {
00084                 ROS_ERROR("Failed to get joint velocity limits");
00085                 return;
00086         }
00087   */
00088         for (int i = 0; i < msg->points.size(); i++)
00089         {
00090                 ROS_INFO("Sending joints trajectory point[%d]", i);
00091 
00092                 JointTrajPt jPt;
00093                 JointTrajPtMessage jMsg;
00094                 SimpleMessage topic;
00095                 trajectory_msgs::JointTrajectoryPoint pt;
00096 
00097                 pt = msg->points[i];
00098 
00099                 // The first and last sequence values must be given a special sequence
00100                 // value
00101                 if (0 == i)
00102                 {
00103                         ROS_DEBUG("First trajectory point, setting special sequence value");
00104                         jPt.setSequence(SpecialSeqValues::START_TRAJECTORY_DOWNLOAD);
00105                 }
00106                 else if (msg->points.size() - 1 == i)
00107                 {
00108                         ROS_DEBUG("Last trajectory point, setting special sequence value");
00109                         jMsg.point_.setSequence(SpecialSeqValues::END_TRAJECTORY);
00110                 }
00111                 else
00112                 {
00113                         jMsg.point_.setSequence(i);
00114                 }
00115 
00116                 // Copy position data to local variable
00117                 JointData data;
00118                 for (int j = 0; j < msg->joint_names.size(); j++)
00119                 {
00120                         pt = msg->points[j];
00121                         data.setJoint(j, pt.positions[j]);
00122                 }
00123 
00124                 // Initialize joint trajectory message
00125                 jPt.setJointPosition(data);
00126 
00127                 jMsg.init(jPt);
00128                 jMsg.toTopic(topic);
00129 
00130                 ROS_DEBUG("Sending joint trajectory point");
00131                 if (this->robot_->sendMsg(topic))
00132                 {
00133                         ROS_INFO("Point[%d] sent to controller", i);
00134                 }
00135                 else
00136                 {
00137                         ROS_WARN("Failed sent joint point, skipping point");
00138                 }
00139         }
00140 }
00141 
00142 } //joint_trajectory_handler
00143 } //motoman
00144 


adept_common
Author(s): root
autogenerated on Fri Jan 3 2014 11:05:39