gazebo_control.cpp
Go to the documentation of this file.
00001 #include "ros/ros.h"
00002 #include <stdio.h>
00003 
00004 #include "std_msgs/Float64.h"
00005 #include "std_msgs/Float32.h"
00006 #include "corobot_msgs/MotorCommand.h"
00007 #include "corobot_gazebo/ModelStates.h"
00008 #include "nav_msgs/Odometry.h"
00009 #include <tf/transform_broadcaster.h>
00010 #include "math.h"
00011 
00012 ros::Publisher left_rear_wheel_pub, right_rear_wheel_pub,left_front_wheel_pub, right_front_wheel_pub, odom_pub;
00013 
00014 void setVelocity(const corobot_msgs::MotorCommand &msg)
00015 {
00016         std_msgs::Float64 msg_left, msg_right;
00017         msg_left.data = msg.leftSpeed/3;
00018         msg_right.data = msg.rightSpeed/3;
00019 
00020         if(left_rear_wheel_pub)
00021                 left_rear_wheel_pub.publish(msg_left);
00022         if(right_rear_wheel_pub)
00023                 right_rear_wheel_pub.publish(msg_right);
00024         if(left_front_wheel_pub)
00025                 left_front_wheel_pub.publish(msg_left);
00026         if(right_front_wheel_pub)
00027                 right_front_wheel_pub.publish(msg_right);
00028 }
00029 
00030 void setLeftWheelVelocity(const std_msgs::Float32 &msg)
00031 {
00032         std_msgs::Float64 msg_left;
00033         msg_left.data = msg.data / (0.105*M_PI / (2*M_PI));// the msg data is a value in meter per second but the gazebo command takes a value of radian per second for the motor. This is just the convertion.
00034 
00035         if(left_rear_wheel_pub)
00036                 left_rear_wheel_pub.publish(msg_left);
00037         if(left_front_wheel_pub)
00038                 left_front_wheel_pub.publish(msg_left);
00039 }
00040 
00041 void setRightWheelVelocity(const std_msgs::Float32 &msg)
00042 {
00043         std_msgs::Float64 msg_right;
00044         msg_right.data = msg.data / (0.105*M_PI / (2*M_PI));// the msg data is a value in meter per second but the gazebo command takes a value of radian per second for the motor. This is just the convertion.
00045 
00046         if(right_rear_wheel_pub)
00047                 right_rear_wheel_pub.publish(msg_right);
00048         if(right_front_wheel_pub)
00049                 right_front_wheel_pub.publish(msg_right);
00050 }
00051 
00052 void getModelState(const corobot_gazebo::ModelStates &msg)
00053 {
00054         //get the gazebo model state and publish odometry and tf instead
00055         nav_msgs::Odometry odom;
00056         static tf::TransformBroadcaster broadcaster;
00057 
00058         odom.header.stamp = ros::Time::now();
00059         odom.header.frame_id = "odom";
00060 
00061 
00062         //first, we'll publish the transform over tf
00063         geometry_msgs::TransformStamped odom_trans;
00064         odom_trans.header.stamp = ros::Time::now();
00065         odom_trans.header.frame_id = "odom";
00066         odom_trans.child_frame_id = "base_footprint";
00067 
00068         odom_trans.transform.translation.x = msg.pose[1].position.x;
00069         odom_trans.transform.translation.y = msg.pose[1].position.y;
00070         odom_trans.transform.translation.z = msg.pose[1].position.z;
00071         odom_trans.transform.rotation = msg.pose[1].orientation;
00072 
00073         geometry_msgs::TransformStamped odom_trans2;
00074         odom_trans2.header.stamp = ros::Time::now();
00075         odom_trans2.header.frame_id = "base_footprint";
00076         odom_trans2.child_frame_id = "base_link";
00077 
00078         odom_trans2.transform.translation.x = 0;
00079         odom_trans2.transform.translation.y = 0;
00080         odom_trans2.transform.translation.z = 0.0;
00081         odom_trans2.transform.rotation = tf::createQuaternionMsgFromYaw(0);
00082 
00083         broadcaster.sendTransform(odom_trans);
00084         broadcaster.sendTransform(odom_trans2);
00085         
00086 
00087         //Publish Odometry
00088         odom.pose.pose = msg.pose[1];
00089         //These covariance values are "random". Some better values could be found after experiments and calculations
00090         odom.pose.covariance[0] = 0.01;
00091         odom.pose.covariance[7] = 0.01;
00092         odom.pose.covariance[14] = 0.01;
00093         odom.pose.covariance[21] = 0.01;
00094         odom.pose.covariance[28] = 0.01;
00095         odom.pose.covariance[35] = 0.1;
00096 
00097 
00098         odom.twist.twist = msg.twist[1];
00099         //These covariance values are "random". Some better values could be found after experiments and calculations
00100         odom.twist.covariance[0] = 0.01;
00101         odom.twist.covariance[7] = 0.01;
00102         odom.twist.covariance[14] = 0.01;
00103         odom.twist.covariance[21] = 0.01;
00104         odom.twist.covariance[28] = 0.01;
00105         odom.twist.covariance[35] = 0.1;
00106 
00107         if (odom_pub)
00108                 odom_pub.publish(odom);
00109 }
00110 
00111 int main(int argc, char **argv)
00112 {
00113         ros::init(argc, argv, "corobot_gazebo_control");
00114         ros::NodeHandle n;
00115 
00116         left_rear_wheel_pub = n.advertise<std_msgs::Float64>("corobot_left_rear_wheel_controller/command", 100);
00117         right_rear_wheel_pub = n.advertise<std_msgs::Float64>("corobot_right_rear_wheel_controller/command", 100);
00118         left_front_wheel_pub = n.advertise<std_msgs::Float64>("corobot_left_front_wheel_controller/command", 100);
00119         right_front_wheel_pub = n.advertise<std_msgs::Float64>("corobot_right_front_wheel_controller/command", 100);
00120         odom_pub = n.advertise<nav_msgs::Odometry>("odometry", 50); 
00121 
00122         ros::Subscriber robotVelocity= n.subscribe("PhidgetMotor", 100, setVelocity); //Command received by corobot_teleop or any other controlling node
00123         ros::Subscriber modelStates = n.subscribe("/gazebo/model_states",100, getModelState);
00124         ros::Subscriber leftWheelVelocity = n.subscribe("lwheel_vtarget",100, setLeftWheelVelocity);
00125         ros::Subscriber rightWheelVelocity = n.subscribe("rwheel_vtarget",100, setRightWheelVelocity);  
00126 
00127         ros::spin();
00128 
00129 
00130         return 0;
00131 }
00132 
00133 


corobot_gazebo
Author(s):
autogenerated on Sun Oct 5 2014 23:18:32