test_optim_node.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2017.
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the institute nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author: Christoph Rösmann
37  *********************************************************************/
38 
40 
42 #include <visualization_msgs/Marker.h>
43 
44 #include <boost/shared_ptr.hpp>
45 #include <boost/make_shared.hpp>
46 
47 
48 using namespace teb_local_planner; // it is ok here to import everything for testing purposes
49 
50 // ============= Global Variables ================
51 // Ok global variables are bad, but here we only have a simple testing node.
54 std::vector<ObstaclePtr> obst_vector;
61 std::vector<ros::Subscriber> obst_vel_subs;
62 unsigned int no_fixed_obstacles;
63 
64 // =========== Function declarations =============
65 void CB_mainCycle(const ros::TimerEvent& e);
66 void CB_publishCycle(const ros::TimerEvent& e);
67 void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level);
68 void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg);
69 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);
70 void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
71 void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg);
72 void CB_via_points(const nav_msgs::Path::ConstPtr& via_points_msg);
73 void CB_setObstacleVelocity(const geometry_msgs::TwistConstPtr& twist_msg, const unsigned int id);
74 
75 
76 // =============== Main function =================
77 int main( int argc, char** argv )
78 {
79  ros::init(argc, argv, "test_optim_node");
80  ros::NodeHandle n("~");
81 
82 
83  // load ros parameters from node handle
85 
86  ros::Timer cycle_timer = n.createTimer(ros::Duration(0.025), CB_mainCycle);
87  ros::Timer publish_timer = n.createTimer(ros::Duration(0.1), CB_publishCycle);
88 
89  // setup dynamic reconfigure
90  dynamic_recfg = boost::make_shared< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> >(n);
91  dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig>::CallbackType cb = boost::bind(CB_reconfigure, _1, _2);
92  dynamic_recfg->setCallback(cb);
93 
94  // setup callback for custom obstacles
95  custom_obst_sub = n.subscribe("obstacles", 1, CB_customObstacle);
96 
97  // setup callback for clicked points (in rviz) that are considered as via-points
98  clicked_points_sub = n.subscribe("/clicked_point", 5, CB_clicked_points);
99 
100  // setup callback for via-points (callback overwrites previously set via-points)
101  via_points_sub = n.subscribe("via_points", 1, CB_via_points);
102 
103  // interactive marker server for simulated dynamic obstacles
104  interactive_markers::InteractiveMarkerServer marker_server("marker_obstacles");
105 
106  obst_vector.push_back( boost::make_shared<PointObstacle>(-3,1) );
107  obst_vector.push_back( boost::make_shared<PointObstacle>(6,2) );
108  obst_vector.push_back( boost::make_shared<PointObstacle>(0,0.1) );
109 // obst_vector.push_back( boost::make_shared<LineObstacle>(1,1.5,1,-1.5) ); //90 deg
110 // obst_vector.push_back( boost::make_shared<LineObstacle>(1,0,-1,0) ); //180 deg
111 // obst_vector.push_back( boost::make_shared<PointObstacle>(-1.5,-0.5) );
112 
113  // Dynamic obstacles
114  Eigen::Vector2d vel (0.1, -0.3);
115  obst_vector.at(0)->setCentroidVelocity(vel);
116  vel = Eigen::Vector2d(-0.3, -0.2);
117  obst_vector.at(1)->setCentroidVelocity(vel);
118 
119  /*
120  PolygonObstacle* polyobst = new PolygonObstacle;
121  polyobst->pushBackVertex(1, -1);
122  polyobst->pushBackVertex(0, 1);
123  polyobst->pushBackVertex(1, 1);
124  polyobst->pushBackVertex(2, 1);
125 
126  polyobst->finalizePolygon();
127  obst_vector.emplace_back(polyobst);
128  */
129 
130  for (unsigned int i=0; i<obst_vector.size(); ++i)
131  {
132  // setup callbacks for setting obstacle velocities
133  std::string topic = "/test_optim_node/obstacle_" + std::to_string(i) + "/cmd_vel";
134  obst_vel_subs.push_back(n.subscribe<geometry_msgs::Twist>(topic, 1, boost::bind(&CB_setObstacleVelocity, _1, i)));
135 
136  //CreateInteractiveMarker(obst_vector.at(i)[0],obst_vector.at(i)[1],i,&marker_server, &CB_obstacle_marker);
137  // Add interactive markers for all point obstacles
138  boost::shared_ptr<PointObstacle> pobst = boost::dynamic_pointer_cast<PointObstacle>(obst_vector.at(i));
139  if (pobst)
140  {
141  CreateInteractiveMarker(pobst->x(),pobst->y(),i, config.map_frame, &marker_server, &CB_obstacle_marker);
142  }
143  }
144  marker_server.applyChanges();
145 
146  // Setup visualization
147  visual = TebVisualizationPtr(new TebVisualization(n, config));
148 
149  // Setup robot shape model
151 
152  // Setup planner (homotopy class planning or just the local teb planner)
154  planner = PlannerInterfacePtr(new HomotopyClassPlanner(config, &obst_vector, robot_model, visual, &via_points));
155  else
156  planner = PlannerInterfacePtr(new TebOptimalPlanner(config, &obst_vector, robot_model, visual, &via_points));
157 
158 
160  ros::spin();
161 
162  return 0;
163 }
164 
165 // Planning loop
167 {
168  planner->plan(PoseSE2(-4,0,0), PoseSE2(4,0,0)); // hardcoded start and goal for testing purposes
169 }
170 
171 // Visualization loop
173 {
174  planner->visualize();
175  visual->publishObstacles(obst_vector);
176  visual->publishViaPoints(via_points);
177 }
178 
179 void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level)
180 {
181  config.reconfigure(reconfig);
182 }
183 
184 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)
185 {
186  // create an interactive marker for our server
187  visualization_msgs::InteractiveMarker i_marker;
188  i_marker.header.frame_id = frame;
189  i_marker.header.stamp = ros::Time::now();
190  std::ostringstream oss;
191  //oss << "obstacle" << id;
192  oss << id;
193  i_marker.name = oss.str();
194  i_marker.description = "Obstacle";
195  i_marker.pose.position.x = init_x;
196  i_marker.pose.position.y = init_y;
197  i_marker.pose.orientation.w = 1.0f; // make quaternion normalized
198 
199  // create a grey box marker
200  visualization_msgs::Marker box_marker;
201  box_marker.type = visualization_msgs::Marker::CUBE;
202  box_marker.id = id;
203  box_marker.scale.x = 0.2;
204  box_marker.scale.y = 0.2;
205  box_marker.scale.z = 0.2;
206  box_marker.color.r = 0.5;
207  box_marker.color.g = 0.5;
208  box_marker.color.b = 0.5;
209  box_marker.color.a = 1.0;
210  box_marker.pose.orientation.w = 1.0f; // make quaternion normalized
211 
212  // create a non-interactive control which contains the box
213  visualization_msgs::InteractiveMarkerControl box_control;
214  box_control.always_visible = true;
215  box_control.markers.push_back( box_marker );
216 
217  // add the control to the interactive marker
218  i_marker.controls.push_back( box_control );
219 
220  // create a control which will move the box, rviz will insert 2 arrows
221  visualization_msgs::InteractiveMarkerControl move_control;
222  move_control.name = "move_x";
223  move_control.orientation.w = 0.707107f;
224  move_control.orientation.x = 0;
225  move_control.orientation.y = 0.707107f;
226  move_control.orientation.z = 0;
227  move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
228 
229 
230  // add the control to the interactive marker
231  i_marker.controls.push_back(move_control);
232 
233  // add the interactive marker to our collection
234  marker_server->insert(i_marker);
235  marker_server->setCallback(i_marker.name,feedback_cb);
236 }
237 
238 void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
239 {
240  std::stringstream ss(feedback->marker_name);
241  unsigned int index;
242  ss >> index;
243 
244  if (index>=no_fixed_obstacles)
245  return;
246  PointObstacle* pobst = static_cast<PointObstacle*>(obst_vector.at(index).get());
247  pobst->position() = Eigen::Vector2d(feedback->pose.position.x,feedback->pose.position.y);
248 }
249 
250 void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg)
251 {
252  // resize such that the vector contains only the fixed obstacles specified inside the main function
254 
255  // Add custom obstacles obtained via message (assume that all obstacles coordiantes are specified in the default planning frame)
256  for (size_t i = 0; i < obst_msg->obstacles.size(); ++i)
257  {
258  if (obst_msg->obstacles.at(i).polygon.points.size() == 1 )
259  {
260  if (obst_msg->obstacles.at(i).radius == 0)
261  {
262  obst_vector.push_back(ObstaclePtr(new PointObstacle( obst_msg->obstacles.at(i).polygon.points.front().x,
263  obst_msg->obstacles.at(i).polygon.points.front().y )));
264  }
265  else
266  {
267  obst_vector.push_back(ObstaclePtr(new CircularObstacle( obst_msg->obstacles.at(i).polygon.points.front().x,
268  obst_msg->obstacles.at(i).polygon.points.front().y,
269  obst_msg->obstacles.at(i).radius )));
270  }
271  }
272  else
273  {
274  PolygonObstacle* polyobst = new PolygonObstacle;
275  for (size_t j=0; j<obst_msg->obstacles.at(i).polygon.points.size(); ++j)
276  {
277  polyobst->pushBackVertex( obst_msg->obstacles.at(i).polygon.points[j].x,
278  obst_msg->obstacles.at(i).polygon.points[j].y );
279  }
280  polyobst->finalizePolygon();
281  obst_vector.push_back(ObstaclePtr(polyobst));
282  }
283 
284  if(!obst_vector.empty())
285  obst_vector.back()->setCentroidVelocity(obst_msg->obstacles.at(i).velocities, obst_msg->obstacles.at(i).orientation);
286  }
287 }
288 
289 
290 void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg)
291 {
292  // we assume for simplicity that the fixed frame is already the map/planning frame
293  // consider clicked points as via-points
294  via_points.push_back( Eigen::Vector2d(point_msg->point.x, point_msg->point.y) );
295  ROS_INFO_STREAM("Via-point (" << point_msg->point.x << "," << point_msg->point.y << ") added.");
296  if (config.optim.weight_viapoint<=0)
297  ROS_WARN("Note, via-points are deactivated, since 'weight_via_point' <= 0");
298 }
299 
300 void CB_via_points(const nav_msgs::Path::ConstPtr& via_points_msg)
301 {
302  ROS_INFO_ONCE("Via-points received. This message is printed once.");
303  via_points.clear();
304  for (const geometry_msgs::PoseStamped& pose : via_points_msg->poses)
305  {
306  via_points.emplace_back(pose.pose.position.x, pose.pose.position.y);
307  }
308 }
309 
310 void CB_setObstacleVelocity(const geometry_msgs::TwistConstPtr& twist_msg, const unsigned int id)
311 {
312  if (id >= obst_vector.size())
313  {
314  ROS_WARN("Cannot set velocity: unknown obstacle id.");
315  return;
316  }
317 
318  Eigen::Vector2d vel (twist_msg->linear.x, twist_msg->linear.y);
319  obst_vector.at(id)->setCentroidVelocity(vel);
320 }
boost::shared_ptr< PlannerInterface > PlannerInterfacePtr
Abbrev. for shared instances of PlannerInterface or it&#39;s subclasses.
Implements a 2D circular obstacle (point obstacle plus radius)
Definition: obstacles.h:447
std::string map_frame
Global planning frame.
Definition: teb_config.h:66
void finalizePolygon()
Call finalizePolygon after the polygon is created with the help of pushBackVertex() methods...
Definition: obstacles.h:926
TebConfig config
unsigned int no_fixed_obstacles
#define ROS_INFO_ONCE(...)
void CB_setObstacleVelocity(const geometry_msgs::TwistConstPtr &twist_msg, const unsigned int id)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void CB_reconfigure(TebLocalPlannerReconfigureConfig &reconfig, uint32_t level)
Config class for the teb_local_planner and its components.
Definition: teb_config.h:61
ros::Subscriber custom_obst_sub
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Local planner that explores alternative homotopy classes, create a plan for each alternative and fina...
static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle &nh)
Get the current robot footprint/contour model.
void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
TebVisualizationPtr visual
#define ROS_WARN(...)
boost::function< void(const FeedbackConstPtr &) > FeedbackCallback
Implements a polygon obstacle with an arbitrary number of vertices.
Definition: obstacles.h:748
void CB_via_points(const nav_msgs::Path::ConstPtr &via_points_msg)
ROSCPP_DECL void spin(Spinner &spinner)
boost::shared_ptr< Obstacle > ObstaclePtr
Abbrev. for shared obstacle pointers.
Definition: obstacles.h:293
void CB_mainCycle(const ros::TimerEvent &e)
void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr &obst_msg)
const Eigen::Vector2d & position() const
Return the current position of the obstacle (read-only)
Definition: obstacles.h:418
void reconfigure(TebLocalPlannerReconfigureConfig &cfg)
Reconfigure parameters from the dynamic_reconfigure config. Change parameters dynamically (e...
Definition: teb_config.cpp:170
void loadRosParamFromNodeHandle(const ros::NodeHandle &nh)
Load parmeters from the ros param server.
Definition: teb_config.cpp:44
struct teb_local_planner::TebConfig::HomotopyClasses hcp
boost::shared_ptr< TebVisualization > TebVisualizationPtr
Abbrev. for shared instances of the TebVisualization.
Implements a 2D point obstacle.
Definition: obstacles.h:305
PlannerInterfacePtr planner
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
int main(int argc, char **argv)
bool enable_homotopy_class_planning
Activate homotopy class planning (Requires much more resources that simple planning, since multiple trajectories are optimized at once).
Definition: teb_config.h:171
std::vector< ros::Subscriber > obst_vel_subs
void pushBackVertex(const Eigen::Ref< const Eigen::Vector2d > &vertex)
Add a vertex to the polygon (edge-point)
Definition: obstacles.h:904
ros::Subscriber via_points_sub
This class implements a pose in the domain SE2: The pose consist of the position x and y and an orie...
Definition: pose_se2.h:57
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)
INTERACTIVE_MARKERS_PUBLIC void insert(const visualization_msgs::InteractiveMarker &int_marker)
#define ROS_INFO_STREAM(args)
INTERACTIVE_MARKERS_PUBLIC void applyChanges()
INTERACTIVE_MARKERS_PUBLIC bool setCallback(const std::string &name, FeedbackCallback feedback_cb, uint8_t feedback_type=DEFAULT_FEEDBACK_CB)
std::vector< ObstaclePtr > obst_vector
This class optimizes an internal Timed Elastic Band trajectory using the g2o-framework.
static Time now()
ros::Subscriber clicked_points_sub
double weight_viapoint
Optimization weight for minimizing the distance to via-points.
Definition: teb_config.h:161
ViaPointContainer via_points
void CB_clicked_points(const geometry_msgs::PointStampedConstPtr &point_msg)
struct teb_local_planner::TebConfig::Optimization optim
Optimization related parameters.
void CB_publishCycle(const ros::TimerEvent &e)
std::vector< Eigen::Vector2d, Eigen::aligned_allocator< Eigen::Vector2d > > ViaPointContainer
Typedef for a container storing via-points.
boost::shared_ptr< dynamic_reconfigure::Server< TebLocalPlannerReconfigureConfig > > dynamic_recfg


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 3 2020 04:03:08