Go to the documentation of this file.00001 #ifndef TRAJECTORYMONITOR_H
00002 #define TRAJECTORYMONITOR_H
00003
00004 #include "robodyn_utilities/GoalStatusSender.h"
00005 #include <sensor_msgs/JointState.h>
00006 #include <r2_msgs/JointTrajectoryReplan.h>
00007 #include "r2_msgs/PoseTrajectoryReplan.h"
00008 #include "r2_msgs/PoseState.h"
00009 #include "r2_msgs/StringArray.h"
00010 #include "r2_msgs/JointControlDataArray.h"
00011 #include "r2_msgs/ReplanType.h"
00012 #include "r2_msgs/JointCommand.h"
00013 #include "r2_msgs/JointCapability.h"
00014 #include "r2_msgs/TrajectoryMonitorFactors.h"
00015 #include "nasa_common_logging/Logger.h"
00016 #include "robodyn_utilities/RosMsgConverter.h"
00017 #include "robot_instance/StringUtilities.h"
00018 #include <tf_conversions/tf_kdl.h>
00019
00023 class TrajectoryMonitor : public GoalStatusSender
00024 {
00025 public:
00026 TrajectoryMonitor(const std::string& name);
00027 virtual ~TrajectoryMonitor();
00028
00029 void setBases(const r2_msgs::StringArray& basesMsg);
00030
00031 void setFactors(const r2_msgs::TrajectoryMonitorFactors& factorsMsg);
00032
00036 void evaluateReplanTrigger(const sensor_msgs::JointState& jointStatesMsg, const r2_msgs::JointCommand& jointCommandMsg,
00037 const r2_msgs::JointControlDataArray& jointStatusMsg, const r2_msgs::JointCapability& jointCapMsg);
00038
00042 void replan();
00043
00047 void updateMonitor(const r2_msgs::JointCommand& jointCommandMsg,
00048 const r2_msgs::PoseState& poseCommandsMsg);
00052 void updateGoals(const actionlib_msgs::GoalStatusArray& goalStatuses);
00053
00054 void addCartesianWaypoints(r2_msgs::PoseTrajectory trajectory, const r2_msgs::PoseState& poseStatesMsg);
00055 void addJointWaypoints(const trajectory_msgs::JointTrajectory& trajectory,
00056 const std::vector<r2_msgs::ReplanType>& replan = std::vector<r2_msgs::ReplanType>());
00057
00058 protected:
00059 virtual void writeCartesianWaypoints(const r2_msgs::PoseTrajectoryReplan& trajectory) = 0;
00060 virtual void writeJointWaypoints(const r2_msgs::JointTrajectoryReplan& trajectory) = 0;
00061 virtual void writeBases(const r2_msgs::StringArray& basesMsg) = 0;
00062
00063 struct TrajectoryMonitorFactors
00064 {
00065 double torqueLimitFactor;
00066 double distanceFactor;
00067 };
00068
00069 TrajectoryMonitorFactors defaultFactors;
00070 std::map<std::string, TrajectoryMonitorFactors> factors;
00071
00072 std::string embeddedSuffix;
00073
00074 private:
00075 struct GoalIdCompare : public std::binary_function<actionlib_msgs::GoalID, actionlib_msgs::GoalID, bool>
00076 {
00077 bool operator()(const actionlib_msgs::GoalID& left, const actionlib_msgs::GoalID& right) const
00078 {
00079 return (left.stamp < right.stamp || (left.stamp == right.stamp && left.id < right.id));
00080 }
00081 };
00082
00083 std::vector<std::string> bases;
00084
00085 struct JointData
00086 {
00087 JointData()
00088 {
00089 statusValid = true;
00090 positionValid = false;
00091 commandValid = false;
00092 embeddedCommand = false;
00093 actualPosition = 0.;
00094 actualVelocity = 0.;
00095 commandPosition = 0.;
00096 commandVelocity = 0.;
00097 torqueLimitCommand = 0.;
00098 replan.type = r2_msgs::ReplanType::NONE;
00099 }
00100
00101 bool statusValid;
00102 bool positionValid;
00103 bool commandValid;
00104 bool embeddedCommand;
00105 double actualPosition;
00106 double actualVelocity;
00107 double commandPosition;
00108 double commandVelocity;
00109 double torqueLimitCommand;
00110 r2_msgs::ReplanType replan;
00111 };
00112
00113 std::map<std::string, JointData> jointData;
00114 std::map<std::string, JointData>::iterator jointDataIt;
00115 trajectory_msgs::JointTrajectory replanTraj;
00116 std::vector<r2_msgs::ReplanType> replanVec;
00117
00118
00119 typedef std::map<actionlib_msgs::GoalID, r2_msgs::PoseTrajectory, GoalIdCompare> PoseTrajectoriesMap_type;
00120 PoseTrajectoriesMap_type poseTrajectories;
00121 typedef std::map<actionlib_msgs::GoalID, trajectory_msgs::JointTrajectory, GoalIdCompare> JointTrajectoriesMap_type;
00122 JointTrajectoriesMap_type jointTrajectories;
00123
00124 void sendCartesianWaypoints(const r2_msgs::PoseTrajectory& trajectory,
00125 const r2_msgs::ReplanType::_type_type& replan = r2_msgs::ReplanType::NONE);
00126 void sendJointWaypoints(const trajectory_msgs::JointTrajectory& trajectory,
00127 const std::vector<r2_msgs::ReplanType>& replan = std::vector<r2_msgs::ReplanType>());
00128 };
00129
00130 #endif