dead_reckoning_node.cpp
Go to the documentation of this file.
00001 
00026 #include "ros/ros.h"
00027 #include "grizzly_motion/dead_reckoning.h"
00028 
00029 void encodersCallback(DeadReckoning* dr, ros::Publisher* pub_odom, 
00030   ros::Publisher* pub_joints,  const grizzly_msgs::DriveConstPtr encoders)
00031 {
00032   nav_msgs::Odometry odom;
00033   sensor_msgs::JointState joints;
00034   if (dr->next(encoders, &odom, &joints)) {
00035     pub_odom->publish(odom);
00036     pub_joints->publish(joints);
00037   }
00038 }
00039 
00043 int main(int argc, char ** argv)
00044 {
00045   double vehicle_width, wheel_radius;
00046 
00047   ros::init(argc, argv, "grizzly_dead_reckoning"); 
00048   ros::param::get("vehicle_width", vehicle_width);
00049   ros::param::get("wheel_radius", wheel_radius);
00050 
00051   DeadReckoning dr(vehicle_width, wheel_radius);
00052   ros::NodeHandle nh("");
00053   ros::Publisher pub_odom(nh.advertise<nav_msgs::Odometry>("odom", 1));
00054   ros::Publisher pub_joints(nh.advertise<sensor_msgs::JointState>("joint_states",1));
00055   ros::Subscriber sub(nh.subscribe<grizzly_msgs::Drive>("motors/encoders", 1, boost::bind(
00056     encodersCallback, &dr, &pub_odom, &pub_joints, _1)));
00057 
00058   ros::spin();
00059 }


grizzly_motion
Author(s):
autogenerated on Thu Jun 6 2019 21:44:02