Accessing waypoints inside a controller plugin

[M-Schroeder]

ros-distro: humble

Feature request

I am currently writing an MPC-based controller plugin and sending waypoints to the action server navigate_through_poses. It would be beneficial to know the waypoints inside the method computeVelocityCommands (of the controller plugin) to increase the weight on driving through them precisely. Currently, the MPC plans a path, that is close to the reference path, but rounded. Therefore the MPC’s plan doesn’t go through the corners of the path, where the waypoints are (see blue line in image). Then the current_goal_checker won’t recognize it as close enough.

I don’t know a way to access the waypoints inside computeVelocityCommands, because the inputs only include PoseStamped, Twist and GoalChecker. Would you consider adding the waypoints as an additional input?

[SteveMacenski]

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 😄

[M-Schroeder]

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()

[SteveMacenski]

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 :-)