이전에 진행한 KIST SLAM & Navigation(MOMA) 프로젝트에서 Local Planner를 mppi를 기반으로 한 planner로 바꾸어주기 위해 move_base 노드에 mppi 를 기반으로 한 planner를 등록해보자.

mppi 소스 코드는 https://github.com/jebeom/proj-svg_mppi 여기를 참고했다 !

1. mppi_plugin.xml 파일 생성

<library path="lib/libmppi_local_planner">
    <class name="mppi_local_planner/MppiLocalPlanner" type="mppi_local_planner::MppiLocalPlanner" base_class_type="nav_core::BaseLocalPlanner">
        <description>
            Simplest Plugin for BaseLocalPlanner
        </description>
    </class>
</library>

2. CMakeLists.txt 확인

CMakeLists.txt 파일에서 플러그인 라이브러리를 빌드하고 설치하는 부분이 올바르게 설정되어 있는지 확인합니다.

cmake_minimum_required(VERSION 3.0.2)
project(mppi_local_planner)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  pluginlib
  nav_core
  costmap_2d
  geometry_msgs
)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES mppi_local_planner
  CATKIN_DEPENDS roscpp pluginlib nav_core costmap_2d geometry_msgs
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

add_library(mppi_local_planner
  src/mppi_local_planner.cpp
)

target_link_libraries(mppi_local_planner
  ${catkin_LIBRARIES}
)

install(TARGETS mppi_local_planner
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)
message(STATUS "CATKIN_PACKAGE_SHARE_DESTINATION: ${CATKIN_PACKAGE_SHARE_DESTINATION}")
message(STATUS "curren directory: ${CMAKE_CURRENT_SOURCE_DIR}")

install(FILES /home/jebeom/kist_slam_navi/catkin_ws/src/summit-pc/ROAS/src/summit_packages/robot/summit_xl_common/mppi_planner/src/mppi_local_planner/mppi_plugin.xml
  DESTINATION /home/jebeom/kist_slam_navi/catkin_ws/src/summit-pc/ROAS/src/summit_packages/robot/summit_xl_common/mppi_planner/devel/share/mppi_local_planner/mppi_plugin.xml
)

3. package.xml 파일 수정

<?xml version="1.0"?>
<package format="2">
  <name>mppi_local_planner</name>
  <version>0.0.0</version>
  <description>The mppi_local_planner package</description>

  <maintainer email="[email protected]">Your Name</maintainer>
  <license>BSD</license>

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>
  <build_depend>pluginlib</build_depend>
  <build_depend>nav_core</build_depend>
  <build_depend>costmap_2d</build_depend>
  <build_depend>geometry_msgs</build_depend>

  <exec_depend>roscpp</exec_depend>
  <exec_depend>pluginlib</exec_depend>
  <exec_depend>nav_core</exec_depend>
  <exec_depend>costmap_2d</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>

  <export>
    <nav_core plugin="${prefix}/mppi_plugin.xml"/>
  </export>
</package>

4. 헤더파일

패키지를 클린 빌드해봅니다.

#ifndef LOCAL_PLANNER_H_
#define LOCAL_PLANNER_H_

#include <nav_core/base_local_planner.h>
#include <ros/ros.h>
#include <tf2_ros/buffer.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>

using namespace std;

namespace mppi_local_planner {

class MppiLocalPlanner : public nav_core::BaseLocalPlanner {
public:
    MppiLocalPlanner();
    MppiLocalPlanner(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros);

    ~MppiLocalPlanner();

    void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros);

    bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);

    bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);

    bool isGoalReached();
private:
    costmap_2d::Costmap2DROS* costmap_ros_;
    tf2_ros::Buffer* tf_;
    bool initialized_;
};

}

#endif

5. 소스코드

local planner를 ROS 에서 새로 등록하려면 4개의 함수들은 무조건 필요하다.

initialize 함수 : tf나 costmap 같은 것을 지속적으로 초기화 ****

setplan : 경로 생성

computevelocitycommands 함수 : 모바일 로봇은 속도와 각속도만 있으면 움직일 수 있으므로, mppi의 경우 속도를 샘플링 하는 알고리즘을 여기 함수에 같이 넣어 속도와 각속도값을 계산