ROSAgent.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Daniel Claes, Maastricht 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
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Maastricht University nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this 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 OWNER 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 
00031 #ifndef ROSAGENT_H
00032 #define ROSAGENT_H
00033 
00034 #include <ros/ros.h>
00035 #include <nav_msgs/Odometry.h>
00036 
00037 #include <boost/thread.hpp>
00038 #include <boost/bind.hpp>
00039 
00040 #include <amcl/PoseArrayWeighted.h>
00041 #include <geometry_msgs/PolygonStamped.h>
00042 
00043 #include <collvoid_msgs/PoseTwistWithCovariance.h>
00044 #include <tf/transform_listener.h>
00045 #include <sensor_msgs/PointCloud.h>
00046 #include <tf/message_filter.h>
00047 #include <message_filters/subscriber.h>
00048 #include <laser_geometry/laser_geometry.h>
00049 
00050 #include "collvoid_local_planner/Agent.h"
00051 
00052 
00053 namespace collvoid {
00054   struct Obstacle{
00055     Vector2 point1;
00056     Vector2 point2;
00057     ros::Time last_seen;
00058   };
00059  
00060   class ROSAgent : public Agent {
00061   public:
00062     typedef boost::shared_ptr<ROSAgent> ROSAgentPtr;
00063 
00064     ROSAgent();
00065 
00066     virtual ~ROSAgent() {};
00067 
00068     void init(ros::NodeHandle private_nh, tf::TransformListener* tf);
00069 
00070     void initParams(ros::NodeHandle private_nh);
00071     
00072     void initAsMe(tf::TransformListener* tf);
00073     void initCommon(ros::NodeHandle nh);
00074 
00075     void computeNewVelocity(Vector2 pref_velocity, geometry_msgs::Twist& cmd_vel);
00076 
00077 
00078     void computeOrcaVelocity(Vector2 pref_velocity);
00079     void computeClearpathVelocity(Vector2 pref_velocity);
00080     void computeSampledVelocity(Vector2 pref_velocity);
00081 
00082     
00083     void addNHConstraints(double min_dist, Vector2 pref_velocity);
00084 
00085     void computeObstacles();
00086 
00087     
00088     bool compareNeighborsPositions(const AgentPtr& agent1, const AgentPtr& agent2); 
00089 
00090     bool compareConvexHullPointsPosition(const ConvexHullPoint& p1, const ConvexHullPoint& p2);
00091     bool compareVectorPosition(const collvoid::Vector2& v1, const collvoid::Vector2& v2);
00092 
00093     void sortObstacleLines();
00094 
00095     collvoid::Vector2 LineSegmentToLineSegmentIntersection(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4);
00096 
00097     bool pointInNeighbor(collvoid::Vector2& point);
00098 
00099     double getDistToFootprint(collvoid::Vector2& point);
00100     void computeObstacleLine(Vector2& point);
00101     void createObstacleLine(std::vector<Vector2>& own_footprint, Vector2& obst1, Vector2& obst2);
00102 
00103   
00104     void setFootprint(geometry_msgs::PolygonStamped footprint);
00105     void setFootprintRadius(double footprint_radius); 
00106     void setMinkowskiFootprintVector2(geometry_msgs::PolygonStamped minkowski_footprint); 
00107 
00108     void setIsHoloRobot(bool holo_robot); 
00109     void setRobotBaseFrame(std::string base_link);
00110     void setGlobalFrame(std::string global_frame);
00111     void setId(std::string id);
00112 
00113     void setMaxVelWithObstacles(double max_vel_with_obstacles);
00114     void setWheelBase(double wheel_base);
00115 
00116     void setAccelerationConstraints(double acc_lim_x, double acc_lim_y, double acc_lim_th);
00117 
00118     void setMinMaxSpeeds(double min_vel_x, double max_vel_x, double min_vel_y, double max_vel_y, double min_vel_th, double max_vel_th, double min_vel_th_inplace);
00119     
00120 
00121     void setPublishPositionsPeriod(double publish_positions_period);
00122     void setPublishMePeriod(double publish_me_period);
00123 
00124     void setTimeToHolo(double time_to_holo);
00125     void setTimeHorizonObst(double time_horizon_obst);
00126     void setMinMaxErrorHolo(double min_error_holo, double max_error_holo);
00127     void setDeleteObservations(bool delete_observations);
00128     void setThresholdLastSeen(double threshold_last_seen); //implement!!
00129     void setLocalizationEps(double eps);
00130 
00131     void setTypeVO(int type_vo);
00132 
00133     void setOrca(bool orca);
00134     void setClearpath(bool clearpath);
00135     void setConvex(bool convex);
00136     void setUseTruncation(bool use_truncation);
00137     void setNumSamples(int num_samples);
00138         
00139     bool isHoloRobot();
00140     ros::Time lastSeen();
00141 
00142 
00143     
00144     void positionShareCallback(const collvoid_msgs::PoseTwistWithCovariance::ConstPtr& msg); 
00145     void amclPoseArrayWeightedCallback(const amcl::PoseArrayWeighted::ConstPtr& msg);
00146     void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
00147 
00148     void updateAllNeighbors();
00149     void setAgentParams(ROSAgent* agent);
00150 
00151     
00152     double vMaxAng();
00153 
00154     void publishMePoseTwist();
00155 
00156     geometry_msgs::PolygonStamped createFootprintMsgFromVector2(const std::vector<Vector2>& footprint);
00157     std::vector<Vector2> rotateFootprint(const std::vector<Vector2>& footprint, double angle);
00158     
00159     geometry_msgs::PoseStamped transformMapPoseToBaseLink(geometry_msgs::PoseStamped in);
00160 
00161     void computeNewLocUncertainty();
00162     
00163     void computeNewMinkowskiFootprint();
00164 
00165     void baseScanCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
00166 
00167     //config
00168     double publish_positions_period_;
00169     double publish_me_period_;
00170 
00171     double threshold_last_seen_;
00172     int num_samples_;
00173     
00174     //helpers
00175     ros::Time last_time_positions_published_;
00176     ros::Time last_time_me_published_;
00177     
00178     //NH stuff
00179     double min_error_holo_;
00180     double max_error_holo_;
00181     bool holo_robot_;
00182     double time_to_holo_;
00183     
00184     //ORCA stuff
00185     double max_vel_with_obstacles_;
00186     collvoid::Vector2 holo_velocity_;
00187 
00188 
00189     //Obstacles
00190     laser_geometry::LaserProjection projector_;
00191 
00192     //Obstacles
00193     std::vector<Obstacle> obstacles_from_laser_;
00194     message_filters::Subscriber<sensor_msgs::LaserScan> laser_scan_sub_;
00195     boost::shared_ptr<tf::MessageFilter<sensor_msgs::LaserScan> > laser_notifier;
00196     
00197     double min_dist_obst_;
00198       
00199     //obstacles
00200     bool delete_observations_;
00201     bool use_obstacles_;
00202     std::vector<collvoid::Vector2> obstacle_points_;
00203     double time_horizon_obst_;
00204 
00205     //Agent description
00206     std::string id_;
00207     std::string base_frame_, global_frame_;
00208     double wheel_base_;
00209     geometry_msgs::PolygonStamped footprint_msg_;
00210     double acc_lim_x_, acc_lim_y_, acc_lim_th_;
00211     double min_vel_x_, max_vel_x_, min_vel_y_, max_vel_y_, max_vel_th_, min_vel_th_, min_vel_th_inplace_;
00212     double footprint_radius_;
00213 
00214 
00215     bool standalone_;
00216     //set automatically
00217     bool initialized_;
00218     bool has_polygon_footprint_;
00219 
00220     std::vector< std::pair< collvoid::Vector2,collvoid::Vector2 > > footprint_lines_;
00221 
00222     
00223     ros::Time last_seen_;
00224     nav_msgs::Odometry base_odom_; 
00225     
00226     //LOC uncertatiny
00227     double eps_;
00228     double cur_loc_unc_radius_;
00229 
00230     //COLLVOID
00231     std::vector<Vector2> minkowski_footprint_;
00232     std::vector<std::pair<double, geometry_msgs::PoseStamped> > pose_array_weighted_;
00233 
00234     boost::mutex me_lock_, obstacle_lock_, neighbors_lock_, convex_lock_;
00235 
00236     //me stuff
00237     tf::TransformListener* tf_;
00238     
00239     //subscribers and publishers
00240     ros::Publisher lines_pub_, neighbors_pub_, polygon_pub_, vo_pub_, me_pub_, samples_pub_, speed_pub_, position_share_pub_, obstacles_pub_;
00241     ros::Subscriber amcl_posearray_sub_, position_share_sub_, odom_sub_;
00242 
00243     
00244   };
00245 
00246     typedef boost::shared_ptr<ROSAgent> ROSAgentPtr;
00247 
00248 
00249 }
00250 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


collvoid_local_planner
Author(s): Daniel Claes
autogenerated on Sun Aug 25 2013 10:10:23