00001 #ifndef ROSMSGFORCETRAJECTORYFACTORY_H 00002 #define ROSMSGFORCETRAJECTORYFACTORY_H 00003 00004 #include <stdexcept> 00005 00006 #include "robodyn_controllers/TrajectoryFactory.h" 00007 #include "robodyn_controllers/RosMsgTrajectory.h" 00008 #include "robodyn_controllers/KdlTreeIk.h" 00009 #include "robodyn_controllers/ForceTrajectory.h" 00010 00011 #include "r2_msgs/ForceControlAxisArray.h" 00012 #include "r2_msgs/StringArray.h" 00013 #include "r2_msgs/ForceControlAxis.h" 00014 00015 #include "nasa_common_logging/Logger.h" 00016 #include "robodyn_utilities/RosMsgConverter.h" 00017 00018 class RosMsgForceTrajectoryFactory 00019 { 00020 public: 00021 RosMsgForceTrajectoryFactory(); 00022 virtual ~RosMsgForceTrajectoryFactory() {} 00023 00024 void setCartesianFactory(boost::shared_ptr<const SynchedCartesianTrajectoryFactory> factory_in) 00025 { 00026 factory = factory_in; 00027 sequenceFactory.reset(new sequenceFactoryType); 00028 sequenceFactory->setTrajectoryFactory(factory); 00029 } 00030 00031 void setTreeIk(boost::shared_ptr<KdlTreeIk> treeIk_in) 00032 { 00033 treeIk = treeIk_in; 00034 } 00035 00036 /********************************************************************************/ 00037 void setForceController(boost::shared_ptr<Cartesian_HybCntrl> forceController_in) 00038 { 00039 forceController = forceController_in; 00040 } 00041 /********************************************************************************/ 00042 00052 virtual boost::shared_ptr<RosMsgJointTrajectory> getTrajectory(const sensor_msgs::JointState& jointPositions, 00053 const sensor_msgs::JointState& jointVels, 00054 const sensor_msgs::JointState& prevJointVels, 00055 const r2_msgs::PoseState& poseState, 00056 const r2_msgs::PoseState& poseVels, 00057 const r2_msgs::PoseTrajectory& goalTraj, 00058 const r2_msgs::ForceControlAxisArray& forceControlAxes) const; 00059 00060 protected: 00061 boost::shared_ptr<KdlTreeIk> treeIk; 00062 /**************************************************/ 00063 boost::shared_ptr<Cartesian_HybCntrl> forceController; 00064 /**************************************************/ 00065 typedef CartesianTrajectorySequenceFactory<SynchedTrajectory> sequenceFactoryType; 00066 boost::shared_ptr<sequenceFactoryType> sequenceFactory; 00067 boost::shared_ptr<const SynchedCartesianTrajectoryFactory> factory; 00068 private: 00069 const std::string logCategory; 00070 }; 00071 00072 #endif // ROSMSGFORCETRAJECTORYFACTORY_H