00001 #include "ros/ros.h"
00002 #include "sensor_msgs/JointState.h"
00003 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00004 #include "std_msgs/Float64.h"
00005
00006 ros::Publisher br_steer_pub;
00007 ros::Publisher bl_steer_pub;
00008 ros::Publisher fr_steer_pub;
00009 ros::Publisher fl_steer_pub;
00010
00011 ros::Publisher br_caster_pub;
00012 ros::Publisher bl_caster_pub;
00013 ros::Publisher fr_caster_pub;
00014 ros::Publisher fl_caster_pub;
00015
00016
00017 void velCallback(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg)
00018 {
00019 if(msg->desired.velocities.size() != 8)
00020 return;
00021 std_msgs::Float64 fl;
00022 for(unsigned int i = 0; i < 8; i++)
00023 {
00024 fl.data = msg->desired.velocities[i];
00025 if(msg->joint_names[i] == "fl_caster_r_wheel_joint")
00026 fl_caster_pub.publish(fl);
00027 if(msg->joint_names[i] == "fr_caster_r_wheel_joint")
00028 fr_caster_pub.publish(fl);
00029 if(msg->joint_names[i] == "bl_caster_r_wheel_joint")
00030 bl_caster_pub.publish(fl);
00031 if(msg->joint_names[i] == "br_caster_r_wheel_joint")
00032 br_caster_pub.publish(fl);
00033
00034 if(msg->joint_names[i] == "fl_caster_rotation_joint")
00035 fl_steer_pub.publish(fl);
00036 if(msg->joint_names[i] == "fr_caster_rotation_joint")
00037 fr_steer_pub.publish(fl);
00038 if(msg->joint_names[i] == "bl_caster_rotation_joint")
00039 bl_steer_pub.publish(fl);
00040 if(msg->joint_names[i] == "br_caster_rotation_joint")
00041 br_steer_pub.publish(fl);
00042 }
00043 }
00044
00045 int main(int argc, char **argv)
00046 {
00047 ros::init(argc, argv, "base_drive_chain_sim");
00048 ros::NodeHandle n;
00049 bl_caster_pub = n.advertise<std_msgs::Float64>("/base_bl_caster_r_wheel_controller/command", 1);
00050 br_caster_pub = n.advertise<std_msgs::Float64>("/base_br_caster_r_wheel_controller/command", 1);
00051 fl_caster_pub = n.advertise<std_msgs::Float64>("/base_fl_caster_r_wheel_controller/command", 1);
00052 fr_caster_pub = n.advertise<std_msgs::Float64>("/base_fr_caster_r_wheel_controller/command", 1);
00053
00054 bl_steer_pub = n.advertise<std_msgs::Float64>("/base_bl_caster_rotation_controller/command", 1);
00055 br_steer_pub = n.advertise<std_msgs::Float64>("/base_br_caster_rotation_controller/command", 1);
00056 fl_steer_pub = n.advertise<std_msgs::Float64>("/base_fl_caster_rotation_controller/command", 1);
00057 fr_steer_pub = n.advertise<std_msgs::Float64>("/base_fr_caster_rotation_controller/command", 1);
00058
00059
00060 ros::Subscriber sub = n.subscribe("joint_command", 1, velCallback);
00061 ros::spin();
00062
00063 return 0;
00064 }
