이전에 진행한 KIST SLAM & Navigation(MOMA) 프로젝트에서 Local Planner를 mppi를 기반으로 한 planner로 바꾸어주기 위해 move_base 노드에 mppi 를 기반으로 한 planner를 등록해보자.
mppi 소스 코드는 https://github.com/jebeom/proj-svg_mppi 여기를 참고했다 !
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>
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
)
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>
패키지를 클린 빌드해봅니다.
#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
local planner를 ROS 에서 새로 등록하려면 4개의 함수들은 무조건 필요하다.
initialize 함수 : tf나 costmap 같은 것을 지속적으로 초기화 ****
setplan : 경로 생성
computevelocitycommands 함수 : 모바일 로봇은 속도와 각속도만 있으면 움직일 수 있으므로, mppi의 경우 속도를 샘플링 하는 알고리즘을 여기 함수에 같이 넣어 속도와 각속도값을 계산