Skip to content

Commit 4566239

Browse files
YuanJunbincaomuqing
YuanJunbin
and
caomuqing
authored
Junbin/planning dev (#290)
* migrate exploration planner from subt * config for running in ros2 gazebo env * gazebo configuration finished * scripts for manual takeoff without gui * adding a mode for receiving target path instead of extracting frontier * some config change for exploration run * manual launch of robot * blosc config for build error * add inspection_planner * repair docker compose * fix cmakelist after merge * fix cmakelist after merge * fixed docker compoer env * manually launch robot * another fix * tiny modification of waypoing routing * publishing inspection pose markers * add filter prediction semantics * update filter prediction semantics * RRT shortening, difference interpolation and extend step size * Update README.md * add doc line in mkdocs * image change * remove inspection planner, not ready * revert docker compose to align with main before pull request * minor change before pull request --------- Co-authored-by: caomuqing <caomuqing@163.com>
1 parent 76f1b6f commit 4566239

38 files changed

+8469
-0
lines changed

‎mkdocs.yml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -100,6 +100,7 @@ nav:
100100
- Planning:
101101
- docs/robot/autonomy/4_global/planning/index.md
102102
- robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/README.md
103+
- robot/ros_ws/src/autonomy/4_global/b_planners/exploration/README.md
103104
- Behavior:
104105
- docs/robot/autonomy/5_behavior/index.md
105106
- docs/robot/autonomy/5_behavior/behavior_tree.md

‎robot/ros_ws/manual_exploration.sh‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
#!/usr/bin/env bash
2+
set -euo pipefail
3+
4+
# Pair 2 (run in parallel)
5+
ros2 service call /robot_1/behavior/global_plan_toggle \
6+
std_srvs/srv/Trigger "{}" & p1=$!
7+
8+
ros2 service call /robot_1/trajectory_controller/set_trajectory_mode \
9+
airstack_msgs/srv/TrajectoryMode "{mode: 3}" & p2=$!
10+
11+
wait $p1 $p2
12+
echo "exploration command sent"

‎robot/ros_ws/manual_takeoff.sh‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
#!/usr/bin/env bash
2+
set -euo pipefail
3+
4+
# Pair 1 (run in parallel)
5+
ros2 service call /robot_1/trajectory_controller/set_trajectory_mode \
6+
airstack_msgs/srv/TrajectoryMode "{mode: 2}" & p1=$!
7+
8+
ros2 service call /robot_1/takeoff_landing_planner/set_takeoff_landing_command \
9+
airstack_msgs/srv/TakeoffLandingCommand "{command: 0}" & p2=$!
10+
11+
wait $p1 $p2
12+
echo "takeoff command sent"

‎robot/ros_ws/src/autonomy/4_global/b_planners/exploration/CMakeLists.txt‎

Lines changed: 146 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,146 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules" ${CMAKE_MODULE_PATH})
3+
project(exploration_planner)
4+
5+
# Default to C++14
6+
if(NOT CMAKE_CXX_STANDARD)
7+
set(CMAKE_CXX_STANDARD 17)
8+
endif()
9+
10+
# Find dependencies
11+
find_package(ament_cmake REQUIRED)
12+
find_package(rclcpp REQUIRED)
13+
find_package(rclcpp_action REQUIRED)
14+
find_package(std_msgs REQUIRED)
15+
find_package(std_srvs REQUIRED)
16+
find_package(geometry_msgs REQUIRED)
17+
find_package(nav_msgs REQUIRED)
18+
find_package(visualization_msgs REQUIRED)
19+
find_package(tf2 REQUIRED)
20+
find_package(tf2_ros REQUIRED)
21+
find_package(tf2_eigen REQUIRED)
22+
find_package(tf2_geometry_msgs REQUIRED)
23+
24+
# set(OpenVDB_INCLUDE_DIR "/usr/local/include/openvdb" CACHE STRING "Path to OpenVDB installation")
25+
find_package(OpenVDB REQUIRED)
26+
# find_package(vdb_mapping REQUIRED)
27+
find_package(Eigen3 REQUIRED)
28+
find_package(PCL REQUIRED)
29+
find_package(pcl_conversions REQUIRED)
30+
find_package(message_filters REQUIRED)
31+
32+
###################################
33+
## ament specific configuration ##
34+
###################################
35+
36+
ament_package()
37+
38+
###########
39+
## Build ##
40+
###########
41+
42+
# Specify additional locations of header files
43+
44+
# Declare a C++ executable
45+
add_executable(exploration_planner
46+
src/exploration_node_run.cpp
47+
src/exploration_node.cpp
48+
src/exploration_logic.cpp
49+
src/viewpoint_sampling.cpp
50+
src/collision_checker.cpp
51+
src/rrt_planner.cpp
52+
src/utils.cpp)
53+
54+
target_include_directories(exploration_planner
55+
PRIVATE
56+
${PCL_INCLUDE_DIRS}
57+
${EIGEN3_INCLUDE_DIR}
58+
${CMAKE_CURRENT_SOURCE_DIR}/include)
59+
60+
target_link_libraries(exploration_planner
61+
# vdb_mapping::vdb_mapping
62+
OpenVDB::openvdb
63+
${EIGEN3_LIBRARIES}
64+
${PCL_LIBRARIES})
65+
66+
add_executable(waypoint_routing_planner
67+
src/waypoint_routing_node.cpp
68+
src/exploration_node.cpp
69+
src/exploration_logic.cpp
70+
src/viewpoint_sampling.cpp
71+
src/collision_checker.cpp
72+
src/rrt_planner.cpp
73+
src/utils.cpp)
74+
75+
target_include_directories(waypoint_routing_planner
76+
PRIVATE
77+
${PCL_INCLUDE_DIRS}
78+
${EIGEN3_INCLUDE_DIR}
79+
${CMAKE_CURRENT_SOURCE_DIR}/include)
80+
81+
target_link_libraries(waypoint_routing_planner
82+
# vdb_mapping::vdb_mapping
83+
OpenVDB::openvdb
84+
${EIGEN3_LIBRARIES}
85+
${PCL_LIBRARIES})
86+
87+
# Link libraries
88+
ament_target_dependencies(exploration_planner
89+
rclcpp
90+
rclcpp_action
91+
std_msgs
92+
std_srvs
93+
geometry_msgs
94+
nav_msgs
95+
visualization_msgs
96+
tf2
97+
tf2_ros
98+
pcl_conversions
99+
tf2_eigen
100+
tf2_geometry_msgs
101+
)
102+
103+
ament_target_dependencies(waypoint_routing_planner
104+
rclcpp
105+
rclcpp_action
106+
std_msgs
107+
std_srvs
108+
geometry_msgs
109+
nav_msgs
110+
visualization_msgs
111+
tf2
112+
tf2_ros
113+
pcl_conversions
114+
tf2_eigen
115+
tf2_geometry_msgs
116+
)
117+
118+
# Install executable
119+
install(TARGETS
120+
exploration_planner
121+
waypoint_routing_planner
122+
DESTINATION lib/${PROJECT_NAME}
123+
)
124+
125+
install(DIRECTORY
126+
launch
127+
DESTINATION share/${PROJECT_NAME})
128+
129+
install(DIRECTORY
130+
config
131+
DESTINATION share/${PROJECT_NAME})
132+
133+
install(DIRECTORY
134+
src
135+
DESTINATION share/${PROJECT_NAME})
136+
137+
138+
#############
139+
## Testing ##
140+
#############
141+
142+
# Add gtest based cpp test target and link libraries
143+
# ament_add_gtest(${PROJECT_NAME}-test test/test_exploration_planner.cpp)
144+
# if(TARGET ${PROJECT_NAME}-test)
145+
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_node)
146+
# endif()

0 commit comments

Comments
 (0)