obstacle_avoidance.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002    STDR Simulator - Simple Two DImensional Robot Simulator
00003    Copyright (C) 2013 STDR Simulator
00004    This program is free software; you can redistribute it and/or modify
00005    it under the terms of the GNU General Public License as published by
00006    the Free Software Foundation; either version 3 of the License, or
00007    (at your option) any later version.
00008    This program is distributed in the hope that it will be useful,
00009    but WITHOUT ANY WARRANTY; without even the implied warranty of
00010    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00011    GNU General Public License for more details.
00012    You should have received a copy of the GNU General Public License
00013    along with this program; if not, write to the Free Software Foundation,
00014    Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
00015    
00016    Authors : 
00017    * Manos Tsardoulias, etsardou@gmail.com
00018    * Aris Thallas, aris.thallas@gmail.com
00019    * Chris Zalidis, zalidis@gmail.com 
00020 ******************************************************************************/
00021 # include "stdr_samples/obstacle_avoidance/obstacle_avoidance.h"
00022 
00027 namespace stdr_samples
00028 {
00035   ObstacleAvoidance::ObstacleAvoidance(int argc,char **argv)
00036   {
00037     if(argc != 3)
00038     {
00039       ROS_ERROR(
00040         "Usage : stdr_obstacle avoidance <robot_frame_id> <laser_frame_id>");
00041       exit(0);
00042     }
00043     laser_topic_ = std::string("/") +
00044       std::string(argv[1]) + std::string("/") + std::string(argv[2]);
00045     speeds_topic_ = std::string("/") +
00046       std::string(argv[1]) + std::string("/cmd_vel");
00047       
00048     subscriber_ = n_.subscribe(
00049       laser_topic_.c_str(), 
00050       1, 
00051       &ObstacleAvoidance::callback,
00052       this);
00053       
00054     cmd_vel_pub_ = n_.advertise<geometry_msgs::Twist>(speeds_topic_.c_str(), 1);
00055   }
00056   
00061   ObstacleAvoidance::~ObstacleAvoidance(void)
00062   {
00063     
00064   }
00065   
00071   void ObstacleAvoidance::callback(const sensor_msgs::LaserScan& msg)
00072   {
00073     scan_ = msg;
00074     float linear = 0, rotational = 0;
00075     for(unsigned int i = 0 ; i < scan_.ranges.size() ; i++)
00076     {
00077       float real_dist = scan_.ranges[i];
00078       linear -= cos(scan_.angle_min + i * scan_.angle_increment) 
00079         / (1.0 + real_dist * real_dist);
00080       rotational -= sin(scan_.angle_min + i * scan_.angle_increment) 
00081         / (1.0 + real_dist * real_dist);
00082     }
00083     geometry_msgs::Twist cmd;
00084     
00085     linear /= scan_.ranges.size();
00086     rotational /= scan_.ranges.size();
00087     
00088     //~ ROS_ERROR("%f %f",linear,rotational);
00089     
00090     if(linear > 0.3)
00091     {
00092       linear = 0.3;
00093     }
00094     else if(linear < -0.3)
00095     {
00096       linear = -0.3;
00097     }
00098 
00099     cmd.linear.x = 0.3 + linear;
00100     cmd.angular.z = rotational;
00101     cmd_vel_pub_.publish(cmd);
00102   }
00103 }


stdr_samples
Author(s): Manos Tsardoulias, Chris Zalidis, Aris Thallas
autogenerated on Wed Sep 2 2015 03:36:41