├── CMakeLists.txt ├── README.md ├── cfg └── SegmentGlobalPlannerParams.cfg ├── include └── segment_global_planner │ ├── common.h │ ├── line_segment.h │ └── segment_global_planner.h ├── package.xml ├── param ├── move_base.xml ├── move_base_params.yaml └── segment_global_planner_params.yaml ├── rviz └── turtlebot3_segment_global_planner.rviz ├── scripts └── test.py ├── segment_global_planner_plugin.xml └── src ├── line_segment.cpp └── segment_global_planner.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(segment_global_planner) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++17) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | nav_core 12 | costmap_2d 13 | base_local_planner 14 | pluginlib 15 | roscpp 16 | geometry_msgs 17 | nav_msgs 18 | tf2 19 | dynamic_reconfigure 20 | ) 21 | 22 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 23 | generate_dynamic_reconfigure_options( 24 | cfg/SegmentGlobalPlannerParams.cfg 25 | # cfg/DynReconf2.cfg 26 | ) 27 | 28 | catkin_package( 29 | INCLUDE_DIRS include 30 | LIBRARIES segment_global_planner 31 | CATKIN_DEPENDS 32 | nav_core 33 | costmap_2d 34 | base_local_planner 35 | pluginlib 36 | roscpp 37 | geometry_msgs 38 | nav_msgs 39 | tf2 40 | dynamic_reconfigur 41 | # DEPENDS system_lib 42 | ) 43 | 44 | include_directories( 45 | include 46 | ${catkin_INCLUDE_DIRS} 47 | ) 48 | 49 | add_library(segment_global_planner 50 | src/segment_global_planner.cpp 51 | src/line_segment.cpp 52 | ) 53 | add_dependencies(segment_global_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) 54 | target_link_libraries(segment_global_planner 55 | ${catkin_LIBRARIES} 56 | ) 57 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # segment_global_planner Version 3 2 | A ROS global planner plugin for segments tracking. 3 | This plugin allows you to set several segments as one global plan with rviz tools "2D Nav Goal" or "Publish Point" easily. The segments are not just line segments in a narrow sense, but also segments of path that generated from other global planners(e.g. A*). 4 | ## Setup plugin 5 | Modify your launch file of move_base. Set the value of rosparam "/move_base/base_global_planner" to **segment_global_planner/SegmentGlobalPlanner**. Make sure the name space is correct. There are two brief files (yaml and xml) for examples in param folder. 6 | ## Parameters 7 | 1. /move_base/SegmentGlobalPlanner/base_global_planner 8 | The real global planner implementation to ganerate path segments.You can feel free to set any general global planner plugins like **global_planner/GlobalPlanner**. If you don't set this parameter then line segement generator will be implemented and the performance is just like the previous version of segment_global_planner. 9 | 2. /move_base/SegmentGlobalPlanner/threshold_point_on_line 10 | Threshold that robot is considered on the tracking line within.(dynamic_reconfigure) If this value is too low, the robot may often derail and make a plan directly to your final goal. 11 | 3. /move_base/LineSegment/point_interval 12 | The max interval of 2 points on a segment.(only for LineSegment) 13 | 4. /move_base/SegmentGlobalPlanner/child_goal_threshold 14 | Threshold that robot is considered reaching the goal within.(dynamic_reconfigure) You should make this larger than the tolerance of local planner and set a not too low frequency for global planner (5.0Hz is adequate for a small robot like Turtlebot3). If this value or the frequency of global planner is too low, the robot may accidentally finish the navigation action at a child goal. 15 | ## Service 16 | * /move_base/SegmentGlobalPlanner/clear_trajectory 17 | A simple service to clear current trajectory of segments. e.g. 18 | `rosservice call /move_base/SegmentGlobalPlanner/clear_trajectory "{}"` 19 | -------------------------------------------------------------------------------- /cfg/SegmentGlobalPlannerParams.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "segment_global_planner" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("threshold_point_on_line", double_t, 0, "Threshold that robot is considered on the tracking line within.", 0.3, 0.01, 2.0) 9 | gen.add("child_goal_threshold", double_t, 0, "Threshold that robot is considered reaching the goal within.", 0.2, 0.01, 0.8) 10 | 11 | exit(gen.generate(PACKAGE, "segment_global_planner", "SegmentGlobalPlanner")) 12 | -------------------------------------------------------------------------------- /include/segment_global_planner/common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace segment_global_planner 7 | { 8 | /** 9 | * @brief distance square 10 | * @param p1 pose1 11 | * @param p2 pose2 12 | * @return distance square value 13 | */ 14 | inline double sq_distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2) 15 | { 16 | double dx = p1.pose.position.x - p2.pose.position.x; 17 | double dy = p1.pose.position.y - p2.pose.position.y; 18 | return dx * dx + dy * dy; 19 | } 20 | 21 | /** 22 | * @brief Set yaw for a pose 23 | * @param pose Pose to be set yaw 24 | * @param angle yaw 25 | */ 26 | inline void setYaw(geometry_msgs::PoseStamped* pose, double angle) 27 | { 28 | tf2::Quaternion tf2q; 29 | tf2q.setRPY(0.0,0.0,angle); 30 | pose->pose.orientation = tf2::toMsg(tf2q); 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /include/segment_global_planner/line_segment.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace segment_global_planner 9 | { 10 | class LineSegment : public nav_core::BaseGlobalPlanner 11 | { 12 | public: 13 | /** 14 | * @brief Override nav_core::BaseGlobalPlanner::makePlan() 15 | * @param start The start pose 16 | * @param goal The goal pose 17 | * @param plan The plan... filled by the planner 18 | * @return True if a valid plan was found, false otherwise 19 | */ 20 | bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, 21 | std::vector& plan) override; 22 | /** 23 | * @brief Override nav_core::BaseGlobalPlanner::initialize() 24 | * @param name The name of this planner 25 | * @param costmap_ros A pointer to the ROS wrapper of the costmap to use for planning 26 | */ 27 | void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) override; 28 | private: 29 | double point_interval_ { 0.05 }; //distance between two points on a segment 30 | costmap_2d::Costmap2DROS* costmap_ros_; //points to the costmap2dros 31 | std::shared_ptr costmap_model_; //costmap model for feasibility checking 32 | }; 33 | } 34 | -------------------------------------------------------------------------------- /include/segment_global_planner/segment_global_planner.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace segment_global_planner 20 | { 21 | /** 22 | * This class is an implementation of nav_core::BaseGlobalPlanner from standard ROS navigation package. It can help users to directly set line segments using RViz and let the robot follow the segments sequentially. 23 | */ 24 | class SegmentGlobalPlanner:public nav_core::BaseGlobalPlanner 25 | { 26 | public: 27 | /** 28 | * @brief Constructor 29 | */ 30 | SegmentGlobalPlanner(); 31 | /** 32 | * @brief Destructor 33 | */ 34 | ~SegmentGlobalPlanner(); 35 | /** 36 | * @brief Override nav_core::BaseGlobalPlanner::makePlan() 37 | * @param start The start pose 38 | * @param goal The goal pose 39 | * @param plan The plan... filled by the planner 40 | * @return True if a valid plan was found, false otherwise 41 | */ 42 | bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector& plan) override; 43 | /** 44 | * @brief Override nav_core::BaseGlobalPlanner::initialize() 45 | * @param name The name of this planner 46 | * @param costmap_ros A pointer to the ROS wrapper of the costmap to use for planning 47 | */ 48 | void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) override; 49 | 50 | private: 51 | /** 52 | * @brief Judge whether the robot reached the segment goal or not 53 | * @return True if the robot reached the goal, false otherwise 54 | */ 55 | bool isChildGoalReached(); 56 | /** 57 | * @brief Dynamic_reconfigure callback function 58 | */ 59 | void reconfigureCB(segment_global_planner::SegmentGlobalPlannerConfig& config, uint32_t level); 60 | /** 61 | * @brief Clear trajectory server callback function 62 | */ 63 | bool clearTrajectoryCB(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); 64 | /** 65 | * @brief clicked_point topic subscriber callback function 66 | */ 67 | void clickedPointCB(const geometry_msgs::PointStamped::ConstPtr& ptr); 68 | /** 69 | * @brief Add a new segment to the segment_list_ 70 | * @param new_plan New segment 71 | */ 72 | void addNewSegment(std::vector& new_plan); 73 | /** 74 | * @brief Switch to next segment 75 | */ 76 | void switchSegment(); 77 | /** 78 | * @brief Find distance from a point to a segment less than threshold_point_on_line_ 79 | * @param point The distance from 80 | * @param segment The distance to 81 | * @return True if find a less distance 82 | */ 83 | bool findDistLessThresh(const geometry_msgs::PoseStamped& point, const std::vector& segment); 84 | /** 85 | * @brief To replan current segment when makePlan() is called 86 | */ 87 | void replanCB(const std_msgs::Bool::ConstPtr& ptr); 88 | /** 89 | * @brief Publish path for display 90 | */ 91 | void displayPath(); 92 | 93 | double threshold_point_on_line_ { 0.1 }; //to determine whether a point is on the line 94 | double goal_threshold_ { 0.2 }; //goal threshold 95 | std::string global_frame_ { "map" }; //global frame id 96 | ros::Publisher plan_pub_; //display path publisher 97 | geometry_msgs::PoseStamped final_goal_, segment_goal_; //current global planner goal (final goal) and current child goal (goal of a segment that is heading to) 98 | std::unique_ptr> dynamic_config_server_; //server pointer for dynamic reconfigure 99 | ros::ServiceServer clear_trajectory_server_; //service for clearing trajectory 100 | ros::Publisher pose_from_clicked_point_pub_ { ros::NodeHandle().advertise("/move_base_simple/goal", 10) }; //publish the goal pose when the clicked_point has been received 101 | ros::Subscriber clicked_point_sub_ { ros::NodeHandle().subscribe("/clicked_point", 10, &SegmentGlobalPlanner::clickedPointCB, this) }; //rviz clicked_point subscriber 102 | tf2_ros::Buffer tf_buffer_ { ros::Duration(5.0) }; 103 | tf2_ros::TransformListener tf_ { tf_buffer_ }; 104 | std::list> segment_list_; //stores segments going to be tracked 105 | pluginlib::ClassLoader bgp_loader_ { "nav_core", "nav_core::BaseGlobalPlanner" }; 106 | boost::shared_ptr planner_implementation_; //real global planner for each segment 107 | bool final_goal_reached_ { true }; //whether final goal is reached 108 | bool got_first_goal { false }; //first goal mark 109 | std::vector last_segment_; //for reaching final goal, when segment_list_ is empty 110 | std::atomic_bool replan_ { false }; //replan flag 111 | ros::Subscriber replan_sub_; //to trigger replanning current segment 112 | bool clicked_a_new_goal_ { false }; //using topic "/clicked_point" setting a new goal 113 | }; 114 | } 115 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | segment_global_planner 4 | 3.0.0 5 | A ROS global planner plugin for segments tracking. 6 | 7 | 8 | 9 | 10 | wind 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | catkin 19 | nav_core 20 | pluginlib 21 | roscpp 22 | dynamic_reconfigur 23 | nav_core 24 | pluginlib 25 | roscpp 26 | dynamic_reconfigur 27 | nav_core 28 | pluginlib 29 | roscpp 30 | dynamic_reconfigur 31 | geometry_msgs 32 | nav_msgs 33 | costmap_2d 34 | base_local_planner 35 | tf2 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /param/move_base.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /param/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | #Make sure that this file is launched from move_base node tag. 2 | base_global_planner: segment_global_planner/SegmentGlobalPlanner 3 | -------------------------------------------------------------------------------- /param/segment_global_planner_params.yaml: -------------------------------------------------------------------------------- 1 | SegmentGlobalPlanner: 2 | child_goal_threshold: 0.2 3 | threshold_point_on_line: 0.3 4 | base_global_planner: global_planner/GlobalPlanner 5 | LineSegment: 6 | point_interval: 0.05 7 | -------------------------------------------------------------------------------- /rviz/turtlebot3_segment_global_planner.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | - /TF1/Tree1 9 | - /Global Map1/Costmap1 10 | - /Global Map1/Planner1 11 | - /Local Map1/Polygon1 12 | - /Local Map1/Costmap1 13 | - /Local Map1/Planner1 14 | Splitter Ratio: 0.5 15 | Tree Height: 478 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: 20 | - /2D Pose Estimate1 21 | - /2D Nav Goal1 22 | - /Publish Point1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.5886790156364441 25 | - Class: rviz/Views 26 | Expanded: 27 | - /Current View1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz/Time 31 | Experimental: false 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: LaserScan 35 | Preferences: 36 | PromptSaveOnExit: true 37 | Toolbars: 38 | toolButtonStyle: 2 39 | Visualization Manager: 40 | Class: "" 41 | Displays: 42 | - Alpha: 0.5 43 | Cell Size: 1 44 | Class: rviz/Grid 45 | Color: 160; 160; 164 46 | Enabled: true 47 | Line Style: 48 | Line Width: 0.029999999329447746 49 | Value: Lines 50 | Name: Grid 51 | Normal Cell Count: 0 52 | Offset: 53 | X: 0 54 | Y: 0 55 | Z: 0 56 | Plane: XY 57 | Plane Cell Count: 20 58 | Reference Frame: 59 | Value: true 60 | - Alpha: 1 61 | Class: rviz/RobotModel 62 | Collision Enabled: false 63 | Enabled: true 64 | Links: 65 | All Links Enabled: true 66 | Expand Joint Details: false 67 | Expand Link Details: false 68 | Expand Tree: false 69 | Link Tree Style: Links in Alphabetic Order 70 | base_footprint: 71 | Alpha: 1 72 | Show Axes: false 73 | Show Trail: false 74 | base_link: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | base_scan: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | camera_link: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | Value: true 89 | caster_back_link: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | imu_link: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | omni_caster_link: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | wheel_left_link: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | wheel_right_link: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | Name: RobotModel 114 | Robot Description: robot_description 115 | TF Prefix: "" 116 | Update Interval: 0 117 | Value: true 118 | Visual Enabled: true 119 | - Class: rviz/TF 120 | Enabled: false 121 | Frame Timeout: 15 122 | Frames: 123 | All Enabled: false 124 | Marker Scale: 1 125 | Name: TF 126 | Show Arrows: true 127 | Show Axes: true 128 | Show Names: false 129 | Tree: 130 | {} 131 | Update Interval: 0 132 | Value: false 133 | - Alpha: 1 134 | Autocompute Intensity Bounds: true 135 | Autocompute Value Bounds: 136 | Max Value: 10 137 | Min Value: -10 138 | Value: true 139 | Axis: Z 140 | Channel Name: intensity 141 | Class: rviz/LaserScan 142 | Color: 0; 255; 0 143 | Color Transformer: FlatColor 144 | Decay Time: 0 145 | Enabled: true 146 | Invert Rainbow: false 147 | Max Color: 255; 255; 255 148 | Max Intensity: 13069 149 | Min Color: 0; 0; 0 150 | Min Intensity: 28 151 | Name: LaserScan 152 | Position Transformer: XYZ 153 | Queue Size: 10 154 | Selectable: true 155 | Size (Pixels): 3 156 | Size (m): 0.030000001192092896 157 | Style: Flat Squares 158 | Topic: /scan 159 | Unreliable: false 160 | Use Fixed Frame: true 161 | Use rainbow: true 162 | Value: true 163 | - Class: rviz/Image 164 | Enabled: false 165 | Image Topic: /raspicam_node/image 166 | Max Value: 1 167 | Median window: 5 168 | Min Value: 0 169 | Name: Image 170 | Normalize Range: true 171 | Queue Size: 2 172 | Transport Hint: compressed 173 | Unreliable: false 174 | Value: false 175 | - Alpha: 0.699999988079071 176 | Class: rviz/Map 177 | Color Scheme: map 178 | Draw Behind: false 179 | Enabled: true 180 | Name: Map 181 | Topic: /map 182 | Unreliable: false 183 | Use Timestamp: false 184 | Value: true 185 | - Alpha: 1 186 | Buffer Length: 1 187 | Class: rviz/Path 188 | Color: 0; 0; 0 189 | Enabled: true 190 | Head Diameter: 0.30000001192092896 191 | Head Length: 0.20000000298023224 192 | Length: 0.30000001192092896 193 | Line Style: Lines 194 | Line Width: 0.029999999329447746 195 | Name: Planner Plan 196 | Offset: 197 | X: 0 198 | Y: 0 199 | Z: 0 200 | Pose Color: 255; 85; 255 201 | Pose Style: None 202 | Radius: 0.029999999329447746 203 | Shaft Diameter: 0.10000000149011612 204 | Shaft Length: 0.10000000149011612 205 | Topic: /move_base/SegmentGlobalPlanner/plan 206 | Unreliable: false 207 | Value: true 208 | - Class: rviz/Group 209 | Displays: 210 | - Alpha: 0.699999988079071 211 | Class: rviz/Map 212 | Color Scheme: costmap 213 | Draw Behind: true 214 | Enabled: true 215 | Name: Costmap 216 | Topic: /move_base/global_costmap/costmap 217 | Unreliable: false 218 | Use Timestamp: false 219 | Value: true 220 | - Alpha: 1 221 | Buffer Length: 1 222 | Class: rviz/Path 223 | Color: 255; 0; 0 224 | Enabled: true 225 | Head Diameter: 0.30000001192092896 226 | Head Length: 0.20000000298023224 227 | Length: 0.30000001192092896 228 | Line Style: Lines 229 | Line Width: 0.029999999329447746 230 | Name: Planner 231 | Offset: 232 | X: 0 233 | Y: 0 234 | Z: 0 235 | Pose Color: 255; 85; 255 236 | Pose Style: None 237 | Radius: 0.029999999329447746 238 | Shaft Diameter: 0.10000000149011612 239 | Shaft Length: 0.10000000149011612 240 | Topic: /move_base/DWAPlannerROS/global_plan 241 | Unreliable: false 242 | Value: true 243 | Enabled: true 244 | Name: Global Map 245 | - Class: rviz/Group 246 | Displays: 247 | - Alpha: 1 248 | Class: rviz/Polygon 249 | Color: 0; 0; 0 250 | Enabled: true 251 | Name: Polygon 252 | Topic: /move_base/local_costmap/footprint 253 | Unreliable: false 254 | Value: true 255 | - Alpha: 0.699999988079071 256 | Class: rviz/Map 257 | Color Scheme: costmap 258 | Draw Behind: false 259 | Enabled: true 260 | Name: Costmap 261 | Topic: /move_base/local_costmap/costmap 262 | Unreliable: false 263 | Use Timestamp: false 264 | Value: true 265 | - Alpha: 1 266 | Buffer Length: 1 267 | Class: rviz/Path 268 | Color: 255; 255; 0 269 | Enabled: true 270 | Head Diameter: 0.30000001192092896 271 | Head Length: 0.20000000298023224 272 | Length: 0.30000001192092896 273 | Line Style: Lines 274 | Line Width: 0.029999999329447746 275 | Name: Planner 276 | Offset: 277 | X: 0 278 | Y: 0 279 | Z: 0 280 | Pose Color: 255; 85; 255 281 | Pose Style: None 282 | Radius: 0.029999999329447746 283 | Shaft Diameter: 0.10000000149011612 284 | Shaft Length: 0.10000000149011612 285 | Topic: /move_base/DWAPlannerROS/local_plan 286 | Unreliable: false 287 | Value: true 288 | Enabled: true 289 | Name: Local Map 290 | - Alpha: 1 291 | Arrow Length: 0.05000000074505806 292 | Axes Length: 0.30000001192092896 293 | Axes Radius: 0.009999999776482582 294 | Class: rviz/PoseArray 295 | Color: 0; 192; 0 296 | Enabled: true 297 | Head Length: 0.07000000029802322 298 | Head Radius: 0.029999999329447746 299 | Name: Amcl Particles 300 | Shaft Length: 0.23000000417232513 301 | Shaft Radius: 0.009999999776482582 302 | Shape: Arrow (Flat) 303 | Topic: /particlecloud 304 | Unreliable: false 305 | Value: true 306 | - Alpha: 1 307 | Axes Length: 1 308 | Axes Radius: 0.10000000149011612 309 | Class: rviz/Pose 310 | Color: 255; 25; 0 311 | Enabled: true 312 | Head Length: 0.30000001192092896 313 | Head Radius: 0.10000000149011612 314 | Name: Goal 315 | Shaft Length: 0.5 316 | Shaft Radius: 0.05000000074505806 317 | Shape: Arrow 318 | Topic: /move_base_simple/goal 319 | Unreliable: false 320 | Value: true 321 | Enabled: true 322 | Global Options: 323 | Background Color: 48; 48; 48 324 | Default Light: true 325 | Fixed Frame: map 326 | Frame Rate: 30 327 | Name: root 328 | Tools: 329 | - Class: rviz/MoveCamera 330 | - Class: rviz/Interact 331 | Hide Inactive Objects: true 332 | - Class: rviz/Select 333 | - Class: rviz/SetInitialPose 334 | Theta std deviation: 0.2617993950843811 335 | Topic: /initialpose 336 | X std deviation: 0.5 337 | Y std deviation: 0.5 338 | - Class: rviz/SetGoal 339 | Topic: /move_base_simple/goal 340 | - Class: rviz/Measure 341 | - Class: rviz/PublishPoint 342 | Single click: false 343 | Topic: /clicked_point 344 | Value: true 345 | Views: 346 | Current: 347 | Angle: -1.5707964897155762 348 | Class: rviz/TopDownOrtho 349 | Enable Stereo Rendering: 350 | Stereo Eye Separation: 0.05999999865889549 351 | Stereo Focal Distance: 1 352 | Swap Stereo Eyes: false 353 | Value: false 354 | Invert Z Axis: false 355 | Name: Current View 356 | Near Clip Distance: 0.009999999776482582 357 | Scale: 179.525634765625 358 | Target Frame: 359 | Value: TopDownOrtho (rviz) 360 | X: 0 361 | Y: 0 362 | Saved: ~ 363 | Window Geometry: 364 | Displays: 365 | collapsed: false 366 | Height: 716 367 | Hide Left Dock: false 368 | Hide Right Dock: true 369 | Image: 370 | collapsed: false 371 | QMainWindow State: 000000ff00000000fd00000004000000000000016a0000026dfc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000410000026d000000dd00fffffffb0000000a0049006d0061006700650000000317000000cc0000001600fffffffb0000000a0049006d0061006700650000000330000000ce0000000000000000000000010000010f000003a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000003a0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003900000026d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 372 | Selection: 373 | collapsed: false 374 | Time: 375 | collapsed: false 376 | Tool Properties: 377 | collapsed: false 378 | Views: 379 | collapsed: true 380 | Width: 1280 381 | X: 0 382 | Y: 28 383 | -------------------------------------------------------------------------------- /scripts/test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #coding:utf-8 3 | from __future__ import print_function 4 | import rospy 5 | from geometry_msgs.msg import PointStamped,PoseStamped 6 | 7 | def test_publish_segments(): 8 | """set a path consisting of segments""" 9 | rospy.init_node('test_publish_segments', anonymous=True) 10 | pub = rospy.Publisher('/clicked_point', PointStamped, queue_size=10) 11 | point_list=[[0.721069872379,-0.348542779684], 12 | [0.343987584114,-0.353359788656], 13 | [0.325208365917,-0.147307574749], 14 | [0.728177905083,0.198717370629], 15 | [0.293201953173,0.238933160901], 16 | [-0.14968355,0.234476700425], 17 | [-0.252078384161,-0.333438158035], 18 | [0.131837338209,-0.365527868271], 19 | [0.279740422964,0.220124557614], 20 | [-0.245669052005,-0.333652347326], 21 | [-0.692820727825,-0.273771047592], 22 | [-0.67185819149,-0.0305300317705], 23 | [-0.194762974977,0.229563802481], 24 | [-0.61670601368,0.275763481855], 25 | [-1.03288269043,0.302511930466], 26 | [-1.07011294365,-0.235483184457], 27 | [-0.789817690849,-0.296207368374], 28 | [-0.738482773304,0.279833465815]]#2020 29 | point_msg=PointStamped() 30 | rospy.sleep(3.0)#wait for the publisher registering 31 | rate = rospy.Rate(5.0) 32 | point_msg.header.frame_id='map' 33 | for each_point in point_list: 34 | point_msg.point.x=each_point[0] 35 | point_msg.point.y=each_point[1] 36 | print('[',each_point[0],',',each_point[1],']') 37 | pub.publish(point_msg) 38 | rate.sleep() 39 | 40 | if __name__ == '__main__': 41 | try: 42 | test_publish_segments() 43 | except rospy.ROSInterruptException: 44 | pass 45 | -------------------------------------------------------------------------------- /segment_global_planner_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A global planner for line tracking. 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/line_segment.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | namespace segment_global_planner 7 | { 8 | bool LineSegment::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector& plan) 9 | { 10 | unsigned int start_x, start_y, end_x, end_y; 11 | std::list trajectory; 12 | trajectory.push_back(start); 13 | trajectory.push_back(goal); 14 | if (costmap_ros_->getCostmap()->worldToMap(trajectory.begin()->pose.position.x, trajectory.begin()->pose.position.y, 15 | start_x, start_y) 16 | && costmap_ros_->getCostmap()->worldToMap((--trajectory.end())->pose.position.x, (--trajectory.end())->pose.position.y, 17 | end_x, end_y)) //convert the start and goal to map coordinate and confirm that they are in the map 18 | { 19 | if (costmap_model_->lineCost(start_x, end_x, start_y, end_y) < 0.0) //lethal 20 | { 21 | ROS_ERROR("Some trajectory points are in lethal obstacle cell."); 22 | return false; 23 | } 24 | } 25 | else 26 | { 27 | ROS_ERROR("Some trajectory points are out of map."); 28 | return false; 29 | } 30 | for (auto itr = trajectory.begin(); itr != --trajectory.end(); itr++) //no need to judge the last pose 31 | { 32 | auto itr_next = itr; 33 | ++itr_next; 34 | double dist_2_point = sq_distance(*itr, *itr_next); //distance square 35 | if(dist_2_point > (point_interval_ + 0.001) * (point_interval_ + 0.001)) //distance is larger than threshold, need to insert one pose 36 | { 37 | dist_2_point = std::sqrt(dist_2_point); //real distance 38 | double proportion = point_interval_ / dist_2_point; 39 | geometry_msgs::PoseStamped point_to_insert; 40 | double& this_x = itr->pose.position.x; 41 | double& this_y = itr->pose.position.y; 42 | double& next_x = itr_next->pose.position.x; 43 | double& next_y = itr_next->pose.position.y; 44 | point_to_insert.pose.position.x = this_x + (next_x - this_x) * proportion; 45 | point_to_insert.pose.position.y = this_y + (next_y - this_y) * proportion; 46 | auto itr_last_point = --trajectory.end(); //last point 47 | double insert_point_yaw = std::atan2(itr_last_point->pose.position.y - this_y, itr_last_point->pose.position.x - this_x); 48 | setYaw(&point_to_insert, insert_point_yaw); 49 | point_to_insert.header.frame_id = goal.header.frame_id; 50 | trajectory.insert(itr_next, point_to_insert); 51 | } 52 | } 53 | plan.clear(); 54 | std::copy(trajectory.begin(), trajectory.end(), std::back_inserter(plan)); 55 | return true; 56 | } 57 | 58 | void LineSegment::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 59 | { 60 | ros::NodeHandle private_nh("~/" + name); 61 | private_nh.getParam("point_interval", point_interval_); 62 | costmap_ros_ = costmap_ros; 63 | costmap_model_ = std::make_shared(*costmap_ros->getCostmap());//make a costmap model 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /src/segment_global_planner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | PLUGINLIB_EXPORT_CLASS(segment_global_planner::SegmentGlobalPlanner, nav_core::BaseGlobalPlanner)//register plugin 11 | 12 | namespace 13 | { 14 | /** 15 | * @brief Calculate diatance from a point(p0) to a line segment(s1 s2) 16 | * @param p0 p0 pose 17 | * @param s1 s1 pose 18 | * @param s2 s2 pose 19 | * @return Distance 20 | */ 21 | double distPointToSegment(const geometry_msgs::PoseStamped& p0,const geometry_msgs::PoseStamped& s1, const geometry_msgs::PoseStamped& s2) 22 | { 23 | double p0s1[2] { s1.pose.position.x - p0.pose.position.x, s1.pose.position.y - p0.pose.position.y }; //vectors 24 | double p0s2[2] { s2.pose.position.x - p0.pose.position.x, s2.pose.position.y - p0.pose.position.y }; 25 | double s1s2[2] { s2.pose.position.x - s1.pose.position.x, s2.pose.position.y - s1.pose.position.y }; 26 | if (s1s2[0] * p0s1[0] + s1s2[1] * p0s1[1] > 0 || s1s2[0] * p0s2[0] + s1s2[1] * p0s2[1] < 0) //dot product, obtuse angle 27 | { 28 | return 1.0 / 0.0; //the point p0 is not between point s1 and point s2 29 | } 30 | double A = s1.pose.position.y - s2.pose.position.y; //parameters of linear equation : Ax+By+C=0 31 | double B = s2.pose.position.x - s1.pose.position.x; 32 | double C = s1.pose.position.x * s2.pose.position.y - s1.pose.position.y * s2.pose.position.x; 33 | return std::abs(A * p0.pose.position.x + B * p0.pose.position.y + C) / std::sqrt(A * A + B * B); //distance form p0 to line s1s2 : |Ax+By+C|/√(A²+B²) 34 | } 35 | } 36 | 37 | namespace segment_global_planner 38 | { 39 | SegmentGlobalPlanner::SegmentGlobalPlanner() : nav_core::BaseGlobalPlanner() 40 | { 41 | ROS_INFO("Constructing segment_global_planner plugin!"); 42 | } 43 | 44 | SegmentGlobalPlanner::~SegmentGlobalPlanner() 45 | { 46 | planner_implementation_.reset(); 47 | } 48 | 49 | bool SegmentGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, 50 | std::vector& plan) 51 | { 52 | if (isChildGoalReached()) //update segment_list_ 53 | { 54 | switchSegment(); 55 | } 56 | bool new_plan = false; 57 | if (!got_first_goal || final_goal_.pose.position.x != goal.pose.position.x 58 | || final_goal_.pose.position.y != goal.pose.position.y) //whether the goal is a new one 59 | { 60 | new_plan = true; 61 | if (final_goal_reached_ || !findDistLessThresh(start, segment_list_.front())) //need to start a new navigation 62 | { 63 | segment_list_.clear(); 64 | final_goal_ = segment_goal_ = start; 65 | } 66 | std::vector new_plan; 67 | if (planner_implementation_->makePlan(final_goal_, goal, new_plan) && !new_plan.empty()) //make a plan 68 | { 69 | addNewSegment(new_plan); //add a segment 70 | } 71 | else //fail to make a new plan 72 | { 73 | return false; 74 | } 75 | } 76 | 77 | if (!segment_list_.empty()) //fill the plan reference 78 | { 79 | bool replan_segment = false; 80 | if (!replan_) 81 | { 82 | plan = segment_list_.front(); //current segment as the plan 83 | } 84 | else //request replanning 85 | { 86 | if (segment_list_.size() == 1 && new_plan) //no need to replan 87 | { 88 | plan = segment_list_.front(); 89 | } 90 | else 91 | { 92 | std::vector replan_path; 93 | if (planner_implementation_->makePlan(start, segment_goal_, replan_path) && !replan_path.empty()) //replan 94 | { 95 | plan = segment_list_.front() = replan_path; //replace first segment 96 | replan_segment = true; 97 | ROS_INFO("Current segment has been replaned."); 98 | } 99 | else //fail to replan 100 | { 101 | return false; 102 | } 103 | } 104 | replan_ = false; 105 | } 106 | 107 | if (new_plan || replan_segment) //display path 108 | { 109 | displayPath(); 110 | } 111 | } 112 | else 113 | { 114 | plan = last_segment_; 115 | } 116 | got_first_goal = true; 117 | return true; 118 | } 119 | 120 | void SegmentGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 121 | { 122 | ROS_INFO("Initializing segment_global_planner plugin!"); 123 | ros::NodeHandle private_nh("~/" + name); 124 | std::string global_planner_name; 125 | if (private_nh.getParam("base_global_planner", global_planner_name)) //initialize planner implementation 126 | { 127 | try 128 | { 129 | planner_implementation_ = bgp_loader_.createInstance(global_planner_name); 130 | planner_implementation_->initialize(bgp_loader_.getName(global_planner_name), costmap_ros); //initialize real global planner 131 | } 132 | catch (const pluginlib::PluginlibException& ex) 133 | { 134 | ROS_WARN("The plugin name is not correct. Loading default LineSegment."); 135 | planner_implementation_ = boost::make_shared(); 136 | planner_implementation_->initialize("LineSegment", costmap_ros); //initialize implementation 137 | } 138 | } 139 | else 140 | { 141 | planner_implementation_ = boost::make_shared(); 142 | planner_implementation_->initialize("LineSegment", costmap_ros); //initialize implementation 143 | } 144 | 145 | plan_pub_=private_nh.advertise("plan", 1); 146 | private_nh.param("child_goal_threshold", goal_threshold_, goal_threshold_); 147 | private_nh.param("threshold_point_on_line", threshold_point_on_line_, threshold_point_on_line_); 148 | dynamic_config_server_.reset(new dynamic_reconfigure::Server(private_nh)); //setup dynamic reconfigure 149 | dynamic_config_server_->setCallback(boost::bind(&SegmentGlobalPlanner::reconfigureCB, this, _1, _2)); 150 | clear_trajectory_server_ = private_nh.advertiseService("clear_trajectory", 151 | &SegmentGlobalPlanner::clearTrajectoryCB, this); //setup clear trajectory service 152 | replan_sub_ = private_nh.subscribe("/replan_segment", 5, &SegmentGlobalPlanner::replanCB, this); 153 | global_frame_ = costmap_ros->getGlobalFrameID(); 154 | return; 155 | } 156 | 157 | bool SegmentGlobalPlanner::isChildGoalReached() 158 | { 159 | if (segment_list_.empty()) 160 | { 161 | final_goal_reached_ = true; 162 | return true; 163 | } 164 | geometry_msgs::TransformStamped tf_map_to_base = tf_buffer_.lookupTransform("map", "base_link", ros::Time(0)); 165 | geometry_msgs::PoseStamped current_pose; 166 | current_pose.pose.position.x = tf_map_to_base.transform.translation.x; 167 | current_pose.pose.position.y = tf_map_to_base.transform.translation.y; 168 | current_pose.pose.position.z = tf_map_to_base.transform.translation.z; 169 | current_pose.pose.orientation = tf_map_to_base.transform.rotation; 170 | if (sq_distance(current_pose, segment_list_.front().back()) < goal_threshold_ * goal_threshold_) //close to segment_goal 171 | { 172 | if (segment_list_.size() == 1) 173 | { 174 | final_goal_reached_ = true; 175 | } 176 | else 177 | { 178 | final_goal_reached_ = false; 179 | } 180 | return true; 181 | } 182 | final_goal_reached_ = false; 183 | return false; 184 | } 185 | 186 | void SegmentGlobalPlanner::reconfigureCB(segment_global_planner::SegmentGlobalPlannerConfig& config, uint32_t level) 187 | { 188 | ROS_INFO("dynamic_reconfigure updates."); 189 | threshold_point_on_line_ = config.threshold_point_on_line; 190 | goal_threshold_ = config.child_goal_threshold; 191 | return; 192 | } 193 | 194 | bool SegmentGlobalPlanner::clearTrajectoryCB(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) 195 | { 196 | final_goal_reached_ = true; 197 | segment_list_.clear(); 198 | nav_msgs::Path display_path; 199 | display_path.header.frame_id = global_frame_; 200 | plan_pub_.publish(display_path); //publish an empty path 201 | ROS_WARN("Trajectory has been cleared!"); 202 | return true; 203 | } 204 | 205 | void SegmentGlobalPlanner::clickedPointCB(const geometry_msgs::PointStamped::ConstPtr& ptr) 206 | { 207 | geometry_msgs::PoseStamped publish_goal; 208 | publish_goal.pose.position = ptr->point; 209 | publish_goal.header = ptr->header; 210 | if (!final_goal_reached_) 211 | { 212 | setYaw(&publish_goal, atan2(publish_goal.pose.position.y - final_goal_.pose.position.y, 213 | publish_goal.pose.position.x - final_goal_.pose.position.x)); //set the orientation of the goal vector from the final goal to this one 214 | } 215 | else 216 | { 217 | try 218 | { 219 | geometry_msgs::TransformStamped tf_map_to_base = tf_buffer_.lookupTransform("map", "base_link", ros::Time(0)); 220 | setYaw(&publish_goal, std::atan2(publish_goal.pose.position.y - tf_map_to_base.transform.translation.y, 221 | publish_goal.pose.position.x - tf_map_to_base.transform.translation.x)); //robot position to new goal 222 | } 223 | catch (const tf2::TransformException& ex) 224 | { 225 | publish_goal.pose.orientation.w = 1.0; //default orientation 226 | } 227 | } 228 | clicked_a_new_goal_ = true; 229 | pose_from_clicked_point_pub_.publish(publish_goal); 230 | } 231 | 232 | void SegmentGlobalPlanner::addNewSegment(std::vector& new_plan) 233 | { 234 | int point_number_to_estimate_orientation = std::min(5, new_plan.size()); 235 | if (clicked_a_new_goal_) //need to set new_plan's goal orientation 236 | { 237 | if (point_number_to_estimate_orientation >= 2) //estimate the orientation with the last several points of new_plan 238 | { 239 | double estimated_yaw = std::atan2(new_plan.back().pose.position.y - new_plan[new_plan.size() - point_number_to_estimate_orientation].pose.position.y, 240 | new_plan.back().pose.position.x - new_plan[new_plan.size() - point_number_to_estimate_orientation].pose.position.x); 241 | tf2::Quaternion tf2q(tf2::Vector3(0.0, 0.0, 1.0), estimated_yaw); 242 | new_plan.back().pose.orientation = tf2::toMsg(tf2q); 243 | } 244 | clicked_a_new_goal_ = false; 245 | } 246 | if (!segment_list_.empty()) //change the orientation of the goal of the last segment according to new_plan 247 | { 248 | if (point_number_to_estimate_orientation >= 2) //estimate the orientation with the first several points of new_plan 249 | { 250 | double estimated_yaw = std::atan2(new_plan[point_number_to_estimate_orientation - 1].pose.position.y - new_plan.front().pose.position.y, 251 | new_plan[point_number_to_estimate_orientation - 1].pose.position.x - new_plan.front().pose.position.x); 252 | tf2::Quaternion tf2q(tf2::Vector3(0.0, 0.0, 1.0), estimated_yaw); 253 | segment_list_.back().back().pose.orientation = tf2::toMsg(tf2q); 254 | } 255 | else //set the orientation with first pose orientation of new_plan 256 | { 257 | segment_list_.back().back().pose.orientation = new_plan.front().pose.orientation; 258 | } 259 | } 260 | else 261 | { 262 | segment_goal_ = new_plan.back(); 263 | } 264 | segment_list_.push_back(new_plan); //add a new plan to the segment list 265 | final_goal_ = new_plan.back(); //update final goal 266 | return; 267 | } 268 | 269 | void SegmentGlobalPlanner::switchSegment() 270 | { 271 | if (segment_list_.empty()) 272 | { 273 | return; 274 | } 275 | last_segment_ = segment_list_.back(); 276 | segment_list_.pop_front(); 277 | if (!segment_list_.empty()) 278 | { 279 | segment_goal_ = segment_list_.front().back(); 280 | } 281 | return; 282 | } 283 | 284 | bool SegmentGlobalPlanner::findDistLessThresh(const geometry_msgs::PoseStamped& point, const std::vector& segment) 285 | { 286 | for (auto itr = segment.begin(); itr != segment.end(); ++itr) //segments are one less than poses 287 | { 288 | auto itr_next = itr; 289 | ++itr_next; 290 | if (distPointToSegment(point, *itr, *itr_next) < threshold_point_on_line_) //find distance less than threshold 291 | { 292 | return true; 293 | } 294 | } 295 | return false; 296 | } 297 | 298 | void SegmentGlobalPlanner::replanCB(const std_msgs::Bool::ConstPtr& ptr) 299 | { 300 | if (ptr->data) 301 | { 302 | replan_ = true; 303 | ROS_INFO("Ready to replan segment."); 304 | } 305 | return; 306 | } 307 | 308 | void SegmentGlobalPlanner::displayPath() 309 | { 310 | nav_msgs::Path display_path; //path to be published 311 | display_path.header.frame_id = global_frame_; 312 | for (auto& segment : segment_list_) 313 | { 314 | std::copy(segment.begin(), segment.end(), std::back_inserter(display_path.poses)); 315 | } 316 | plan_pub_.publish(display_path); 317 | return; 318 | } 319 | } 320 | --------------------------------------------------------------------------------