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 }