Comments (3)
You could have your BT that takes in the set of goals
publish out the goals
vector that the MPC can listen to so that when it receives the path, its knows which points belong to the waypoints and which are path-planned points. You could do that right before calling the MPC controller so you know its received before that is attempted to start tracking it. To guarantee that, you could make it a service & the Nav2 BT tools provides a base class to make service calls easy.
Basically all you need to do is make a ~20 line BT node, add it to your BT XML right before sending the path to the controller, and then make a service/subscription in the MPC plugin that stores that information. Then you should be good to go 😄
from navigation2.
Thank you for this solution. I tried to implement such a BT node and ran into the problem, that cmake doesn't find the package behaviortree_cpp.
If you need more details:
I used remove_passed_goals_action.hpp and remove_passed_goals_action.cpp as a starting point to copy and modify it to what I need. For the CMakeList, I used this for guidance. My whole CMakeList.txt now looks like this:
cmake_minimum_required(VERSION 3.8)
project(nav2_controller_mpc_w_dyn_obstacles)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(nav2_common REQUIRED)
find_package(nav2_core REQUIRED)
find_package(nav2_costmap_2d REQUIRED)
find_package(nav2_util REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(pluginlib REQUIRED)
find_package(tf2 REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
find_package(var_predictor_interfaces REQUIRED)
find_package(nav2_mpc_interface REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(behaviortree_cpp REQUIRED)
nav2_package()
include_directories(
include
)
set(dependencies
rclcpp
geometry_msgs
nav2_costmap_2d
pluginlib
nav_msgs
std_msgs
nav2_util
nav2_core
tf2
var_predictor_interfaces
nav2_mpc_interface
rclcpp_action
rclcpp_lifecycle
behaviortree_cpp
)
add_library(nav2_controller_mpc_w_dyn_obstacles SHARED
src/nav2_controller_mpc_w_dyn_obstacles.cpp)
# prevent pluginlib from using boost
target_compile_definitions(nav2_controller_mpc_w_dyn_obstacles PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
ament_target_dependencies(nav2_controller_mpc_w_dyn_obstacles
${dependencies}
)
add_library(bt_waypoint_pub_bt_node SHARED src/bt_waypoint_pub.cpp)
ament_target_dependencies(bt_waypoint_pub_bt_node ${dependencies})
target_compile_definitions(bt_waypoint_pub_bt_node PRIVATE BT_PLUGIN_EXPORT)
install(TARGETS nav2_controller_mpc_w_dyn_obstacles bt_waypoint_pub_bt_node
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(DIRECTORY include/
DESTINATION include/
)
ament_python_install_package(${PROJECT_NAME})
install(PROGRAMS
nav2_controller_mpc_w_dyn_obstacles/vel_smoother_integrator.py
nav2_controller_mpc_w_dyn_obstacles/mpc_calculation_node.py
nav2_controller_mpc_w_dyn_obstacles/define_mpc.py
DESTINATION lib/${PROJECT_NAME}
)
install(
DIRECTORY launch behavior_trees
DESTINATION share/${PROJECT_NAME}
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
set(ament_cmake_copyright_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_export_include_directories(include)
ament_export_libraries(nav2_controller_mpc_w_dyn_obstacles bt_waypoint_pub_bt_node)
ament_export_dependencies(${dependencies})
pluginlib_export_plugin_description_file(nav2_core nav2_controller_mpc_w_dyn_obstacles.xml)
ament_package()
from navigation2.
I know it sounds like a dumb question, but is it installed? Also its behaviortree_cpp_v3
for Humble https://github.com/ros-navigation/navigation2/blob/humble/nav2_behavior_tree/CMakeLists.txt#L37
Make sure to reference the right branch for your distribution :-)
Closing since I think this is answered, but feel free to comment back if you need anything else! I'd love to see a link to your MPC controller / BT use once its ready (if its open-source)! We definitely need more controller options in the ROS 2 ecosystem :-)
from navigation2.
Related Issues (20)
- Collision Monitor in Nav2 Not Displaying Polygons Correctly or Following Robot HOT 4
- In the running of goToPose(), cancelTask() does not work HOT 4
- Docking is not available for ros humble. HOT 1
- Both Slack invite links may have expired HOT 1
- Revert Double Spin in #4558 once rclcpp #2589 fixed
- Wait node no action server HOT 1
- Custom constraints and custom cost function in mppi HOT 1
- Nav2 BasicNavigator raises ValueError exception HOT 2
- Exception: According to the loaded plugin descriptions the class nav2_smac_planner::SmacPlannerHybrid with base class type nav2_core::GlobalPlanner does not exist. HOT 2
- setCostmap function in Footprint Collision Checker HOT 3
- About Nav2 CycloneDDS and Micro XRCE-DDS HOT 2
- Model State Initialization HOT 1
- AMCL is unable to correctly publish the map->odom coordinate transformation. HOT 4
- Create BT action node RemoveInCollisionGoals HOT 5
- [smac_planner] On Hybrid A*, any reason to use floor to quantize angles? HOT 5
- Issue with Bidirectional Acceleration and Deceleration Settings in MPPI Controller HOT 3
- Collision Monitor parameters on ROS2 Humble HOT 4
- loopback_sim: provide realistic scan data
- The default_server_timeout parameter in bt_navigator is always overridden by the server_timeout port in each BT action node. HOT 2
- The callback function of subscriber is not called while navigation is in progress. HOT 1
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from navigation2.