00001 00008 #ifndef ROS_MSG_TREE_IK_TRAJECTORY_FACTORY_H 00009 #define ROS_MSG_TREE_IK_TRAJECTORY_FACTORY_H 00010 00011 #include <robodyn_controllers/TrajectoryFactory.h> 00012 #include <robodyn_controllers/RosMsgTrajectory.h> 00013 #include <robodyn_controllers/KdlTreeIk.h> 00014 #include <robodyn_controllers/KdlTreeTr.h> 00015 #include <robodyn_controllers/TreeIkTrajectory.h> 00016 00017 #include <r2_msgs/StringArray.h> 00018 00019 #include <stdexcept> 00020 #include "nasa_common_logging/Logger.h" 00021 #include "robodyn_utilities/RosMsgConverter.h" 00022 00023 class RosMsgTreeIkTrajectoryFactory 00024 { 00025 public: 00026 RosMsgTreeIkTrajectoryFactory() {} 00027 virtual ~RosMsgTreeIkTrajectoryFactory() {} 00028 00029 void setCartesianFactory(boost::shared_ptr<const SynchedCartesianTrajectoryFactory> factory_in) 00030 { 00031 factory = factory_in; 00032 sequenceFactory.reset(new sequenceFactoryType); 00033 sequenceFactory->setTrajectoryFactory(factory); 00034 } 00035 00036 void setTreeIk(boost::shared_ptr<KdlTreeIk> treeIk_in) 00037 { 00038 treeIk = treeIk_in; 00039 } 00040 00050 virtual boost::shared_ptr<RosMsgJointTrajectory> getTrajectory(const sensor_msgs::JointState& jointPositions, 00051 const sensor_msgs::JointState& jointVels, 00052 const sensor_msgs::JointState& prevJointVels, 00053 const r2_msgs::PoseState& poseState, 00054 const r2_msgs::PoseState& poseVels, 00055 const r2_msgs::PoseTrajectory& goalTraj) const; 00056 00057 protected: 00058 boost::shared_ptr<KdlTreeIk> treeIk; 00059 00060 typedef CartesianTrajectorySequenceFactory<SynchedTrajectory> sequenceFactoryType; 00061 boost::shared_ptr<sequenceFactoryType> sequenceFactory; 00062 boost::shared_ptr<const SynchedCartesianTrajectoryFactory> factory; 00063 }; 00064 00065 #endif