00001 #include <robodyn_controllers/RosMsgTrajectory.h> 00002 #include "nasa_common_logging/Logger.h" 00003 #include <tf_conversions/tf_kdl.h> 00004 00005 void RosMsgJointTrajectory::getPose(double time, trajectory_msgs::JointTrajectoryPoint& pose) 00006 { 00007 static KDL::JntArrayAcc jntPose; 00008 trajectory->getPose(time, jntPose); 00009 pose.positions.resize(jntPose.q.rows()); 00010 pose.velocities.resize(jntPose.qdot.rows()); 00011 pose.accelerations.resize(jntPose.qdotdot.rows()); 00012 00013 for (unsigned int i = 0; i < jntPose.q.rows(); ++i) 00014 { 00015 pose.positions[i] = jntPose.q(i); 00016 pose.velocities[i] = jntPose.qdot(i); 00017 pose.accelerations[i] = jntPose.qdotdot(i); 00018 } 00019 00020 pose.time_from_start = ros::Duration(time); 00021 } 00022 00023 void RosMsgSynchedCartesianTrajectory::getPose(double time, r2_msgs::PoseTrajectoryPoint& pose) 00024 { 00025 static SynchedCartesianTrajectory::poseType tmpCartPose; 00026 static geometry_msgs::Pose tmpPose; 00027 pose.positions.clear(); 00028 pose.velocities.clear(); 00029 pose.accelerations.clear(); 00030 00031 trajectory->getPose(time, tmpCartPose); 00032 00033 for (SynchedCartesianTrajectory::poseType::const_iterator it = tmpCartPose.begin(); it != tmpCartPose.end(); ++it) 00034 { 00035 tf::poseKDLToMsg(it->GetFrame(), tmpPose); 00036 pose.positions.push_back(tmpPose); 00037 } 00038 00039 pose.time_from_start = ros::Duration(time); 00040 } 00041