00001 #include <sstream>
00002 #include <stdexcept>
00003 #include <boost/bind.hpp>
00004 #include <boost/date_time.hpp>
00005
00006 #include <Eigen/LU>
00007
00008 #include <LinearMath/btMatrix3x3.h>
00009 #include <LinearMath/btQuaternion.h>
00010
00011 #include <ros/ros.h>
00012
00013 #include <walk_interfaces/yaml.hh>
00014
00015 #include "geometry_msgs/Pose.h"
00016 #include "visualization_msgs/MarkerArray.h"
00017 #include "walk_msgs/Footprint2d.h"
00018 #include "walk_msgs/GetPath.h"
00019
00020 #include <walk_msgs/conversion.hh>
00021
00022 #include <walk_msgs/abstract-node.hh>
00023
00024 using walk::HomogeneousMatrix3d;
00025 using walk::Posture;
00026
00027 using walk_msgs::convertPoseToHomogeneousMatrix;
00028 using walk_msgs::convertPointToVector3d;
00029 using walk_msgs::convertTrajectoryToPath;
00030 using walk_msgs::convertTrajectoryV3dToPath;
00031 using walk_msgs::convertTrajectoryV2dToPath;
00032
00033 std::string getParam (const std::string& param,
00034 const std::string& defaultValue)
00035 {
00036 std::string result;
00037 ros::param::param(param, result, defaultValue);
00038 return result;
00039 }
00040
00041 class GeneratorYamlNode :
00042 public walk_msgs::AbstractNode<
00043 walk::YamlReader<walk::DiscretizedPatternGenerator2d>,
00044 walk_msgs::Footprint2d,
00045 walk_msgs::GetPath>
00046 {
00047 public:
00048 explicit GeneratorYamlNode ();
00049 ~GeneratorYamlNode ();
00050
00051 virtual void convertFootprint
00052 (patternGenerator_t::footprints_t& dst,
00053 const std::vector<footprintRosType_t>& src);
00054
00055 virtual void
00056 setupPatternGenerator (walk_msgs::GetPath::Request& req);
00057 };
00058
00059 GeneratorYamlNode::GeneratorYamlNode ()
00060 : walk_msgs::AbstractNode<
00061 walk::YamlReader<walk::DiscretizedPatternGenerator2d>,
00062 walk_msgs::Footprint2d,
00063 walk_msgs::GetPath>
00064 ("", getParam ("~world_frame_id", "/world"),
00065 walk::YamlReader<walk::DiscretizedPatternGenerator2d>
00066 (getParam ("~yaml", "")),
00067 false)
00068 {
00069 walk_msgs::GetPath::Response res;
00070 prepareTopicsData (res, patternGenerator().startWithLeftFoot());
00071 writeMotionAsParameter ();
00072 ROS_INFO ("motion parsed succesfully, "
00073 "starting to publish reference trajectories...");
00074 }
00075
00076 GeneratorYamlNode::~GeneratorYamlNode ()
00077 {}
00078
00079 void
00080 GeneratorYamlNode::convertFootprint (patternGenerator_t::footprints_t& dst,
00081 const std::vector<footprintRosType_t>& src)
00082 {
00083 using boost::posix_time::seconds;
00084 using boost::posix_time::milliseconds;
00085
00086 dst.clear();
00087 std::vector<walk_msgs::Footprint2d>::const_iterator it = src.begin();
00088 for (; it != src.end(); ++it)
00089 {
00090 patternGenerator_t::footprint_t footprint;
00091 footprint.beginTime = (it->beginTime).toBoost();
00092 footprint.duration =
00093 seconds(it->duration.sec) + milliseconds(it->duration.nsec * 1000);
00094 footprint.position(0) = it->x;
00095 footprint.position(1) = it->y;
00096 footprint.position(2) = it->theta;
00097 dst.push_back(footprint);
00098 }
00099 }
00100
00101 void
00102 GeneratorYamlNode::setupPatternGenerator (walk_msgs::GetPath::Request& req)
00103 {
00104 }
00105
00106
00107 int main(int argc, char **argv)
00108 {
00109 try
00110 {
00111 ros::init(argc, argv, "walk_msgs");
00112
00113 if (getParam ("~yaml", "").empty ())
00114 {
00115 ROS_FATAL_STREAM
00116 ("Failed to start node, missing YAML motion.\n"
00117 << "Please provide a motion to load:\n"
00118 << "rosrun walk_msgs " << argv[0] << " _yaml:=/path/to/motion.yaml");
00119 return 1;
00120 }
00121
00122 GeneratorYamlNode node;
00123 if (ros::ok())
00124 node.spin();
00125 }
00126 catch (std::exception& e)
00127 {
00128 std::cerr << "fatal error: " << e.what() << std::endl;
00129 ROS_ERROR_STREAM("fatal error: " << e.what());
00130 return 1;
00131 }
00132 catch (...)
00133 {
00134 ROS_ERROR_STREAM("unexpected error");
00135 return 2;
00136 }
00137 return 0;
00138 }
00139
