[ROS2] Navigation2 분석(4) - Custom Controller

Custom Controller

앞의 포스팅에서 말했듯이 nav2에서는 controller가 plugin으로 작동하고 있어서 interface만 맞춰서 plugin을 만들면 나만의 Controller를 적용 할 수 있다.

참고

https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html

Writing a New Controller Plugin — Navigation 2 1.0.0 documentation

1. Package 생성

먼저 controller를 만들 package를 생성해 준다.

ros2 pkg create --build-type ament_cmake custom_controller

2. header file 작성 (custom_controller.hpp)

#ifndef CUSTOM_CONTROLLER__CUSTOM_CONTROLLER_HPP_
#define CUSTOM_CONTROLLER__CUSTOM_CONTROLLER_HPP_

#include <nav2_core/controller.hpp>
#include <nav_msgs/msg/path.hpp>
#include <std_msgs/msg/string.hpp>

namespace custom_controller
{

class CustomController : public nav2_core::Controller
{
public:
  CustomController();

  void configure(
    const rclcpp_lifecycle::LifecycleNode::SharedPtr &parents,
    std::string name, const std::shared_ptr<tf2_ros::Buffer> &tf,
    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> &) override;

  void cleanup() override;

  void activate() override;

  void deactivate() override;

  void setPlan(const nav_msgs::msg::Path & path) override;

  geometry_msgs::msg::TwistStamped computeVelocityCommands(
    const geometry_msgs::msg::PoseStamped & pose,
    const geometry_msgs::msg::Twist & velocity) override;

private:
  rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
  std::shared_ptr<tf2_ros::Buffer> tf_;
};

} // namespace custom_controller

#endif

• configure
  • ros2 lifecycle node에서 configure시 넘어오는 paramter들이다.
  • node interface , plugin id name, tf, costmap ros가 넘어오게 된다.
  • node intereface
    • publish나 subscribe를 하고 싶을 때 사용한다.
    • paramter를 받아오는데도 사용한다.
  • plugin id name
    • plugin id 이름이 넘어온다. Debuging용으로 사용하자
  • tf
    • 로봇의 좌표를 얻거나 여러 다른 좌표 계산을 할 때 사용하자.
  • costmap
    • map & 장애물 정보가 2D array로 들어온다.
    • collision이나 path를 계산 할 때 사용하자.
• cleanup
  • ros2 lifecycle node의 인터페이스로, plugin을 초기화하는 작동들을 넣어주자
• activate
  • ros2 lifecycle node의 인터페이스로, plugin이 작동 시작하게 하기 위한 절차들을 넣어주자.
  • configure에서 rclcpp_lifecycle::LifecyclePublisher를 생성했다면 여기서 꼭 activate를 해주자.
  • 예시

  void 
  CustomController::configure(
    const rclcpp_lifecycle::LifecycleNode::SharedPtr &parents,
    std::string name, const std::shared_ptr<tf2_ros::Buffer> &tf,
    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> &)
  {
    node_ = parents;
    publisher_ = node_->create_publisher<std_msgs::msg::String>("topic", 10);
  }
  
  void 
  CustomController::activate()
  {
    publisher_->on_activate();
  }

• deactivate
  • activate 반대
• setPlan
  • local planner에게 global plan을 다시 setting한다. (최초와 중간이 같음)
• computeVelocityCommands
  • 현재 로봇 위치와 이전 로봇의 속도가 주어진다.
  • set 된 global plan과 로봇위치, 이전 속도를 가지고 다음으로 로봇이 움직여야 할 cmd_vel을 계산해서 주면 된다.

3. Source file 작성

#include <custom_controller/custom_contorller.hpp>

namespace custom_controller
{
CustomController::CustomController()
: nav2_core::Controller()
{}

void 
CustomController::configure(
  const rclcpp_lifecycle::LifecycleNode::SharedPtr &parents,
  std::string name, const std::shared_ptr<tf2_ros::Buffer> &tf,
  const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> &)
{
  node_ = parents;
  tf_ = tf;
}

void 
CustomController::cleanup()
{}

void 
CustomController::activate()
{
}

void 
CustomController::deactivate()
{}

void 
CustomController::setPlan(const nav_msgs::msg::Path & path)
{}

geometry_msgs::msg::TwistStamped 
CustomController::computeVelocityCommands(
  const geometry_msgs::msg::PoseStamped & pose,
  const geometry_msgs::msg::Twist & velocity)
{
  return geometry_msgs::msg::TwistStamped();
}
} // namespace custom_cotroller

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(custom_cotroller::CustomController, nav2_core::Controller)

!!! 지금은 plugin만드는 법만 설명하니까 세부 구현체들은 넘어간다.

4. CMakeLists.txt 작성

cmake_minimum_required(VERSION 3.5)
project(custom_controller)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(nav2_common REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(std_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(nav2_util REQUIRED)
find_package(nav2_core REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(builtin_interfaces REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(nav2_costmap_2d REQUIRED)
find_package(pluginlib REQUIRED)
find_package(angles REQUIRED)

include_directories(
  include
)

set(library_name custom_controller_core)

set(dependencies
  rclcpp
  rclcpp_action
  rclcpp_lifecycle
  std_msgs
  visualization_msgs
  nav2_util
  nav2_msgs
  nav_msgs
  geometry_msgs
  builtin_interfaces
  tf2_ros
  nav2_costmap_2d
  nav2_core
  pluginlib
)

add_library(${library_name} SHARED
  src/custom_controller.cpp
)

ament_target_dependencies(${library_name}
  ${dependencies}
)

target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")

pluginlib_export_plugin_description_file(nav2_core custom_controller_plugin.xml)

install(TARGETS ${library_name}
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

install(DIRECTORY include/
  DESTINATION include/
)

install(FILES custom_controller_plugin.xml
  DESTINATION share/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_export_include_directories(include)
ament_export_libraries(${library_name})
ament_export_dependencies(${dependencies})
ament_export_definitions("PLUGINLIB__DISBLE_BOOST_FUNCTIONS")
ament_package()

5. package.xml 작성

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>custom_controller</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="root@todo.todo">root</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>rclcpp</depend>
  <depend>rclcpp_action</depend>
  <depend>rclcpp_lifecycle</depend>
  <depend>visualization_msgs</depend>
  <depend>nav2_util</depend>
  <depend>nav2_msgs</depend>
  <depend>nav_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>builtin_interfaces</depend>
  <depend>nav2_common</depend>
  <depend>tf2_ros</depend>
  <depend>nav2_costmap_2d</depend>
  <depend>nav2_core</depend>
  <depend>pluginlib</depend>
  <depend>angles</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
    <nav2_core plugin="${prefix}/custom_controller_plugin.xml" />
  </export>
</package>

6. custom_controller_plugin.xml 작성

<library path="custom_controller_core">
	<class name="custom_controller/CustomController" type="custom_controller::CustomController" base_class_type="nav2_core::Controller">
	  <description></description>
	</class>
</library>

7. Navigation Parameter 변경

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    progress_checker_plugin: "progress_checker"
    goal_checker_plugin: "goal_checker"
    controller_plugins: ["Custom"]

    Custom:
      plugin: "custom_controller/CustomController"

8. BT 변경

<root>
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
					<!-->!!!!!!!!!!!!!!!!!!변경!!!!!!!!!!!!!!!!</--!>
          <FollowPath path="{path}" controller_id="Custom"/>
          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </PipelineSequence>
      <ReactiveFallback name="RecoveryFallback">
        <GoalUpdated/>
        <SequenceStar name="RecoveryActions">
          <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
          <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
          <Spin spin_dist="1.57"/>
          <Wait wait_duration="5"/>
        </SequenceStar>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

그러면 이제 custom으로 만든 controller가 잘 되는 걸 확인 할 수 있다.

위 예제 repository

https://github.com/ladianchad/nav2_study

GitHub - ladianchad/nav2_study