Go to the documentation of this file.00001 #ifndef WALK_MSGS_ABSTRACT_NODE_HH
00002 # define WALK_MSGS_ABSTRACT_NODE_HH
00003 # include <string>
00004 # include <vector>
00005
00006 # include <ros/ros.h>
00007 # include <walk_interfaces/pattern-generator.hh>
00008
00009 # include <nav_msgs/Path.h>
00010 # include <visualization_msgs/MarkerArray.h>
00011
00012 namespace walk_msgs
00013 {
00030 template <typename T, typename U, typename S>
00031 class AbstractNode
00032 {
00033 public:
00035 typedef T patternGenerator_t;
00037 typedef U footprintRosType_t;
00038
00040 typedef S serviceRosType_t;
00041
00047 explicit AbstractNode (const std::string& rosNamespace,
00048 const std::string& frameWorldId,
00049 bool enableService = true);
00050
00062 explicit AbstractNode (const std::string& rosNamespace,
00063 const std::string& frameWorldId,
00064 const patternGenerator_t& pg,
00065 bool enableService = true);
00066
00068 ~AbstractNode ();
00069
00071 void spin();
00072
00073 protected:
00083 bool getPath(typename serviceRosType_t::Request& req,
00084 typename serviceRosType_t::Response& res);
00085
00090 virtual void convertFootprint
00091 (typename patternGenerator_t::footprints_t& dst,
00092 const std::vector<footprintRosType_t>& src) = 0;
00093
00105 virtual void
00106 setupPatternGenerator (typename serviceRosType_t::Request& req) = 0;
00107
00109 patternGenerator_t& patternGenerator ()
00110 {
00111 return patternGenerator_;
00112 }
00113
00115 void prepareTopicsData (typename serviceRosType_t::Response& res,
00116 bool startWithLeftFoot);
00117
00120 void writeMotionAsParameter ();
00121
00122 private:
00124 ros::NodeHandle nodeHandle_;
00125
00127 ros::Rate rate_;
00128
00130 ros::ServiceServer getPathSrv_;
00131
00135 std::string frameName_;
00136
00138 std::string parameterName_;
00139
00142 patternGenerator_t patternGenerator_;
00143
00145 visualization_msgs::MarkerArray footprints_;
00146
00148 nav_msgs::Path leftFootPath_;
00150 nav_msgs::Path rightFootPath_;
00151
00153 nav_msgs::Path comPath_;
00155 nav_msgs::Path zmpPath_;
00156
00158 ros::Publisher footprintsPub_;
00160 ros::Publisher leftFootPub_;
00162 ros::Publisher rightFootPub_;
00164 ros::Publisher comPub_;
00166 ros::Publisher zmpPub_;
00167
00169 double footPrintWidth_;
00171 double footPrintHeight_;
00172 };
00173
00174 }
00175
00176 # include <walk_msgs/abstract-node.hxx>
00177 #endif //! WALK_MSGS_ABSTRACT_NODE_HH