Go to the documentation of this file.00001
00026 #include <ros/ros.h>
00027 #include <geometry_msgs/Twist.h>
00028 #include <grizzly_msgs/Drive.h>
00029
00034 class MotionGenerator
00035 {
00036 public:
00037 MotionGenerator() : nh_("")
00038 {
00039 ros::param::get("vehicle_width", width_);
00040 ros::param::get("wheel_radius", radius_);
00041
00042 pub_ = nh_.advertise<grizzly_msgs::Drive>("cmd_drive", 1);
00043 sub_ = nh_.subscribe("cmd_vel", 1, &MotionGenerator::twist_callback, this);
00044 }
00045
00046 protected:
00047 void twist_callback(const geometry_msgs::TwistConstPtr&);
00048
00049 ros::NodeHandle nh_;
00050 ros::Publisher pub_;
00051 ros::Subscriber sub_;
00052 double width_, radius_;
00053 };
00054
00060 void MotionGenerator::twist_callback(const geometry_msgs::TwistConstPtr& twist)
00061 {
00062 double right_speed = twist->linear.x + twist->angular.z * (width_ / 2);
00063 double left_speed = twist->linear.x - twist->angular.z * (width_ / 2);
00064
00065 grizzly_msgs::Drive drive;
00066 drive.header.stamp = ros::Time::now();
00067 drive.front_left = drive.rear_left = left_speed / radius_;
00068 drive.front_right = drive.rear_right = right_speed / radius_;
00069 pub_.publish(drive);
00070 }
00071
00075 int main (int argc, char ** argv)
00076 {
00077 ros::init(argc, argv, "grizzly_motion_generator");
00078 MotionGenerator mg;
00079 ros::spin();
00080 }