volksbot_node.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Osnabrueck University
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  * * Redistributions of source code must retain the above copyright notice,
00009  *   this list of conditions and the following disclaimer.
00010  * * Redistributions in binary form must reproduce the above copyright notice,
00011  *   this list of conditions and the following disclaimer in the documentation
00012  *   and/or other materials provided with the distribution.
00013  * * Neither the name of the Osnabrueck University nor the names of its
00014  *   contributors may be used to endorse or promote products derived from this
00015  *   software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <geometry_msgs/Twist.h>
00031 #include <nav_msgs/Odometry.h>
00032 #include <tf/transform_broadcaster.h>
00033 #include <tf/transform_listener.h>
00034 #include <sensor_msgs/JointState.h>
00035 #include <ros/ros.h>
00036 #include <ros/console.h>
00037 
00038 #include <string>
00039 
00040 #include "volksbot.h"
00041 
00042 class ROSComm : public Comm
00043 {
00044   public:
00045     ROSComm(
00046         const ros::NodeHandle &n,
00047         double sigma_x,
00048         double sigma_theta,
00049         double cov_x_y,
00050         double cov_x_theta,
00051         double cov_y_theta) :
00052       n_(n),
00053       sigma_x_(sigma_x),
00054       sigma_theta_(sigma_theta),
00055       cov_x_y_(cov_x_y),
00056       cov_x_theta_(cov_x_theta),
00057       cov_y_theta_(cov_y_theta),
00058       publish_tf_(false),
00059       odom_pub_(n_.advertise<nav_msgs::Odometry> ("odom", 10)),
00060       joint_pub_(n_.advertise<sensor_msgs::JointState> ("joint_states", 1)) { }
00061     virtual void send_odometry(double x, double y, double theta, double v_x,
00062         double v_theta, double wheelpos_l, double wheelpos_r);
00063 
00064     void setTFPrefix(const std::string &tf_prefix);
00065 
00066   private:
00067     void populateCovariance(nav_msgs::Odometry &msg, double v_x, double
00068         v_theta);
00069 
00070     ros::NodeHandle n_;
00071     double sigma_x_, sigma_theta_, cov_x_y_, cov_x_theta_, cov_y_theta_;
00072     bool publish_tf_;
00073     std::string tf_prefix_;
00074 
00075     tf::TransformBroadcaster odom_broadcaster_;
00076     ros::Publisher odom_pub_;
00077     ros::Publisher joint_pub_;
00078 };
00079 
00080 void ROSComm::setTFPrefix(const std::string &tf_prefix)
00081 {
00082   tf_prefix_ = tf_prefix;
00083 }
00084 
00085 void ROSComm::populateCovariance(nav_msgs::Odometry &msg, double v_x, double v_theta)
00086 {
00087   double odom_multiplier = 1.0;
00088 
00089   if (fabs(v_x) <= 1e-8 && fabs(v_theta) <= 1e-8)
00090   {
00091     //nav_msgs::Odometry has a 6x6 covariance matrix
00092     msg.twist.covariance[0] = 1e-12;
00093     msg.twist.covariance[35] = 1e-12;
00094 
00095     msg.twist.covariance[30] = 1e-12;
00096     msg.twist.covariance[5] = 1e-12;
00097   }
00098   else
00099   {
00100     //nav_msgs::Odometry has a 6x6 covariance matrix
00101     msg.twist.covariance[0] = odom_multiplier * pow(sigma_x_, 2);
00102     msg.twist.covariance[35] = odom_multiplier * pow(sigma_theta_, 2);
00103 
00104     msg.twist.covariance[30] = odom_multiplier * cov_x_theta_;
00105     msg.twist.covariance[5] = odom_multiplier * cov_x_theta_;
00106   }
00107 
00108   msg.twist.covariance[7] = DBL_MAX;
00109   msg.twist.covariance[14] = DBL_MAX;
00110   msg.twist.covariance[21] = DBL_MAX;
00111   msg.twist.covariance[28] = DBL_MAX;
00112 
00113   msg.pose.covariance = msg.twist.covariance;
00114 
00115   if (fabs(v_x) <= 1e-8 && fabs(v_theta) <= 1e-8)
00116   {
00117     msg.pose.covariance[7] = 1e-12;
00118 
00119     msg.pose.covariance[1] = 1e-12;
00120     msg.pose.covariance[6] = 1e-12;
00121 
00122     msg.pose.covariance[31] = 1e-12;
00123     msg.pose.covariance[11] = 1e-12;
00124   }
00125   else
00126   {
00127     msg.pose.covariance[7] = odom_multiplier * pow(sigma_x_, 2) * pow(sigma_theta_, 2);
00128 
00129     msg.pose.covariance[1] = odom_multiplier * cov_x_y_;
00130     msg.pose.covariance[6] = odom_multiplier * cov_x_y_;
00131 
00132     msg.pose.covariance[31] = odom_multiplier * cov_y_theta_;
00133     msg.pose.covariance[11] = odom_multiplier * cov_y_theta_;
00134   }
00135 }
00136 
00137 void ROSComm::send_odometry(double x, double y, double theta, double v_x, double v_theta, double wheelpos_l, double wheelpos_r)
00138 {
00139   nav_msgs::Odometry odom;
00140   odom.header.frame_id = tf::resolve(tf_prefix_, "odom_combined");
00141   odom.child_frame_id = tf::resolve(tf_prefix_, "base_footprint");
00142 
00143   odom.header.stamp = ros::Time::now();
00144   odom.pose.pose.position.x = x;
00145   odom.pose.pose.position.y = y;
00146   odom.pose.pose.position.z = 0.0;
00147   odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(theta);
00148 
00149   odom.twist.twist.linear.x = v_x;
00150   odom.twist.twist.linear.y = 0.0;
00151   odom.twist.twist.angular.z = v_theta;
00152   populateCovariance(odom, v_x, v_theta);
00153 
00154   odom_pub_.publish(odom);
00155 
00156   if (publish_tf_)
00157   {
00158     geometry_msgs::TransformStamped odom_trans;
00159     odom_trans.header.frame_id = tf::resolve(tf_prefix_, "odom_combined");
00160     odom_trans.child_frame_id = tf::resolve(tf_prefix_, "base_footprint");
00161 
00162     odom_trans.header.stamp = ros::Time::now();
00163     odom_trans.transform.translation.x = x;
00164     odom_trans.transform.translation.y = y;
00165     odom_trans.transform.translation.z = 0.0;
00166     odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(theta);
00167 
00168     odom_broadcaster_.sendTransform(odom_trans);
00169   }
00170 
00171   sensor_msgs::JointState joint_state;
00172   joint_state.header.stamp = ros::Time::now();
00173   joint_state.name.resize(6);
00174   joint_state.position.resize(6);
00175   joint_state.name[0] = "left_front_wheel_joint";
00176   joint_state.name[1] = "left_middle_wheel_joint";
00177   joint_state.name[2] = "left_rear_wheel_joint";
00178   joint_state.name[3] = "right_front_wheel_joint";
00179   joint_state.name[4] = "right_middle_wheel_joint";
00180   joint_state.name[5] = "right_rear_wheel_joint";
00181 
00182   joint_state.position[0] = joint_state.position[1] = joint_state.position[2] = wheelpos_l;
00183   joint_state.position[3] = joint_state.position[4] = joint_state.position[5] = wheelpos_r;
00184 
00185   joint_pub_.publish(joint_state);
00186 }
00187 
00188 class ROSCall
00189 {
00190   public:
00191     ROSCall(Volksbot &volksbot, double axis_length) :
00192       volksbot_(volksbot),
00193       axis_length_(axis_length),
00194       last_cmd_vel_time_(0.0) { }
00195     void velCallback(const geometry_msgs::Twist::ConstPtr& msg);
00196     void cmd_velWatchdog(const ros::TimerEvent& event);
00197 
00198   private:
00199     Volksbot &volksbot_;
00200     double axis_length_;
00201     ros::Time last_cmd_vel_time_;
00202 };
00203 
00204 void ROSCall::velCallback(const geometry_msgs::Twist::ConstPtr& msg)
00205 {
00206   last_cmd_vel_time_ = ros::Time::now();
00207   volksbot_.set_wheel_speed(msg->linear.x - axis_length_ * msg->angular.z * 0.5, msg->linear.x + axis_length_ * msg->angular.z * 0.5);
00208 }
00209 
00210 void ROSCall::cmd_velWatchdog(const ros::TimerEvent& event)
00211 {
00212   if (ros::Time::now() - last_cmd_vel_time_ > ros::Duration(0.6))
00213     volksbot_.set_wheel_speed(0.0, 0.0);
00214 }
00215 
00216 int main(int argc, char** argv)
00217 {
00218   ros::init(argc, argv, "volksbot");
00219   ros::NodeHandle n;
00220   ros::NodeHandle nh_ns("~");
00221 
00222   /* This is the wheel Radius for the odometry, accounting for some slip in the movement.
00223    * therefor it's not the same as the one in the volksbot.urdf.xacro */
00224   double wheel_radius;
00225   nh_ns.param("wheel_radius", wheel_radius, 0.0985);
00226   double axis_length;
00227   nh_ns.param("axis_length", axis_length, 0.41);
00228   int gear_ratio;
00229   nh_ns.param("gear_ratio", gear_ratio, 74);
00230   int max_vel_l;
00231   nh_ns.param("max_vel_l", max_vel_l, 8250);
00232   int max_vel_r;
00233   nh_ns.param("max_vel_r", max_vel_r, 8400);
00234   int max_acc_l;
00235   nh_ns.param("max_acc_l", max_acc_l, 10000);
00236   int max_acc_r;
00237   nh_ns.param("max_acc_r", max_acc_r, 10000);
00238   bool drive_backwards;
00239   nh_ns.param("drive_backwards", drive_backwards, false);
00240 
00241   double turning_adaptation;
00242   nh_ns.param("turning_adaptation", turning_adaptation, 0.95);
00243 
00244   double sigma_x, sigma_theta, cov_x_y, cov_x_theta, cov_y_theta;
00245   nh_ns.param("x_stddev", sigma_x, 0.002);
00246   nh_ns.param("rotation_stddev", sigma_theta, 0.017);
00247   nh_ns.param("cov_xy", cov_x_y, 0.0);
00248   nh_ns.param("cov_xrotation", cov_x_theta, 0.0);
00249   nh_ns.param("cov_yrotation", cov_y_theta, 0.0);
00250 
00251   ROSComm roscomm(n, sigma_x, sigma_theta, cov_x_y, cov_x_theta, cov_y_theta);
00252 
00253   Volksbot volksbot(roscomm, wheel_radius, axis_length, turning_adaptation, gear_ratio, max_vel_l, max_vel_r, max_acc_l, max_acc_r, drive_backwards);
00254 
00255   bool publish_tf;
00256   nh_ns.param("publish_tf", publish_tf, false);
00257   std::string tf_prefix;
00258   tf_prefix = tf::getPrefixParam(nh_ns);
00259   roscomm.setTFPrefix(tf_prefix);
00260 
00261   ROSCall roscall(volksbot, axis_length);
00262 
00263   ros::Timer timer = n.createTimer(ros::Duration(0.1), &ROSCall::cmd_velWatchdog, &roscall);
00264   ros::Subscriber cmd_vel_sub = n.subscribe("cmd_vel", 10, &ROSCall::velCallback, &roscall);
00265 
00266   while (ros::ok())
00267   {
00268     volksbot.odometry();
00269     ros::spinOnce();
00270   }
00271 
00272   return 0;
00273 }


volksbot_driver
Author(s): Jochen Sprickerhof
autogenerated on Mon Oct 6 2014 08:49:49