test_optim_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2016,
00006  *  TU Dortmund - Institute of Control Theory and Systems Engineering.
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the institute nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * Author: Christoph Rösmann
00037  *********************************************************************/
00038 
00039 #include <teb_local_planner/teb_local_planner_ros.h>
00040 
00041 #include <interactive_markers/interactive_marker_server.h>
00042 #include <visualization_msgs/Marker.h>
00043 
00044 #include <boost/shared_ptr.hpp>
00045 #include <boost/make_shared.hpp>
00046 
00047 
00048 using namespace teb_local_planner; // it is ok here to import everything for testing purposes
00049 
00050 // ============= Global Variables ================
00051 // Ok global variables are bad, but here we only have a simple testing node.
00052 PlannerInterfacePtr planner;
00053 TebVisualizationPtr visual;
00054 std::vector<ObstaclePtr> obst_vector;
00055 ViaPointContainer via_points;
00056 TebConfig config;
00057 boost::shared_ptr< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> > dynamic_recfg;
00058 ros::Subscriber custom_obst_sub;
00059 ros::Subscriber clicked_points_sub;
00060 unsigned int no_fixed_obstacles;
00061 
00062 // =========== Function declarations =============
00063 void CB_mainCycle(const ros::TimerEvent& e);
00064 void CB_publishCycle(const ros::TimerEvent& e);
00065 void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level);
00066 void CB_customObstacle(const teb_local_planner::ObstacleMsg::ConstPtr& obst_msg);
00067 void CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame, interactive_markers::InteractiveMarkerServer* marker_server, interactive_markers::InteractiveMarkerServer::FeedbackCallback feedback_cb);
00068 void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
00069 void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg);
00070 
00071 
00072 // =============== Main function =================
00073 int main( int argc, char** argv )
00074 {
00075   ros::init(argc, argv, "test_optim_node");
00076   ros::NodeHandle n("~");
00077  
00078   
00079   // load ros parameters from node handle
00080   config.loadRosParamFromNodeHandle(n);
00081  
00082   ros::Timer cycle_timer = n.createTimer(ros::Duration(0.025), CB_mainCycle);
00083   ros::Timer publish_timer = n.createTimer(ros::Duration(0.1), CB_publishCycle);
00084   
00085   // setup dynamic reconfigure
00086   dynamic_recfg = boost::make_shared< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> >(n);
00087   dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig>::CallbackType cb = boost::bind(CB_reconfigure, _1, _2);
00088   dynamic_recfg->setCallback(cb);
00089   
00090   // setup callback for custom obstacles
00091   custom_obst_sub = n.subscribe("obstacles", 1, CB_customObstacle);
00092   
00093   // setup callback for clicked points (in rviz) that are considered as via-points
00094   clicked_points_sub = n.subscribe("/clicked_point", 5, CB_clicked_points);
00095   
00096   // interactive marker server for simulated dynamic obstacles
00097   interactive_markers::InteractiveMarkerServer marker_server("marker_obstacles");
00098 
00099   obst_vector.push_back( boost::make_shared<PointObstacle>(-5,1) );
00100   obst_vector.push_back( boost::make_shared<PointObstacle>(-5,2.2) );
00101   obst_vector.push_back( boost::make_shared<PointObstacle>(0,0.1) );
00102 //  obst_vector.push_back( boost::make_shared<LineObstacle>(1,1.5,1,-1.5) ); //90 deg
00103 //  obst_vector.push_back( boost::make_shared<LineObstacle>(1,0,-1,0) ); //180 deg
00104 //  obst_vector.push_back( boost::make_shared<PointObstacle>(-1.5,-0.5) );
00105   
00106   /*
00107   PolygonObstacle* polyobst = new PolygonObstacle;
00108   polyobst->pushBackVertex(1, -1);
00109   polyobst->pushBackVertex(0, 1);
00110   polyobst->pushBackVertex(1, 1);
00111   polyobst->pushBackVertex(2, 1);
00112  
00113   polyobst->finalizePolygon();
00114   obst_vector.emplace_back(polyobst);
00115   */
00116   
00117   for (unsigned int i=0; i<obst_vector.size(); ++i)
00118   {
00119     //CreateInteractiveMarker(obst_vector.at(i)[0],obst_vector.at(i)[1],i,&marker_server, &CB_obstacle_marker);  
00120     // Add interactive markers for all point obstacles
00121     boost::shared_ptr<PointObstacle> pobst = boost::dynamic_pointer_cast<PointObstacle>(obst_vector.at(i));
00122     if (pobst)
00123     {
00124       CreateInteractiveMarker(pobst->x(),pobst->y(),i, config.map_frame, &marker_server, &CB_obstacle_marker);  
00125     }
00126   }
00127   marker_server.applyChanges();
00128   
00129   
00130   // Add via points
00131   //via_points.push_back( Eigen::Vector2d( 0.0, 1.5 ) );
00132   
00133   // Setup visualization
00134   visual = TebVisualizationPtr(new TebVisualization(n, config));
00135   
00136   // Setup robot shape model
00137   RobotFootprintModelPtr robot_model = TebLocalPlannerROS::getRobotFootprintFromParamServer(n);
00138   
00139   // Setup planner (homotopy class planning or just the local teb planner)
00140   if (config.hcp.enable_homotopy_class_planning)
00141     planner = PlannerInterfacePtr(new HomotopyClassPlanner(config, &obst_vector, robot_model, visual, &via_points));
00142   else
00143     planner = PlannerInterfacePtr(new TebOptimalPlanner(config, &obst_vector, robot_model, visual, &via_points));
00144   
00145 
00146   no_fixed_obstacles = obst_vector.size();
00147   ros::spin();
00148 
00149   return 0;
00150 }
00151 
00152 // Planning loop
00153 void CB_mainCycle(const ros::TimerEvent& e)
00154 {
00155   planner->plan(PoseSE2(-4,0,0), PoseSE2(4,0,0), Eigen::Vector2d(0,0)); // hardcoded start and goal for testing purposes
00156 }
00157 
00158 // Visualization loop
00159 void CB_publishCycle(const ros::TimerEvent& e)
00160 {
00161   planner->visualize();
00162   visual->publishObstacles(obst_vector);
00163   visual->publishViaPoints(via_points);
00164 }
00165 
00166 void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level)
00167 {
00168   config.reconfigure(reconfig);
00169 }
00170 
00171 void CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame, interactive_markers::InteractiveMarkerServer* marker_server, interactive_markers::InteractiveMarkerServer::FeedbackCallback feedback_cb)
00172 {
00173   // create an interactive marker for our server
00174   visualization_msgs::InteractiveMarker i_marker;
00175   i_marker.header.frame_id = frame;
00176   i_marker.header.stamp = ros::Time::now();
00177   std::ostringstream oss;
00178   //oss << "obstacle" << id;
00179   oss << id;
00180   i_marker.name = oss.str();
00181   i_marker.description = "Obstacle";
00182   i_marker.pose.position.x = init_x;
00183   i_marker.pose.position.y = init_y;
00184 
00185   // create a grey box marker
00186   visualization_msgs::Marker box_marker;
00187   box_marker.type = visualization_msgs::Marker::CUBE;
00188   box_marker.id = id;
00189   box_marker.scale.x = 0.2;
00190   box_marker.scale.y = 0.2;
00191   box_marker.scale.z = 0.2;
00192   box_marker.color.r = 0.5;
00193   box_marker.color.g = 0.5;
00194   box_marker.color.b = 0.5;
00195   box_marker.color.a = 1.0;
00196 
00197   // create a non-interactive control which contains the box
00198   visualization_msgs::InteractiveMarkerControl box_control;
00199   box_control.always_visible = true;
00200   box_control.markers.push_back( box_marker );
00201 
00202   // add the control to the interactive marker
00203   i_marker.controls.push_back( box_control );
00204 
00205   // create a control which will move the box, rviz will insert 2 arrows
00206   visualization_msgs::InteractiveMarkerControl move_control;
00207   move_control.name = "move_x";
00208   move_control.orientation.w = 1;
00209   move_control.orientation.x = 0;
00210   move_control.orientation.y = 1;
00211   move_control.orientation.z = 0;
00212   move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
00213 
00214 
00215   // add the control to the interactive marker
00216   i_marker.controls.push_back(move_control);
00217 
00218   // add the interactive marker to our collection
00219   marker_server->insert(i_marker);
00220   marker_server->setCallback(i_marker.name,feedback_cb);
00221 }
00222 
00223 void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00224 {
00225   std::stringstream ss(feedback->marker_name);
00226   unsigned int index;
00227   ss >> index;
00228   
00229   if (index>=no_fixed_obstacles) 
00230     return;
00231   PointObstacle* pobst = static_cast<PointObstacle*>(obst_vector.at(index).get());
00232   pobst->position() = Eigen::Vector2d(feedback->pose.position.x,feedback->pose.position.y);       
00233 }
00234 
00235 void CB_customObstacle(const teb_local_planner::ObstacleMsg::ConstPtr& obst_msg)
00236 {
00237   // resize such that the vector contains only the fixed obstacles specified inside the main function
00238   obst_vector.resize(no_fixed_obstacles);
00239   
00240   // Add custom obstacles obtained via message (assume that all obstacles coordiantes are specified in the default planning frame)  
00241   for (std::vector<geometry_msgs::PolygonStamped>::const_iterator obst_it = obst_msg->obstacles.begin(); obst_it != obst_msg->obstacles.end(); ++obst_it)
00242   {
00243     if (obst_it->polygon.points.size() == 1 )
00244     {
00245       obst_vector.push_back(ObstaclePtr(new PointObstacle( obst_it->polygon.points.front().x, obst_it->polygon.points.front().y )));
00246     }
00247     else
00248     {
00249       PolygonObstacle* polyobst = new PolygonObstacle;
00250       for (int i=0; i<(int)obst_it->polygon.points.size(); ++i)
00251       {
00252         polyobst->pushBackVertex( obst_it->polygon.points[i].x, obst_it->polygon.points[i].y );
00253       }
00254       polyobst->finalizePolygon();
00255       obst_vector.push_back(ObstaclePtr(polyobst));
00256     }
00257   }
00258 }
00259 
00260 
00261 void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg)
00262 {
00263   // we assume for simplicity that the fixed frame is already the map/planning frame
00264   // consider clicked points as via-points
00265   via_points.push_back( Eigen::Vector2d(point_msg->point.x, point_msg->point.y) );
00266   ROS_INFO_STREAM("Via-point (" << point_msg->point.x << "," << point_msg->point.y << ") added.");
00267   if (config.optim.weight_viapoint<=0)
00268     ROS_WARN("Note, via-points are deactivated, since 'weight_via_point' <= 0");
00269 }


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Mon Oct 24 2016 05:31:15