JointTrajectoryAction.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Robot Control and Pattern Recognition Group, Warsaw University of Technology.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Robot Control and Pattern Recognition Group,
00014  *       Warsaw University of Technology nor the names of its contributors may
00015  *       be used to endorse or promote products derived from this software
00016  *       without specific prior written permission.
00017  *
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 /*
00032  * JointTrajectoryAction.cpp
00033  *
00034  *  Created on: 23-09-2010
00035  *      Author: Konrad Banachowicz
00036  */
00037 
00038 #include <ocl/Component.hpp>
00039 
00040 #include "JointTrajectoryAction.h"
00041 
00042 JointTrajectoryAction::JointTrajectoryAction(const std::string& name) :
00043   RTT::TaskContext(name, PreOperational), trajectoryPoint_port("trajectory_point"), bufferReady_port("buffer_ready"),
00044       numberOfJoints_prop("number_of_joints", "", 0), command_port_("command"),
00045       trajectoryCompleat_port("trajectory_compleat"), as(this, "JointTrajectoryAction",
00046                                                          boost::bind(&JointTrajectoryAction::goalCB, this, _1),
00047                                                          boost::bind(&JointTrajectoryAction::cancelCB, this, _1), true)
00048 {
00049 
00050   this->addPort(trajectoryPoint_port);
00051   this->addEventPort(bufferReady_port, boost::bind(&JointTrajectoryAction::bufferReadyCB, this));
00052   this->addEventPort(command_port_, boost::bind(&JointTrajectoryAction::commandCB, this));
00053   this->addEventPort(trajectoryCompleat_port, boost::bind(&JointTrajectoryAction::compleatCB, this));
00054   this->addProperty(numberOfJoints_prop);
00055 
00056 }
00057 
00058 JointTrajectoryAction::~JointTrajectoryAction()
00059 {
00060 
00061 }
00062 
00063 bool JointTrajectoryAction::configureHook()
00064 {
00065   if ((numberOfJoints = numberOfJoints_prop.get()) == 0)
00066   {
00067     return false;
00068   }
00069 
00070   jointNames.resize(numberOfJoints);
00071 
00072   for (unsigned int i = 0; i < numberOfJoints; i++)
00073   {
00074     jointNames[i]
00075         = ((RTT::Property<std::string>*)this->getProperty(std::string("joint") + (char)(i + 48) + "_name"))->get();
00076   }
00077 
00078   return true;
00079 }
00080 
00081 bool JointTrajectoryAction::startHook()
00082 {
00083   goal_active = false;
00084   enable = true;
00085   return true;
00086 }
00087 
00088 void JointTrajectoryAction::updateHook()
00089 {
00090   if(enable)
00091     as.spinOnce();
00092     
00093   enable = true;
00094 }
00095 
00096 void JointTrajectoryAction::goalCB(GoalHandle gh)
00097 {
00098   if (!goal_active)
00099   {
00100     std::vector<int> remapTable;
00101     remapTable.resize(numberOfJoints);
00102 
00103     Goal g = gh.getGoal();
00104 
00105     RTT::Logger::log(RTT::Logger::Debug) << "Received trajectory contain " << g->trajectory.points.size() << " points"
00106         << RTT::endlog();
00107 
00108     // fill remap table
00109     for (unsigned int i = 0; i < numberOfJoints; i++)
00110     {
00111       int jointId = -1;
00112       for (unsigned int j = 0; j < g->trajectory.joint_names.size(); j++)
00113       {
00114         if (g->trajectory.joint_names[j] == jointNames[i])
00115         {
00116           jointId = j;
00117           break;
00118         }
00119       }
00120       if (jointId < 0)
00121       {
00122         RTT::Logger::log(RTT::Logger::Error) << "Trajectory contains invalid joint" << RTT::endlog();
00123         gh.setRejected();
00124         return;
00125       }
00126       else
00127       {
00128         remapTable[i] = jointId;
00129       }
00130 
00131     }
00132 
00133     //remap joints
00134 
00135     trajectory.resize(g->trajectory.points.size());
00136 
00137     for (unsigned int i = 0; i < g->trajectory.points.size(); i++)
00138     {
00139       trajectory[i].positions.resize(g->trajectory.points[i].positions.size());
00140       for (unsigned int j = 0; j < g->trajectory.points[i].positions.size(); j++)
00141       {
00142         trajectory[i].positions[j] = g->trajectory.points[i].positions[remapTable[j]];
00143       }
00144 
00145       trajectory[i].velocities.resize(g->trajectory.points[i].velocities.size());
00146       for (unsigned int j = 0; j < g->trajectory.points[i].velocities.size(); j++)
00147       {
00148         trajectory[i].velocities[j] = g->trajectory.points[i].velocities[remapTable[j]];
00149       }
00150 
00151       trajectory[i].accelerations.resize(g->trajectory.points[i].accelerations.size());
00152       for (unsigned int j = 0; j < g->trajectory.points[i].accelerations.size(); j++)
00153       {
00154         trajectory[i].accelerations[j] = g->trajectory.points[i].accelerations[remapTable[j]];
00155       }
00156 
00157       if (i == 0)
00158       {
00159         trajectory[i].time_from_start = g->trajectory.points[i].time_from_start;
00160       }
00161       else
00162       {
00163         trajectory[i].time_from_start = g->trajectory.points[i].time_from_start
00164             - g->trajectory.points[i - 1].time_from_start;
00165       }
00166     }
00167 
00168     endPoint = g->trajectory.points.size();
00169     currentPoint = 0;
00170 
00171     activeGoal = gh;
00172     goal_active = true;
00173     
00174     bool ok = true;
00175     
00176     RTT::TaskContext::PeerList peers = this->getPeerList();
00177     for(size_t i = 0; i < peers.size(); i++)
00178     {
00179       RTT::Logger::log(RTT::Logger::Debug) << "Starting peer : " << peers[i] << RTT::endlog();
00180       ok = ok && this->getPeer(peers[i])->start();
00181     }
00182     
00183     if(ok)
00184     {
00185       gh.setAccepted();
00186     } else
00187     {
00188       gh.setRejected();
00189       goal_active = false;
00190     }
00191   }
00192   else
00193   {
00194     gh.setRejected();
00195   }
00196 }
00197 
00198 void JointTrajectoryAction::cancelCB(GoalHandle gh)
00199 {
00200   goal_active = false;
00201 }
00202 
00203 void JointTrajectoryAction::commandCB()
00204 {
00205   trajectory_msgs::JointTrajectory trj;
00206   if ((command_port_.read(trj) == RTT::NewData)&&(!goal_active))
00207   {
00208     std::vector<int> remapTable;
00209     remapTable.resize(numberOfJoints);
00210 
00211     RTT::Logger::log(RTT::Logger::Debug) << "Received trajectory contain " << trj.points.size() << " points"
00212         << RTT::endlog();
00213 
00214     // fill remap table
00215     for (unsigned int i = 0; i < numberOfJoints; i++)
00216     {
00217       int jointId = -1;
00218       for (unsigned int j = 0; j < trj.joint_names.size(); j++)
00219       {
00220         if (trj.joint_names[j] == jointNames[i])
00221         {
00222           jointId = j;
00223           break;
00224         }
00225       }
00226       if (jointId < 0)
00227       {
00228         RTT::Logger::log(RTT::Logger::Error) << "Trajectory contains invalid joint" << RTT::endlog();
00229         return;
00230       }
00231       else
00232       {
00233         remapTable[i] = jointId;
00234       }
00235 
00236     }
00237 
00238     //remap joints
00239 
00240     trajectory.resize(trj.points.size());
00241 
00242     for (unsigned int i = 0; i < trj.points.size(); i++)
00243     {
00244       trajectory[i].positions.resize(trj.points[i].positions.size());
00245       for (unsigned int j = 0; j < trj.points[i].positions.size(); j++)
00246       {
00247         trajectory[i].positions[j] = trj.points[i].positions[remapTable[j]];
00248       }
00249 
00250       trajectory[i].velocities.resize(trj.points[i].velocities.size());
00251       for (unsigned int j = 0; j < trj.points[i].velocities.size(); j++)
00252       {
00253         trajectory[i].velocities[j] = trj.points[i].velocities[remapTable[j]];
00254       }
00255 
00256       trajectory[i].accelerations.resize(trj.points[i].accelerations.size());
00257       for (unsigned int j = 0; j < trj.points[i].accelerations.size(); j++)
00258       {
00259         trajectory[i].accelerations[j] = trj.points[i].accelerations[remapTable[j]];
00260       }
00261 
00262       if (i == 0)
00263       {
00264         trajectory[i].time_from_start = trj.points[i].time_from_start;
00265       }
00266       else
00267       {
00268         trajectory[i].time_from_start = trj.points[i].time_from_start
00269             - trj.points[i - 1].time_from_start;
00270       }
00271     }
00272 
00273     endPoint = trj.points.size();
00274     currentPoint = 0;
00275 
00276     goal_active = true;
00277   }
00278 
00279 }
00280 
00281 void JointTrajectoryAction::compleatCB()
00282 {
00283   if (goal_active)
00284   {
00285     activeGoal.setSucceeded();
00286     goal_active = false;
00287     
00288     RTT::TaskContext::PeerList peers = this->getPeerList();
00289     for(size_t i = 0; i < peers.size(); i++)
00290     {
00291       RTT::Logger::log(RTT::Logger::Debug) << "Stoping peer : " << peers[i] << RTT::endlog();
00292       this->getPeer(peers[i])->stop();
00293     }
00294     RTT::Logger::log(RTT::Logger::Debug) << "Trajectory complete" << RTT::endlog();
00295   }
00296 }
00297 
00298 void JointTrajectoryAction::bufferReadyCB()
00299 {
00300   enable = false;
00301 
00302   bool tmp;
00303   if (bufferReady_port.read(tmp) == RTT::NewData)
00304   {
00305     if (tmp && goal_active)
00306     {
00307       if (currentPoint < endPoint)
00308       {
00309         while(trajectory[currentPoint].time_from_start.toSec() < 0.01)
00310           if(!(++currentPoint < endPoint))
00311             return;
00312         RTT::Logger::log(RTT::Logger::Debug) << "Sending new point tmp = " << tmp << " goal_active = " << goal_active << " currentPoint = " << currentPoint << RTT::endlog();
00313         trajectoryPoint_port.write(trajectory[currentPoint]);
00314         ++currentPoint;
00315       }
00316     }
00317   }
00318 }
00319 
00320 ORO_CREATE_COMPONENT( JointTrajectoryAction )


oro_joint_trajectory_action
Author(s): Konrad Banachowicz
autogenerated on Mon Oct 6 2014 03:10:05