From 6ac179ff65aff2b6423ffd8fc896d06987676685 Mon Sep 17 00:00:00 2001 From: Christian Mai Date: Fri, 30 Nov 2018 14:38:12 +0100 Subject: [PATCH 1/5] First attempt at via point poses modified: cfg/TebLocalPlannerReconfigure.cfg modified: include/teb_local_planner/g2o_types/edge_via_point.h modified: include/teb_local_planner/optimal_planner.h modified: include/teb_local_planner/teb_config.h modified: src/homotopy_class_planner.cpp modified: src/optimal_planner.cpp modified: src/teb_config.cpp modified: src/test_optim_node.cpp modified: src/visualization.cpp --- cfg/TebLocalPlannerReconfigure.cfg | 4 ++++ include/teb_local_planner/g2o_types/edge_via_point.h | 9 +++++---- include/teb_local_planner/optimal_planner.h | 2 +- include/teb_local_planner/teb_config.h | 2 ++ src/homotopy_class_planner.cpp | 2 +- src/optimal_planner.cpp | 9 ++++++--- src/teb_config.cpp | 2 ++ src/test_optim_node.cpp | 2 +- src/visualization.cpp | 2 +- 9 files changed, 23 insertions(+), 11 deletions(-) diff --git a/cfg/TebLocalPlannerReconfigure.cfg b/cfg/TebLocalPlannerReconfigure.cfg index b0066261..e2be5822 100755 --- a/cfg/TebLocalPlannerReconfigure.cfg +++ b/cfg/TebLocalPlannerReconfigure.cfg @@ -269,6 +269,10 @@ grp_optimization.add("weight_dynamic_obstacle_inflation", double_t, 0, grp_optimization.add("weight_viapoint", double_t, 0, "Optimization weight for minimizing the distance to via-points", 1, 0, 1000) + +grp_optimization.add("weight_viapoint_orientation", double_t, 0, + "Optimization weight for minimizing the orientation error w.r.t via-points", + 1, 0, 1000) grp_optimization.add("weight_adapt_factor", double_t, 0, "Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new: weight_old * factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.", diff --git a/include/teb_local_planner/g2o_types/edge_via_point.h b/include/teb_local_planner/g2o_types/edge_via_point.h index 9ddefcb8..41d9c2f6 100755 --- a/include/teb_local_planner/g2o_types/edge_via_point.h +++ b/include/teb_local_planner/g2o_types/edge_via_point.h @@ -63,7 +63,7 @@ namespace teb_local_planner * @see TebOptimalPlanner::AddEdgesViaPoints * @remarks Do not forget to call setTebConfig() and setViaPoint() */ -class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPose> +class EdgeViaPoint : public BaseTebUnaryEdge<2, const PoseSE2*, VertexPose> { public: @@ -83,7 +83,8 @@ class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPo ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig(), setViaPoint() on EdgeViaPoint()"); const VertexPose* bandpt = static_cast(_vertices[0]); - _error[0] = (bandpt->position() - *_measurement).norm(); + _error[0] = (bandpt->position() - _measurement->position()).norm(); // as in EdgeViaPoint + _error[1] = g2o::normalize_theta(bandpt->theta() - _measurement->theta()); ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeViaPoint::computeError() _error[0]=%f\n",_error[0]); } @@ -92,7 +93,7 @@ class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPo * @brief Set pointer to associated via point for the underlying cost function * @param via_point 2D position vector containing the position of the via point */ - void setViaPoint(const Eigen::Vector2d* via_point) + void setViaPoint(const PoseSE2* via_point) { _measurement = via_point; } @@ -102,7 +103,7 @@ class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPo * @param cfg TebConfig class * @param via_point 2D position vector containing the position of the via point */ - void setParameters(const TebConfig& cfg, const Eigen::Vector2d* via_point) + void setParameters(const TebConfig& cfg, const PoseSE2* via_point) { cfg_ = &cfg; _measurement = via_point; diff --git a/include/teb_local_planner/optimal_planner.h b/include/teb_local_planner/optimal_planner.h index 194d560b..afb99a69 100644 --- a/include/teb_local_planner/optimal_planner.h +++ b/include/teb_local_planner/optimal_planner.h @@ -89,7 +89,7 @@ typedef g2o::LinearSolverCSparse TEBLinearSolver //typedef g2o::LinearSolverCholmod TEBLinearSolver; //! Typedef for a container storing via-points -typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator > ViaPointContainer; +typedef std::vector< PoseSE2, Eigen::aligned_allocator > ViaPointContainer; /** diff --git a/include/teb_local_planner/teb_config.h b/include/teb_local_planner/teb_config.h index 98643956..63e5437d 100644 --- a/include/teb_local_planner/teb_config.h +++ b/include/teb_local_planner/teb_config.h @@ -154,6 +154,7 @@ class TebConfig double weight_dynamic_obstacle; //!< Optimization weight for satisfying a minimum separation from dynamic obstacles double weight_dynamic_obstacle_inflation; //!< Optimization weight for the inflation penalty of dynamic obstacles (should be small) double weight_viapoint; //!< Optimization weight for minimizing the distance to via-points + double weight_viapoint_orientation; //!< Optimization weight for minimizing the orientation error v.r.t via-points double weight_prefer_rotdir; //!< Optimization weight for preferring a specific turning direction (-> currently only activated if an oscillation is detected, see 'oscillation_recovery' double weight_adapt_factor; //!< Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new = weight_old*factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem. @@ -296,6 +297,7 @@ class TebConfig optim.weight_dynamic_obstacle = 50; optim.weight_dynamic_obstacle_inflation = 0.1; optim.weight_viapoint = 1; + optim.weight_viapoint_orientation = 1; optim.weight_prefer_rotdir = 50; optim.weight_adapt_factor = 2.0; diff --git a/src/homotopy_class_planner.cpp b/src/homotopy_class_planner.cpp index 589c17e1..03823a2b 100644 --- a/src/homotopy_class_planner.cpp +++ b/src/homotopy_class_planner.cpp @@ -295,7 +295,7 @@ void HomotopyClassPlanner::renewAndAnalyzeOldTebs(bool delete_detours) void HomotopyClassPlanner::updateReferenceTrajectoryViaPoints(bool all_trajectories) { - if ( (!all_trajectories && !initial_plan_) || !via_points_ || via_points_->empty() || cfg_->optim.weight_viapoint <= 0) + if ( (!all_trajectories && !initial_plan_) || !via_points_ || via_points_->empty() || (cfg_->optim.weight_viapoint <= 0 && cfg_->optim.weight_viapoint_orientation)) return; if(equivalence_classes_.size() < tebs_.size()) diff --git a/src/optimal_planner.cpp b/src/optimal_planner.cpp index bbc7e517..4a6f5304 100644 --- a/src/optimal_planner.cpp +++ b/src/optimal_planner.cpp @@ -664,7 +664,7 @@ void TebOptimalPlanner::AddEdgesDynamicObstacles(double weight_multiplier) void TebOptimalPlanner::AddEdgesViaPoints() { - if (cfg_->optim.weight_viapoint==0 || via_points_==NULL || via_points_->empty() ) + if ((cfg_->optim.weight_viapoint==0 && cfg_->optim.weight_viapoint_orientation==0)|| via_points_==NULL || via_points_->empty() ) return; // if weight equals zero skip adding edges! int start_pose_idx = 0; @@ -696,9 +696,12 @@ void TebOptimalPlanner::AddEdgesViaPoints() continue; // skip via points really close or behind the current robot pose } } - Eigen::Matrix information; - information.fill(cfg_->optim.weight_viapoint); + Eigen::Matrix information; + information(0,0) = cfg_->optim.weight_viapoint; + information(1,1) = cfg_->optim.weight_viapoint_orientation; + information(0,1) = information(1,0) = 0; + EdgeViaPoint* edge_viapoint = new EdgeViaPoint; edge_viapoint->setVertex(0,teb_.PoseVertex(index)); edge_viapoint->setInformation(information); diff --git a/src/teb_config.cpp b/src/teb_config.cpp index d9408ecf..e8d9d62d 100644 --- a/src/teb_config.cpp +++ b/src/teb_config.cpp @@ -119,6 +119,7 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh) nh.param("weight_dynamic_obstacle", optim.weight_dynamic_obstacle, optim.weight_dynamic_obstacle); nh.param("weight_dynamic_obstacle_inflation", optim.weight_dynamic_obstacle_inflation, optim.weight_dynamic_obstacle_inflation); nh.param("weight_viapoint", optim.weight_viapoint, optim.weight_viapoint); + nh.param("weight_viapoint_orientation", optim.weight_viapoint_orientation, optim.weight_viapoint_orientation); nh.param("weight_prefer_rotdir", optim.weight_prefer_rotdir, optim.weight_prefer_rotdir); nh.param("weight_adapt_factor", optim.weight_adapt_factor, optim.weight_adapt_factor); @@ -227,6 +228,7 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg) optim.weight_dynamic_obstacle = cfg.weight_dynamic_obstacle; optim.weight_dynamic_obstacle_inflation = cfg.weight_dynamic_obstacle_inflation; optim.weight_viapoint = cfg.weight_viapoint; + optim.weight_viapoint_orientation = cfg.weight_viapoint_orientation; optim.weight_adapt_factor = cfg.weight_adapt_factor; // Homotopy Class Planner diff --git a/src/test_optim_node.cpp b/src/test_optim_node.cpp index 5386d110..1765bed4 100644 --- a/src/test_optim_node.cpp +++ b/src/test_optim_node.cpp @@ -284,7 +284,7 @@ void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg) // consider clicked points as via-points via_points.push_back( Eigen::Vector2d(point_msg->point.x, point_msg->point.y) ); ROS_INFO_STREAM("Via-point (" << point_msg->point.x << "," << point_msg->point.y << ") added."); - if (config.optim.weight_viapoint<=0) + if (config.optim.weight_viapoint<=0 && config.optim.weight_viapoint_orientation<=0) ROS_WARN("Note, via-points are deactivated, since 'weight_via_point' <= 0"); } diff --git a/src/visualization.cpp b/src/visualization.cpp index 58d32cea..23091bca 100644 --- a/src/visualization.cpp +++ b/src/visualization.cpp @@ -294,7 +294,7 @@ void TebVisualization::publishObstacles(const ObstContainer& obstacles) const } -void TebVisualization::publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator >& via_points, const std::string& ns) const +void TebVisualization::publishViaPoints(const std::vector< PoseSE2, Eigen::aligned_allocator >& via_points, const std::string& ns) const { if ( via_points.empty() || printErrorWhenNotInitialized() ) return; From 068f5970863cfcbcbc3bd21424f987673b19f036 Mon Sep 17 00:00:00 2001 From: Christian Mai Date: Fri, 30 Nov 2018 15:46:03 +0100 Subject: [PATCH 2/5] modified: include/teb_local_planner/visualization.h modified: src/optimal_planner.cpp modified: src/teb_local_planner_ros.cpp --- CMakeLists.txt | 18 +- include/teb_local_planner/visualization.h | 2 +- src/optimal_planner.cpp | 3 +- src/teb_local_planner_ros.cpp | 22 +- src/test_optim_node.cpp | 311 ---------------------- 5 files changed, 32 insertions(+), 324 deletions(-) delete mode 100644 src/test_optim_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index d610e0f0..4634a723 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -203,13 +203,13 @@ target_link_libraries(teb_local_planner ## Build additional executables -add_executable(test_optim_node src/test_optim_node.cpp) +##add_executable(test_optim_node src/test_optim_node.cpp) -target_link_libraries(test_optim_node - teb_local_planner - ${EXTERNAL_LIBS} - ${catkin_LIBRARIES} -) +##target_link_libraries(test_optim_node +## teb_local_planner +## ${EXTERNAL_LIBS} +## ${catkin_LIBRARIES} +##) ############# @@ -230,9 +230,9 @@ target_link_libraries(test_optim_node install(TARGETS teb_local_planner LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) -install(TARGETS test_optim_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +##install(TARGETS test_optim_node +## RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +##) ## Mark cpp header files for installation install(DIRECTORY include/${PROJECT_NAME}/ diff --git a/include/teb_local_planner/visualization.h b/include/teb_local_planner/visualization.h index db14ed3a..cb4014ae 100644 --- a/include/teb_local_planner/visualization.h +++ b/include/teb_local_planner/visualization.h @@ -147,7 +147,7 @@ class TebVisualization * @brief Publish via-points to the ros topic \e ../../teb_markers * @param via_points via-point container */ - void publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator >& via_points, const std::string& ns = "ViaPoints") const; + void publishViaPoints(const std::vector< PoseSE2, Eigen::aligned_allocator >& via_points, const std::string& ns = "ViaPoints") const; /** * @brief Publish a boost::adjacency_list (boost's graph datatype) via markers. diff --git a/src/optimal_planner.cpp b/src/optimal_planner.cpp index 4a6f5304..6fda554d 100644 --- a/src/optimal_planner.cpp +++ b/src/optimal_planner.cpp @@ -675,8 +675,9 @@ void TebOptimalPlanner::AddEdgesViaPoints() for (ViaPointContainer::const_iterator vp_it = via_points_->begin(); vp_it != via_points_->end(); ++vp_it) { + PoseSE2 temp = *vp_it; - int index = teb_.findClosestTrajectoryPose(*vp_it, NULL, start_pose_idx); + int index = teb_.findClosestTrajectoryPose(temp.position(), NULL, start_pose_idx); if (cfg_->trajectory.via_points_ordered) start_pose_idx = index+2; // skip a point to have a DOF inbetween for further via-points diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp index dd65ad98..6c90c9ce 100644 --- a/src/teb_local_planner_ros.cpp +++ b/src/teb_local_planner_ros.cpp @@ -587,8 +587,17 @@ void TebLocalPlannerROS::updateViaPointsContainer(const std::vectorposes) { - via_points_.emplace_back(pose.pose.position.x, pose.pose.position.y); + tf::Quaternion q( + pose.pose.orientation.x, + pose.pose.orientation.y, + pose.pose.orientation.z, + pose.pose.orientation.w); + tf::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + + via_points_.emplace_back(pose.pose.position.x, pose.pose.position.y, yaw); } custom_via_points_active_ = !via_points_.empty(); } diff --git a/src/test_optim_node.cpp b/src/test_optim_node.cpp deleted file mode 100644 index 1765bed4..00000000 --- a/src/test_optim_node.cpp +++ /dev/null @@ -1,311 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017. - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include - -#include -#include - -#include -#include - - -using namespace teb_local_planner; // it is ok here to import everything for testing purposes - -// ============= Global Variables ================ -// Ok global variables are bad, but here we only have a simple testing node. -PlannerInterfacePtr planner; -TebVisualizationPtr visual; -std::vector obst_vector; -ViaPointContainer via_points; -TebConfig config; -boost::shared_ptr< dynamic_reconfigure::Server > dynamic_recfg; -ros::Subscriber custom_obst_sub; -ros::Subscriber via_points_sub; -ros::Subscriber clicked_points_sub; -std::vector obst_vel_subs; -unsigned int no_fixed_obstacles; - -// =========== Function declarations ============= -void CB_mainCycle(const ros::TimerEvent& e); -void CB_publishCycle(const ros::TimerEvent& e); -void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level); -void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg); -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); -void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); -void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg); -void CB_via_points(const nav_msgs::Path::ConstPtr& via_points_msg); -void CB_setObstacleVelocity(const geometry_msgs::TwistConstPtr& twist_msg, const unsigned int id); - - -// =============== Main function ================= -int main( int argc, char** argv ) -{ - ros::init(argc, argv, "test_optim_node"); - ros::NodeHandle n("~"); - - - // load ros parameters from node handle - config.loadRosParamFromNodeHandle(n); - - ros::Timer cycle_timer = n.createTimer(ros::Duration(0.025), CB_mainCycle); - ros::Timer publish_timer = n.createTimer(ros::Duration(0.1), CB_publishCycle); - - // setup dynamic reconfigure - dynamic_recfg = boost::make_shared< dynamic_reconfigure::Server >(n); - dynamic_reconfigure::Server::CallbackType cb = boost::bind(CB_reconfigure, _1, _2); - dynamic_recfg->setCallback(cb); - - // setup callback for custom obstacles - custom_obst_sub = n.subscribe("obstacles", 1, CB_customObstacle); - - // setup callback for clicked points (in rviz) that are considered as via-points - clicked_points_sub = n.subscribe("/clicked_point", 5, CB_clicked_points); - - // setup callback for via-points (callback overwrites previously set via-points) - via_points_sub = n.subscribe("via_points", 1, CB_via_points); - - // interactive marker server for simulated dynamic obstacles - interactive_markers::InteractiveMarkerServer marker_server("marker_obstacles"); - - obst_vector.push_back( boost::make_shared(-3,1) ); - obst_vector.push_back( boost::make_shared(6,2) ); - obst_vector.push_back( boost::make_shared(0,0.1) ); -// obst_vector.push_back( boost::make_shared(1,1.5,1,-1.5) ); //90 deg -// obst_vector.push_back( boost::make_shared(1,0,-1,0) ); //180 deg -// obst_vector.push_back( boost::make_shared(-1.5,-0.5) ); - - // Dynamic obstacles - Eigen::Vector2d vel (0.1, -0.3); - obst_vector.at(0)->setCentroidVelocity(vel); - vel = Eigen::Vector2d(-0.3, -0.2); - obst_vector.at(1)->setCentroidVelocity(vel); - - /* - PolygonObstacle* polyobst = new PolygonObstacle; - polyobst->pushBackVertex(1, -1); - polyobst->pushBackVertex(0, 1); - polyobst->pushBackVertex(1, 1); - polyobst->pushBackVertex(2, 1); - - polyobst->finalizePolygon(); - obst_vector.emplace_back(polyobst); - */ - - for (unsigned int i=0; i(topic, 1, boost::bind(&CB_setObstacleVelocity, _1, i))); - - //CreateInteractiveMarker(obst_vector.at(i)[0],obst_vector.at(i)[1],i,&marker_server, &CB_obstacle_marker); - // Add interactive markers for all point obstacles - boost::shared_ptr pobst = boost::dynamic_pointer_cast(obst_vector.at(i)); - if (pobst) - { - CreateInteractiveMarker(pobst->x(),pobst->y(),i, config.map_frame, &marker_server, &CB_obstacle_marker); - } - } - marker_server.applyChanges(); - - // Setup visualization - visual = TebVisualizationPtr(new TebVisualization(n, config)); - - // Setup robot shape model - RobotFootprintModelPtr robot_model = TebLocalPlannerROS::getRobotFootprintFromParamServer(n); - - // Setup planner (homotopy class planning or just the local teb planner) - if (config.hcp.enable_homotopy_class_planning) - planner = PlannerInterfacePtr(new HomotopyClassPlanner(config, &obst_vector, robot_model, visual, &via_points)); - else - planner = PlannerInterfacePtr(new TebOptimalPlanner(config, &obst_vector, robot_model, visual, &via_points)); - - - no_fixed_obstacles = obst_vector.size(); - ros::spin(); - - return 0; -} - -// Planning loop -void CB_mainCycle(const ros::TimerEvent& e) -{ - planner->plan(PoseSE2(-4,0,0), PoseSE2(4,0,0)); // hardcoded start and goal for testing purposes -} - -// Visualization loop -void CB_publishCycle(const ros::TimerEvent& e) -{ - planner->visualize(); - visual->publishObstacles(obst_vector); - visual->publishViaPoints(via_points); -} - -void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level) -{ - config.reconfigure(reconfig); -} - -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) -{ - // create an interactive marker for our server - visualization_msgs::InteractiveMarker i_marker; - i_marker.header.frame_id = frame; - i_marker.header.stamp = ros::Time::now(); - std::ostringstream oss; - //oss << "obstacle" << id; - oss << id; - i_marker.name = oss.str(); - i_marker.description = "Obstacle"; - i_marker.pose.position.x = init_x; - i_marker.pose.position.y = init_y; - i_marker.pose.orientation.w = 1.0f; // make quaternion normalized - - // create a grey box marker - visualization_msgs::Marker box_marker; - box_marker.type = visualization_msgs::Marker::CUBE; - box_marker.id = id; - box_marker.scale.x = 0.2; - box_marker.scale.y = 0.2; - box_marker.scale.z = 0.2; - box_marker.color.r = 0.5; - box_marker.color.g = 0.5; - box_marker.color.b = 0.5; - box_marker.color.a = 1.0; - box_marker.pose.orientation.w = 1.0f; // make quaternion normalized - - // create a non-interactive control which contains the box - visualization_msgs::InteractiveMarkerControl box_control; - box_control.always_visible = true; - box_control.markers.push_back( box_marker ); - - // add the control to the interactive marker - i_marker.controls.push_back( box_control ); - - // create a control which will move the box, rviz will insert 2 arrows - visualization_msgs::InteractiveMarkerControl move_control; - move_control.name = "move_x"; - move_control.orientation.w = 0.707107f; - move_control.orientation.x = 0; - move_control.orientation.y = 0.707107f; - move_control.orientation.z = 0; - move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; - - - // add the control to the interactive marker - i_marker.controls.push_back(move_control); - - // add the interactive marker to our collection - marker_server->insert(i_marker); - marker_server->setCallback(i_marker.name,feedback_cb); -} - -void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) -{ - std::stringstream ss(feedback->marker_name); - unsigned int index; - ss >> index; - - if (index>=no_fixed_obstacles) - return; - PointObstacle* pobst = static_cast(obst_vector.at(index).get()); - pobst->position() = Eigen::Vector2d(feedback->pose.position.x,feedback->pose.position.y); -} - -void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg) -{ - // resize such that the vector contains only the fixed obstacles specified inside the main function - obst_vector.resize(no_fixed_obstacles); - - // Add custom obstacles obtained via message (assume that all obstacles coordiantes are specified in the default planning frame) - for (size_t i = 0; i < obst_msg->obstacles.size(); ++i) - { - if (obst_msg->obstacles.at(i).polygon.points.size() == 1 ) - { - obst_vector.push_back(ObstaclePtr(new PointObstacle( obst_msg->obstacles.at(i).polygon.points.front().x, - obst_msg->obstacles.at(i).polygon.points.front().y ))); - } - else - { - PolygonObstacle* polyobst = new PolygonObstacle; - for (size_t j=0; jobstacles.at(i).polygon.points.size(); ++j) - { - polyobst->pushBackVertex( obst_msg->obstacles.at(i).polygon.points[j].x, - obst_msg->obstacles.at(i).polygon.points[j].y ); - } - polyobst->finalizePolygon(); - obst_vector.push_back(ObstaclePtr(polyobst)); - } - - if(!obst_vector.empty()) - obst_vector.back()->setCentroidVelocity(obst_msg->obstacles.at(i).velocities, obst_msg->obstacles.at(i).orientation); - } -} - - -void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg) -{ - // we assume for simplicity that the fixed frame is already the map/planning frame - // consider clicked points as via-points - via_points.push_back( Eigen::Vector2d(point_msg->point.x, point_msg->point.y) ); - ROS_INFO_STREAM("Via-point (" << point_msg->point.x << "," << point_msg->point.y << ") added."); - if (config.optim.weight_viapoint<=0 && config.optim.weight_viapoint_orientation<=0) - ROS_WARN("Note, via-points are deactivated, since 'weight_via_point' <= 0"); -} - -void CB_via_points(const nav_msgs::Path::ConstPtr& via_points_msg) -{ - ROS_INFO_ONCE("Via-points received. This message is printed once."); - via_points.clear(); - for (const geometry_msgs::PoseStamped& pose : via_points_msg->poses) - { - via_points.emplace_back(pose.pose.position.x, pose.pose.position.y); - } -} - -void CB_setObstacleVelocity(const geometry_msgs::TwistConstPtr& twist_msg, const unsigned int id) -{ - if (id >= obst_vector.size()) - { - ROS_WARN("Cannot set velocity: unknown obstacle id."); - return; - } - - Eigen::Vector2d vel (twist_msg->linear.x, twist_msg->linear.y); - obst_vector.at(id)->setCentroidVelocity(vel); -} From b7d27ca32776c71bb836a8b173b7a93a02587111 Mon Sep 17 00:00:00 2001 From: Christian Mai Date: Fri, 30 Nov 2018 15:50:46 +0100 Subject: [PATCH 3/5] Readd test_optim_node to CMakeLists modified: CMakeLists.txt --- CMakeLists.txt | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4634a723..d610e0f0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -203,13 +203,13 @@ target_link_libraries(teb_local_planner ## Build additional executables -##add_executable(test_optim_node src/test_optim_node.cpp) +add_executable(test_optim_node src/test_optim_node.cpp) -##target_link_libraries(test_optim_node -## teb_local_planner -## ${EXTERNAL_LIBS} -## ${catkin_LIBRARIES} -##) +target_link_libraries(test_optim_node + teb_local_planner + ${EXTERNAL_LIBS} + ${catkin_LIBRARIES} +) ############# @@ -230,9 +230,9 @@ target_link_libraries(teb_local_planner install(TARGETS teb_local_planner LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) -##install(TARGETS test_optim_node -## RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -##) +install(TARGETS test_optim_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) ## Mark cpp header files for installation install(DIRECTORY include/${PROJECT_NAME}/ From 4dcca964fd649ab2313f3724acaf4451391531e5 Mon Sep 17 00:00:00 2001 From: Christian Mai Date: Sat, 1 Dec 2018 14:02:39 +0100 Subject: [PATCH 4/5] Readd test_optim_node new file: src/test_optim_node.cpp --- src/test_optim_node.cpp | 311 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 311 insertions(+) create mode 100644 src/test_optim_node.cpp diff --git a/src/test_optim_node.cpp b/src/test_optim_node.cpp new file mode 100644 index 00000000..5386d110 --- /dev/null +++ b/src/test_optim_node.cpp @@ -0,0 +1,311 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017. + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#include + +#include +#include + +#include +#include + + +using namespace teb_local_planner; // it is ok here to import everything for testing purposes + +// ============= Global Variables ================ +// Ok global variables are bad, but here we only have a simple testing node. +PlannerInterfacePtr planner; +TebVisualizationPtr visual; +std::vector obst_vector; +ViaPointContainer via_points; +TebConfig config; +boost::shared_ptr< dynamic_reconfigure::Server > dynamic_recfg; +ros::Subscriber custom_obst_sub; +ros::Subscriber via_points_sub; +ros::Subscriber clicked_points_sub; +std::vector obst_vel_subs; +unsigned int no_fixed_obstacles; + +// =========== Function declarations ============= +void CB_mainCycle(const ros::TimerEvent& e); +void CB_publishCycle(const ros::TimerEvent& e); +void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level); +void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg); +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); +void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); +void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg); +void CB_via_points(const nav_msgs::Path::ConstPtr& via_points_msg); +void CB_setObstacleVelocity(const geometry_msgs::TwistConstPtr& twist_msg, const unsigned int id); + + +// =============== Main function ================= +int main( int argc, char** argv ) +{ + ros::init(argc, argv, "test_optim_node"); + ros::NodeHandle n("~"); + + + // load ros parameters from node handle + config.loadRosParamFromNodeHandle(n); + + ros::Timer cycle_timer = n.createTimer(ros::Duration(0.025), CB_mainCycle); + ros::Timer publish_timer = n.createTimer(ros::Duration(0.1), CB_publishCycle); + + // setup dynamic reconfigure + dynamic_recfg = boost::make_shared< dynamic_reconfigure::Server >(n); + dynamic_reconfigure::Server::CallbackType cb = boost::bind(CB_reconfigure, _1, _2); + dynamic_recfg->setCallback(cb); + + // setup callback for custom obstacles + custom_obst_sub = n.subscribe("obstacles", 1, CB_customObstacle); + + // setup callback for clicked points (in rviz) that are considered as via-points + clicked_points_sub = n.subscribe("/clicked_point", 5, CB_clicked_points); + + // setup callback for via-points (callback overwrites previously set via-points) + via_points_sub = n.subscribe("via_points", 1, CB_via_points); + + // interactive marker server for simulated dynamic obstacles + interactive_markers::InteractiveMarkerServer marker_server("marker_obstacles"); + + obst_vector.push_back( boost::make_shared(-3,1) ); + obst_vector.push_back( boost::make_shared(6,2) ); + obst_vector.push_back( boost::make_shared(0,0.1) ); +// obst_vector.push_back( boost::make_shared(1,1.5,1,-1.5) ); //90 deg +// obst_vector.push_back( boost::make_shared(1,0,-1,0) ); //180 deg +// obst_vector.push_back( boost::make_shared(-1.5,-0.5) ); + + // Dynamic obstacles + Eigen::Vector2d vel (0.1, -0.3); + obst_vector.at(0)->setCentroidVelocity(vel); + vel = Eigen::Vector2d(-0.3, -0.2); + obst_vector.at(1)->setCentroidVelocity(vel); + + /* + PolygonObstacle* polyobst = new PolygonObstacle; + polyobst->pushBackVertex(1, -1); + polyobst->pushBackVertex(0, 1); + polyobst->pushBackVertex(1, 1); + polyobst->pushBackVertex(2, 1); + + polyobst->finalizePolygon(); + obst_vector.emplace_back(polyobst); + */ + + for (unsigned int i=0; i(topic, 1, boost::bind(&CB_setObstacleVelocity, _1, i))); + + //CreateInteractiveMarker(obst_vector.at(i)[0],obst_vector.at(i)[1],i,&marker_server, &CB_obstacle_marker); + // Add interactive markers for all point obstacles + boost::shared_ptr pobst = boost::dynamic_pointer_cast(obst_vector.at(i)); + if (pobst) + { + CreateInteractiveMarker(pobst->x(),pobst->y(),i, config.map_frame, &marker_server, &CB_obstacle_marker); + } + } + marker_server.applyChanges(); + + // Setup visualization + visual = TebVisualizationPtr(new TebVisualization(n, config)); + + // Setup robot shape model + RobotFootprintModelPtr robot_model = TebLocalPlannerROS::getRobotFootprintFromParamServer(n); + + // Setup planner (homotopy class planning or just the local teb planner) + if (config.hcp.enable_homotopy_class_planning) + planner = PlannerInterfacePtr(new HomotopyClassPlanner(config, &obst_vector, robot_model, visual, &via_points)); + else + planner = PlannerInterfacePtr(new TebOptimalPlanner(config, &obst_vector, robot_model, visual, &via_points)); + + + no_fixed_obstacles = obst_vector.size(); + ros::spin(); + + return 0; +} + +// Planning loop +void CB_mainCycle(const ros::TimerEvent& e) +{ + planner->plan(PoseSE2(-4,0,0), PoseSE2(4,0,0)); // hardcoded start and goal for testing purposes +} + +// Visualization loop +void CB_publishCycle(const ros::TimerEvent& e) +{ + planner->visualize(); + visual->publishObstacles(obst_vector); + visual->publishViaPoints(via_points); +} + +void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level) +{ + config.reconfigure(reconfig); +} + +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) +{ + // create an interactive marker for our server + visualization_msgs::InteractiveMarker i_marker; + i_marker.header.frame_id = frame; + i_marker.header.stamp = ros::Time::now(); + std::ostringstream oss; + //oss << "obstacle" << id; + oss << id; + i_marker.name = oss.str(); + i_marker.description = "Obstacle"; + i_marker.pose.position.x = init_x; + i_marker.pose.position.y = init_y; + i_marker.pose.orientation.w = 1.0f; // make quaternion normalized + + // create a grey box marker + visualization_msgs::Marker box_marker; + box_marker.type = visualization_msgs::Marker::CUBE; + box_marker.id = id; + box_marker.scale.x = 0.2; + box_marker.scale.y = 0.2; + box_marker.scale.z = 0.2; + box_marker.color.r = 0.5; + box_marker.color.g = 0.5; + box_marker.color.b = 0.5; + box_marker.color.a = 1.0; + box_marker.pose.orientation.w = 1.0f; // make quaternion normalized + + // create a non-interactive control which contains the box + visualization_msgs::InteractiveMarkerControl box_control; + box_control.always_visible = true; + box_control.markers.push_back( box_marker ); + + // add the control to the interactive marker + i_marker.controls.push_back( box_control ); + + // create a control which will move the box, rviz will insert 2 arrows + visualization_msgs::InteractiveMarkerControl move_control; + move_control.name = "move_x"; + move_control.orientation.w = 0.707107f; + move_control.orientation.x = 0; + move_control.orientation.y = 0.707107f; + move_control.orientation.z = 0; + move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; + + + // add the control to the interactive marker + i_marker.controls.push_back(move_control); + + // add the interactive marker to our collection + marker_server->insert(i_marker); + marker_server->setCallback(i_marker.name,feedback_cb); +} + +void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) +{ + std::stringstream ss(feedback->marker_name); + unsigned int index; + ss >> index; + + if (index>=no_fixed_obstacles) + return; + PointObstacle* pobst = static_cast(obst_vector.at(index).get()); + pobst->position() = Eigen::Vector2d(feedback->pose.position.x,feedback->pose.position.y); +} + +void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg) +{ + // resize such that the vector contains only the fixed obstacles specified inside the main function + obst_vector.resize(no_fixed_obstacles); + + // Add custom obstacles obtained via message (assume that all obstacles coordiantes are specified in the default planning frame) + for (size_t i = 0; i < obst_msg->obstacles.size(); ++i) + { + if (obst_msg->obstacles.at(i).polygon.points.size() == 1 ) + { + obst_vector.push_back(ObstaclePtr(new PointObstacle( obst_msg->obstacles.at(i).polygon.points.front().x, + obst_msg->obstacles.at(i).polygon.points.front().y ))); + } + else + { + PolygonObstacle* polyobst = new PolygonObstacle; + for (size_t j=0; jobstacles.at(i).polygon.points.size(); ++j) + { + polyobst->pushBackVertex( obst_msg->obstacles.at(i).polygon.points[j].x, + obst_msg->obstacles.at(i).polygon.points[j].y ); + } + polyobst->finalizePolygon(); + obst_vector.push_back(ObstaclePtr(polyobst)); + } + + if(!obst_vector.empty()) + obst_vector.back()->setCentroidVelocity(obst_msg->obstacles.at(i).velocities, obst_msg->obstacles.at(i).orientation); + } +} + + +void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg) +{ + // we assume for simplicity that the fixed frame is already the map/planning frame + // consider clicked points as via-points + via_points.push_back( Eigen::Vector2d(point_msg->point.x, point_msg->point.y) ); + ROS_INFO_STREAM("Via-point (" << point_msg->point.x << "," << point_msg->point.y << ") added."); + if (config.optim.weight_viapoint<=0) + ROS_WARN("Note, via-points are deactivated, since 'weight_via_point' <= 0"); +} + +void CB_via_points(const nav_msgs::Path::ConstPtr& via_points_msg) +{ + ROS_INFO_ONCE("Via-points received. This message is printed once."); + via_points.clear(); + for (const geometry_msgs::PoseStamped& pose : via_points_msg->poses) + { + via_points.emplace_back(pose.pose.position.x, pose.pose.position.y); + } +} + +void CB_setObstacleVelocity(const geometry_msgs::TwistConstPtr& twist_msg, const unsigned int id) +{ + if (id >= obst_vector.size()) + { + ROS_WARN("Cannot set velocity: unknown obstacle id."); + return; + } + + Eigen::Vector2d vel (twist_msg->linear.x, twist_msg->linear.y); + obst_vector.at(id)->setCentroidVelocity(vel); +} From 6e83d0705e2191f8274e4cb227150930a0b3d3da Mon Sep 17 00:00:00 2001 From: Christian Mai Date: Tue, 4 Dec 2018 09:36:07 +0100 Subject: [PATCH 5/5] Fix test_optim_node for new PoseSE2 modified: src/test_optim_node.cpp --- src/test_optim_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/test_optim_node.cpp b/src/test_optim_node.cpp index 5386d110..a279ccb5 100644 --- a/src/test_optim_node.cpp +++ b/src/test_optim_node.cpp @@ -282,7 +282,7 @@ void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg) { // we assume for simplicity that the fixed frame is already the map/planning frame // consider clicked points as via-points - via_points.push_back( Eigen::Vector2d(point_msg->point.x, point_msg->point.y) ); + via_points.emplace_back(point_msg->point.x, point_msg->point.y, 0); ROS_INFO_STREAM("Via-point (" << point_msg->point.x << "," << point_msg->point.y << ") added."); if (config.optim.weight_viapoint<=0) ROS_WARN("Note, via-points are deactivated, since 'weight_via_point' <= 0"); @@ -294,7 +294,7 @@ void CB_via_points(const nav_msgs::Path::ConstPtr& via_points_msg) via_points.clear(); for (const geometry_msgs::PoseStamped& pose : via_points_msg->poses) { - via_points.emplace_back(pose.pose.position.x, pose.pose.position.y); + via_points.emplace_back(pose.pose.position.x, pose.pose.position.y, 0); } }