motion_generator_node.cpp
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 }


grizzly_motion
Author(s):
autogenerated on Fri Aug 28 2015 10:55:30