agvs_robot_control.cpp
Go to the documentation of this file.
00001 /*
00002  * agvs_robot_control
00003  * Copyright (c) 2014, Robotnik Automation, SLL
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of the Robotnik Automation, SLL. nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  *
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  *
00030  * \author Robotnik
00031  * \brief Controller for the AGVS robot Ackerman Drive
00032  * \brief (will include dual odometry measurement)
00033  */
00034 
00035 #include <ros/ros.h>
00036 #include <sensor_msgs/JointState.h>
00037 #include <sensor_msgs/Imu.h>
00038 #include <geometry_msgs/Twist.h>
00039 #include <std_msgs/Float64.h>
00040 #include <tf/transform_broadcaster.h>
00041 #include <nav_msgs/Odometry.h>
00042 #include <robotnik_msgs/set_mode.h>
00043 #include <robotnik_msgs/get_mode.h>
00044 #include <robotnik_msgs/set_odometry.h>
00045 #include <robotnik_msgs/ptz.h>
00046 
00047 #include "ackermann_msgs/AckermannDriveStamped.h"
00048 
00049 #include "diagnostic_msgs/DiagnosticStatus.h"
00050 #include "diagnostic_updater/diagnostic_updater.h"
00051 #include "diagnostic_updater/update_functions.h"
00052 #include "diagnostic_updater/DiagnosticStatusWrapper.h"
00053 #include "diagnostic_updater/publisher.h"
00054 #include <std_srvs/Empty.h>
00055 
00056 
00057 #define PI 3.1415926535
00058 #define AGVS_MIN_COMMAND_REC_FREQ     5.0
00059 #define AGVS_MAX_COMMAND_REC_FREQ     150.0
00060 
00061 #define AGVS_WHEEL_DIAMETER               0.2195      // Default wheel diameter
00062 #define DEFAULT_DIST_CENTER_TO_WHEEL  0.479       // Default distance center to motorwheel
00063     
00064 #define MAX_ELEVATOR_POSITION   0.05            // meters
00065 
00066 using namespace std;
00067 
00068 class AGVSControllerClass {
00069 
00070 public:
00071 
00072   ros::NodeHandle node_handle_;
00073   ros::NodeHandle private_node_handle_;
00074   double desired_freq_;
00075 
00076   // Diagnostics
00077   diagnostic_updater::Updater diagnostic_;                      // General status diagnostic updater
00078   diagnostic_updater::FrequencyStatus freq_diag_;                        // Component frequency diagnostics
00079   diagnostic_updater::HeaderlessTopicDiagnostic *subs_command_freq; // Topic reception frequency diagnostics
00080   ros::Time last_command_time_;                                 // Last moment when the component received a command
00081   diagnostic_updater::FunctionDiagnosticTask command_freq_;
00082 
00083   // Robot model 
00084   std::string robot_model_;
00085   
00086   // Velocity and position references to low level controllers
00087   ros::Publisher ref_vel_fwd_;
00088   ros::Publisher ref_vel_bwd_;
00089   ros::Publisher ref_pos_fwd_;
00090   ros::Publisher ref_pos_bwd_;
00091   ros::Publisher ref_pos_elevator_;
00092 
00093   // Joint states published by the joint_state_controller of the Controller Manager
00094   ros::Subscriber joint_state_sub_;
00095 
00096   // High level robot command
00097   ros::Subscriber cmd_sub_;
00098         
00099   // Services
00100   ros::ServiceServer srv_SetOdometry_;
00101   ros::ServiceServer srv_SetMode_;
00102   ros::ServiceServer srv_GetMode_;
00103   ros::ServiceServer srv_RaiseElevator_;
00104   ros::ServiceServer srv_LowerElevator_;
00105   
00106   // Topics - Ackerman - velocity
00107   std::string fwd_vel_topic_;
00108   std::string bwd_vel_topic_;
00109   
00110   // Joint names - Ackerman - velocity 
00111   std::string joint_front_wheel;
00112   std::string joint_back_wheel;
00113 
00114   // Topics - Ackerman - position
00115   std::string fwd_pos_topic_;
00116   std::string bwd_pos_topic_;
00117   std::string elevator_pos_topic_;
00118   
00119   std::string imu_topic_;
00120     
00121   // Joint names - Ackerman - position
00122   std::string joint_front_motor_wheel;
00123   std::string joint_back_motor_wheel;
00124     
00125   // Indexes to joint_states
00126   int fwd_vel_, bwd_vel_;
00127   int fwd_pos_, bwd_pos_;
00128 
00129   // Robot Speeds
00130   double linearSpeedXMps_;
00131   double linearSpeedYMps_;
00132   double angularSpeedRads_;
00133 
00134   // Robot Positions
00135   double robot_pose_px_;
00136   double robot_pose_py_;
00137   double robot_pose_pa_;
00138   double robot_pose_vx_;
00139   double robot_pose_vy_;
00140   
00141   // Robot Joint States
00142   sensor_msgs::JointState joint_state_;
00143   
00144   // Command reference
00145   // geometry_msgs::Twist base_vel_msg_;
00146   ackermann_msgs::AckermannDriveStamped base_vel_msg_;
00147 
00148   // External references
00149   double v_ref_;
00150   double a_ref_;
00151 
00152   double v_mps_;  // Measured real robot speed traction wheel speed
00153   
00154   // Flag to indicate if joint_state has been read
00155   bool read_state_; 
00156   
00157   // Robot configuration parameters 
00158   double agvs_wheel_diameter_; 
00159   double agvs_dist_to_center_;
00160 
00161   // IMU values
00162   double ang_vel_x_;
00163   double ang_vel_y_;
00164   double ang_vel_z_;
00165 
00166   double lin_acc_x_;
00167   double lin_acc_y_;
00168   double lin_acc_z_;
00169 
00170   double orientation_diff_x_;
00171   double orientation_diff_y_;
00172   double orientation_diff_z_;
00173   double orientation_diff_w_;
00174 
00175   // Parameter that defines if odom tf is published or not
00176   bool publish_odom_tf_;
00177 
00178   ros::Subscriber imu_sub_; 
00179   
00180   // Publisher for odom topic
00181   ros::Publisher odom_pub_; 
00182 
00183   // Broadcaster for odom tf  
00184   tf::TransformBroadcaster odom_broadcaster;
00185 
00186 
00190 AGVSControllerClass(ros::NodeHandle h) : diagnostic_(),
00191   node_handle_(h), private_node_handle_("~"), 
00192   desired_freq_(100.0),
00193   freq_diag_(diagnostic_updater::FrequencyStatusParam(&desired_freq_, &desired_freq_, 0.05)   ),
00194   command_freq_("Command frequency check", boost::bind(&AGVSControllerClass::check_command_subscriber, this, _1))
00195   {
00196 
00197   ROS_INFO("agvs_robot_control_node - Init ");
00198   
00199   ros::NodeHandle agvs_robot_control_node_handle(node_handle_, "agvs_robot_control");
00200 
00201   // Get robot model from the parameters
00202   if (!private_node_handle_.getParam("model", robot_model_)) {
00203           ROS_ERROR("Robot model not defined.");
00204           exit(-1);
00205           }
00206   else ROS_INFO("Robot Model : %s", robot_model_.c_str());
00207 
00208   // Ackerman configuration - topics (control actions)
00209   private_node_handle_.param<std::string>("fwd_vel_topic", fwd_vel_topic_, "/agvs/joint_front_wheel_controller/command");
00210   private_node_handle_.param<std::string>("bwd_vel_topic", bwd_vel_topic_, "/agvs/joint_back_wheel_controller/command");
00211   private_node_handle_.param<std::string>("fwd_pos_topic", fwd_pos_topic_, "/agvs/joint_front_motor_wheel_controller/command");
00212   private_node_handle_.param<std::string>("bwd_pos_topic", bwd_pos_topic_, "/agvs/joint_back_motor_wheel_controller/command");
00213   private_node_handle_.param<std::string>("elevator_pos_topic", elevator_pos_topic_, "/agvs/joint_elevator_controller/command");
00214   private_node_handle_.param<std::string>("imu_topic", imu_topic_, "/agvs/imu_data");
00215 
00216   // Ackerman configuration - joint names 
00217   private_node_handle_.param<std::string>("joint_front_wheel", joint_front_wheel, "joint_front_wheel");
00218   private_node_handle_.param<std::string>("joint_back_wheel", joint_back_wheel, "joint_back_wheel");
00219   private_node_handle_.param<std::string>("joint_front_motor_wheel", joint_front_motor_wheel, "joint_front_motor_wheel");
00220   private_node_handle_.param<std::string>("joint_back_motor_wheel", joint_back_motor_wheel, "joint_back_motor_wheel");
00221 
00222   // Robot kinematic parameters 
00223   if (!private_node_handle_.getParam("agvs_wheel_diameter", agvs_wheel_diameter_))
00224                 agvs_wheel_diameter_ = AGVS_WHEEL_DIAMETER; 
00225   if (!private_node_handle_.getParam("agvs_dist_to_center", agvs_dist_to_center_))
00226                 agvs_dist_to_center_ = DEFAULT_DIST_CENTER_TO_WHEEL;
00227   //ROS_INFO("agvs_wheel_diameter_ = %5.2f", agvs_wheel_diameter_);
00228   //ROS_INFO("agvs_dist_to_center_ = %5.2f", agvs_dist_to_center_);
00229 
00230   private_node_handle_.param("publish_odom_tf", publish_odom_tf_, true);
00231   if (publish_odom_tf_) ROS_INFO("PUBLISHING odom->base_footprint tf");
00232   else ROS_INFO("NOT PUBLISHING odom->base_footprint tf");
00233   
00234   // Robot Speeds
00235   linearSpeedXMps_   = 0.0;
00236   linearSpeedYMps_   = 0.0;
00237   angularSpeedRads_  = 0.0;
00238 
00239   // Robot Positions
00240   robot_pose_px_ = 0.0;
00241   robot_pose_py_ = 0.0;
00242   robot_pose_pa_ = 0.0;
00243   robot_pose_vx_ = 0.0;
00244   robot_pose_vy_ = 0.0;
00245 
00246   // External speed references
00247   v_ref_ = 0.0;
00248   a_ref_ = 0.0;
00249 
00250   // Imu variables
00251   ang_vel_x_ = 0.0; ang_vel_y_ = 0.0; ang_vel_z_ = 0.0;
00252   lin_acc_x_ = 0.0; lin_acc_y_ = 0.0; lin_acc_z_ = 0.0;
00253   orientation_diff_x_ = 0.0; orientation_diff_y_ = 0.0; orientation_diff_z_ = 0.0; orientation_diff_w_ = 1.0;
00254 
00255   // Advertise services
00256   srv_SetOdometry_ = private_node_handle_.advertiseService("set_odometry",  &AGVSControllerClass::srvCallback_SetOdometry, this);
00257   srv_RaiseElevator_ = private_node_handle_.advertiseService("raise_elevator",  &AGVSControllerClass::srvCallback_RaiseElevator, this);
00258   srv_LowerElevator_ = private_node_handle_.advertiseService("lower_elevator",  &AGVSControllerClass::srvCallback_LowerElevator, this);
00259 
00260   // Subscribe to joint states topic
00261   joint_state_sub_ = agvs_robot_control_node_handle.subscribe<sensor_msgs::JointState>("/agvs/joint_states", 1, &AGVSControllerClass::jointStateCallback, this);
00262   //joint_state_sub_ = private_node_handle_.subscribe<sensor_msgs::JointState>("joint_states", 1, &AGVSControllerClass::jointStateCallback, this);
00263   //joint_state_sub_ = summit_xl_robot_control_node_handle.subscribe<sensor_msgs::JointState>("/summit_xl/joint_states", 1, &SummitXLControllerClass::jointStateCallback, this);
00264 
00265 
00266   // Subscribe to imu data
00267   // imu_sub_ = agvs_robot_control_node_handle.subscribe("/agvs/imu_data", 1, &AGVSControllerClass::imuCallback, this);
00268   imu_sub_ = private_node_handle_.subscribe(imu_topic_, 1, &AGVSControllerClass::imuCallback, this);
00269 
00270   // Adevertise reference topics for the controllers 
00271   ref_vel_fwd_ = private_node_handle_.advertise<std_msgs::Float64>( fwd_vel_topic_, 50);
00272   ref_vel_bwd_ = private_node_handle_.advertise<std_msgs::Float64>( bwd_vel_topic_, 50);
00273   ref_pos_fwd_ = private_node_handle_.advertise<std_msgs::Float64>( fwd_pos_topic_, 50);
00274   ref_pos_bwd_ = private_node_handle_.advertise<std_msgs::Float64>( bwd_pos_topic_, 50);          
00275   ref_pos_elevator_ = private_node_handle_.advertise<std_msgs::Float64>( elevator_pos_topic_, 50);        
00276           
00277   // Subscribe to command topic
00278   cmd_sub_ = private_node_handle_.subscribe<ackermann_msgs::AckermannDriveStamped>("command", 1, &AGVSControllerClass::commandCallback, this ); 
00279     
00280   // TODO odom topic as parameter
00281   // Publish odometry 
00282   odom_pub_ = private_node_handle_.advertise<nav_msgs::Odometry>("odom", 1000);
00283 
00284   // Component frequency diagnostics
00285   diagnostic_.setHardwareID("agvs_robot_control - simulation");
00286   diagnostic_.add( freq_diag_ );
00287   diagnostic_.add( command_freq_ );
00288     
00289   // Topics freq control 
00290   // For /agvs_robot_control/command
00291   double min_freq = AGVS_MIN_COMMAND_REC_FREQ; // If you update these values, the
00292   double max_freq = AGVS_MAX_COMMAND_REC_FREQ; // HeaderlessTopicDiagnostic will use the new values.
00293   subs_command_freq = new diagnostic_updater::HeaderlessTopicDiagnostic("/agvs_robot_control/command", diagnostic_,
00294                             diagnostic_updater::FrequencyStatusParam(&min_freq, &max_freq, 0.1, 10));
00295   subs_command_freq->addTask(&command_freq_); // Adding an additional task to the control
00296   
00297   // Flag to indicate joint_state has been read
00298   read_state_ = false;
00299   
00300   // Robot ackermann measured speed.
00301   v_mps_ = 0;
00302 }
00303 
00305 int starting()
00306 {
00307 
00308   ROS_INFO("AGVSControllerClass::starting");
00309 
00310 // name: ['joint_back_motor_wheel', 'joint_back_wheel', 'joint_front_motor_wheel', 'joint_front_wheel']
00311 // position: [6.283185307179586, -3.14159, 6.283185307179586, -3.14159]
00312 // velocity: [nan, nan, nan, nan]
00313 
00314   // Initialize joint indexes according to joint names 
00315   if (read_state_) {
00316     vector<string> joint_names = joint_state_.name;
00317     fwd_vel_ = find (joint_names.begin(),joint_names.end(), string(joint_front_wheel)) - joint_names.begin();
00318     bwd_vel_ = find (joint_names.begin(),joint_names.end(), string(joint_back_wheel)) - joint_names.begin();
00319     fwd_pos_ = find (joint_names.begin(),joint_names.end(), string(joint_front_motor_wheel)) - joint_names.begin();
00320     bwd_pos_ = find (joint_names.begin(),joint_names.end(), string(joint_back_motor_wheel)) - joint_names.begin();
00321     return 0;
00322     }
00323   else return -1;
00324 }
00325 
00330 void UpdateOdometry(){
00331 
00332     // TODO UpdateOdometry differential drive
00333         
00334    // double v_left_mps, v_right_mps;
00335    // Calculate its own velocities for realize the motor control 
00336    // v_left_mps = ((joint_state_.velocity[blw_vel_] + joint_state_.velocity[flw_vel_]) / 2.0) * (summit_xl_wheel_diameter_ / 2.0);
00337    // v_right_mps = -((joint_state_.velocity[brw_vel_] + joint_state_.velocity[frw_vel_]) / 2.0) * (summit_xl_wheel_diameter_ / 2.0); 
00338    // sign according to urdf (if wheel model is not symetric, should be inverted)
00339    // angularSpeedRads_ = (v_right_mps - v_left_mps) / summit_xl_d_tracks_m_;    // rad/s
00340 
00341 
00342         double fBetaRads = 0.0;
00343         double v_mps = 0.0;
00344 
00345         // Compute Position
00346         double fSamplePeriod = 1.0 / desired_freq_;
00347 
00348         // Linear speed of each wheel
00349         double v_fwd, v_bwd; 
00350         v_fwd = joint_state_.velocity[fwd_vel_] * (agvs_wheel_diameter_ / 2.0);
00351         v_bwd = joint_state_.velocity[bwd_vel_] * (agvs_wheel_diameter_ / 2.0);
00352         //ROS_INFO("v_fwd = %.3lf, v_bwd = %.3lf", v_fwd, v_bwd);
00353         v_mps = -(v_fwd + v_bwd) / 2.0;
00354 
00355         // Angle of fwd and bwd motorwheels
00356         double a_fwd, a_bwd;
00357         a_fwd = radnorm2( joint_state_.position[fwd_pos_] );
00358         a_bwd = radnorm2( joint_state_.position[bwd_pos_] );
00359         fBetaRads = a_fwd; // consider to get a mean, but both angles are antisimetric
00360         
00361         // Filter noise
00362         if(fabs(v_mps) < 0.00001) v_mps = 0.0;
00363 
00364         // Compute Odometry
00365         double w = (v_mps / agvs_dist_to_center_) * sin(fBetaRads);
00366         robot_pose_pa_ += w * fSamplePeriod;
00367 
00368         // normalize
00369         radnorm(&robot_pose_pa_);
00370         //ROS_INFO("Orientation = %.3lf", robot_pose_pa_);
00371         // Velocities
00372         robot_pose_vx_ = v_mps * cos(fBetaRads) * cos(robot_pose_pa_);
00373         robot_pose_vy_ = v_mps * cos(fBetaRads) * sin(robot_pose_pa_);
00374         
00375         // Positions
00376         robot_pose_px_ += robot_pose_vx_ * fSamplePeriod;
00377         robot_pose_py_ += robot_pose_vy_ * fSamplePeriod;
00378 }
00379 
00380 // Publish robot odometry tf and topic depending 
00381 void PublishOdometry()
00382 {
00383         ros::Time current_time = ros::Time::now();
00384         
00385     //first, we'll publish the transform over tf
00386     geometry_msgs::TransformStamped odom_trans;
00387     odom_trans.header.stamp = current_time;
00388     odom_trans.header.frame_id = "odom";
00389     odom_trans.child_frame_id = "base_footprint";
00390 
00391     odom_trans.transform.translation.x = robot_pose_px_;
00392     odom_trans.transform.translation.y = robot_pose_py_;
00393     odom_trans.transform.translation.z = 0.0;
00394 
00395         // Convert theta from yaw (rads) to quaternion. note that this is only 2D !!!
00396     double theta = robot_pose_pa_;
00397     geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw( theta );
00398     odom_trans.transform.rotation.x = quat.x;
00399         odom_trans.transform.rotation.y = quat.y;
00400         odom_trans.transform.rotation.z = quat.z;
00401         odom_trans.transform.rotation.w = quat.w;
00402 
00403         
00404     // send the transform over /tf
00405         // activate / deactivate with param
00406         // this tf in needed when not using robot_pose_ekf
00407     if (publish_odom_tf_) odom_broadcaster.sendTransform(odom_trans);  
00408         
00409     //next, we'll publish the odometry message over ROS
00410     nav_msgs::Odometry odom;
00411     odom.header.stamp = current_time;
00412     odom.header.frame_id = "odom";
00413 
00414     //set the position
00415         // Position
00416     odom.pose.pose.position.x = robot_pose_px_;
00417     odom.pose.pose.position.y = robot_pose_py_;
00418     odom.pose.pose.position.z = 0.0;
00419         // Orientation
00420     odom.pose.pose.orientation.x = orientation_diff_x_;
00421         odom.pose.pose.orientation.y = orientation_diff_y_;
00422         odom.pose.pose.orientation.z = orientation_diff_z_;
00423         odom.pose.pose.orientation.w = orientation_diff_w_;
00424         
00425     // Pose covariance
00426     for(int i = 0; i < 6; i++)
00427                 odom.pose.covariance[i*6+i] = 0.1;  // test 0.001
00428 
00429     //set the velocity
00430     odom.child_frame_id = "base_footprint";
00431         // Linear velocities
00432     odom.twist.twist.linear.x = robot_pose_vx_;
00433     odom.twist.twist.linear.y = robot_pose_vy_;
00434         odom.twist.twist.linear.z = 0.0;
00435         // Angular velocities
00436         odom.twist.twist.angular.x = ang_vel_x_;
00437         odom.twist.twist.angular.y = ang_vel_y_;
00438     odom.twist.twist.angular.z = ang_vel_z_;
00439         // Twist covariance
00440         for(int i = 0; i < 6; i++)
00441                 odom.twist.covariance[6*i+i] = 0.1;  // test 0.001
00442 
00443     //publish the message
00444     odom_pub_.publish(odom);
00445 }
00446 
00447 void UpdateControl()
00448 {
00449   // Ackerman reference messages
00450   std_msgs::Float64 vel_ref_msg; 
00451   std_msgs::Float64 pos_ref_msg;
00452   static double ev_ant = 0.0;
00453 
00454   // Note that the controllers are not in speed mode, but in effort. Therefore the speed ref will be Nm.
00455   // Open loop - works well but there is some inertia in the whole system and just setting torque to 0 does not stop the robot.
00456   // vel_ref_msg.data = -v_ref_ * 20.0;
00457   
00458   // If using a JointEffortController Try a velocity control loop
00459   // After 1.9.2 VelocityControllers in agvs_control should work 
00460   /*
00461   double ev = v_mps_ - v_ref_;
00462   double Kp = 30.0;
00463   double Kd = 30.0;
00464   vel_ref_msg.data = Kp * ev;  //+ Kd * (ev - ev_ant);
00465   ev_ant = ev;
00466   */
00467 
00468   // Reference for velocity controllers
00469   double Kp = 10.0;  // ref is in [m/s] while VelocityController expects ?
00470   vel_ref_msg.data = -v_ref_ * Kp;
00471 
00472   pos_ref_msg.data = a_ref_;
00473                   
00474   // Publish references 
00475   ref_vel_fwd_.publish( vel_ref_msg );
00476   ref_vel_bwd_.publish( vel_ref_msg );
00477   
00478   ref_pos_fwd_.publish( pos_ref_msg );
00479   pos_ref_msg.data = -a_ref_; // symetric angle
00480   ref_pos_bwd_.publish( pos_ref_msg );
00481 }
00482 
00483 // Sets the motor position to the desired value
00484 void SetElevatorPosition(double val){
00485         
00486         std_msgs::Float64 ref_msg; 
00487         
00488         ref_msg.data = val;
00489         
00490         ref_pos_elevator_.publish( ref_msg );
00491 }
00492 
00494 void stopping()
00495 {}
00496 
00497 /*
00498  *      \brief Checks that the robot is receiving at a correct frequency the command messages. Diagnostics
00499  *
00500  */
00501 void check_command_subscriber(diagnostic_updater::DiagnosticStatusWrapper &stat)
00502 {
00503         ros::Time current_time = ros::Time::now();
00504 
00505         double diff = (current_time - last_command_time_).toSec();
00506 
00507         if(diff > 1.0){
00508                 stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Topic is not receiving commands");
00509                 //ROS_INFO("check_command_subscriber: %lf seconds without commands", diff);
00510                 // TODO: Set Speed References to 0
00511         }else{
00512                 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Topic receiving commands");
00513         }
00514 }
00515 
00516 // Service SetOdometry 
00517 bool srvCallback_SetOdometry(robotnik_msgs::set_odometry::Request &request, robotnik_msgs::set_odometry::Response &response )
00518 {
00519         // ROS_INFO("summit_xl_odometry::set_odometry: request -> x = %f, y = %f, a = %f", req.x, req.y, req.orientation);
00520         robot_pose_px_ = request.x;
00521         robot_pose_py_ = request.y;
00522         robot_pose_pa_ = request.orientation;
00523 
00524         response.ret = true;
00525         return true;
00526 }
00527 
00528 // Service Raise Elevator 
00529 bool srvCallback_RaiseElevator(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response )
00530 {
00531         
00532         SetElevatorPosition(MAX_ELEVATOR_POSITION);
00533         
00534         return true;
00535 }
00536 
00537 // Service Lower Elevator 
00538 bool srvCallback_LowerElevator(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response )
00539 {
00540         SetElevatorPosition(0.0);
00541         
00542         return true;
00543 }
00544 
00545 
00546 // Topic command
00547 void jointStateCallback(const sensor_msgs::JointStateConstPtr& msg)
00548 {       
00549   joint_state_ = *msg;
00550   read_state_ = true;
00551 }
00552 
00553 // Topic command
00554 void commandCallback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& msg)
00555 {
00556   // Safety check
00557   last_command_time_ = ros::Time::now();
00558   subs_command_freq->tick();                    // For diagnostics
00559 
00560   double speed_limit = 2.0;  // m/s
00561   double angle_limit = PI;   
00562   v_ref_ = saturation(msg->drive.speed, -speed_limit, speed_limit);  
00563   a_ref_ = saturation(msg->drive.steering_angle, -angle_limit, angle_limit);
00564 }
00565 
00566 // Imu callback
00567 void imuCallback( const sensor_msgs::Imu& imu_msg){
00568 
00569         orientation_diff_x_ = imu_msg.orientation.x;
00570         orientation_diff_y_ = imu_msg.orientation.y;
00571         orientation_diff_z_ = imu_msg.orientation.z;
00572         orientation_diff_w_ = imu_msg.orientation.w;
00573 
00574         ang_vel_x_ = imu_msg.angular_velocity.x;
00575         ang_vel_y_ = imu_msg.angular_velocity.y;
00576         ang_vel_z_ = imu_msg.angular_velocity.z;
00577 
00578         lin_acc_x_ = imu_msg.linear_acceleration.x;
00579         lin_acc_y_ = imu_msg.linear_acceleration.y;
00580     lin_acc_z_ = imu_msg.linear_acceleration.z;
00581 }
00582 
00583 double saturation(double u, double min, double max)
00584 {
00585  if (u>max) u=max;
00586  if (u<min) u=min;
00587  return u; 
00588 }
00589 
00590 
00591 
00593 static inline void radnorm(double* radians)
00594 {
00595         while (*radians >= (PI)) {
00596                 *radians -= 2.0 * PI;
00597                 }
00598         while (*radians <= (-PI)) {
00599                 *radians += 2.0 * PI;
00600                 }
00601 }
00602 
00603 static inline double radnorm2( double value ) 
00604 {
00605   while (value > 2.0*PI) value -= 2.0*PI;
00606   while (value < -2.0*PI) value += 2.0*PI;
00607   return value;
00608 }
00609 
00610 bool spin()
00611 {
00612     ROS_INFO("agvs_robot_control::spin()");
00613     ros::Rate r(desired_freq_);  // 50.0 
00614 
00615     while (!ros::isShuttingDown()) // Using ros::isShuttingDown to avoid restarting the node during a shutdown.
00616     {
00617       if (starting() == 0)
00618       {
00619             while(ros::ok() && node_handle_.ok()) {
00620           UpdateControl();
00621           UpdateOdometry();
00622           PublishOdometry();
00623           diagnostic_.update();
00624           ros::spinOnce();
00625               r.sleep();
00626           }
00627               ROS_INFO("END OF ros::ok() !!!");
00628       } else {
00629        // No need for diagnostic here since a broadcast occurs in start
00630        // when there is an error.
00631        usleep(1000000);
00632        ros::spinOnce();
00633       }
00634    }
00635 
00636    ROS_INFO("agvs_robot_control::spin() - end");
00637    return true;
00638 }
00639 
00640 }; // Class AGVSControllerClass
00641 
00642 int main(int argc, char** argv)
00643 {
00644         ros::init(argc, argv, "agvs_robot_control");
00645 
00646         ros::NodeHandle n;              
00647         AGVSControllerClass sxlrc(n);
00648 
00649     sxlrc.spin();
00650 
00651         return (0);
00652 
00653         
00654 }
00655 


agvs_robot_control
Author(s): Roberto Guzmán
autogenerated on Thu Aug 27 2015 12:08:25