ROS2:: Moveit Task Constructor에 대해서 알아보기(2)

패키지를 생성해준다.

ros2 pkg create \ --build-type ament_cmake \ --dependencies moveit_task_constructor_core rclcpp \ --node-name mtc_node mtc_tutorial

 

ros2로 넘어가면서 노드 이름을 추가해서 cpp 파일을 바로 만들 수 있다.

만들어진 mtc_node.cpp 파일을 확인해보자.

​

#include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/solvers.h>
#include <moveit/task_constructor/stages.h>
#if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>)
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#endif
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h>
#endif

헤더에는 로스2에 필요한 rclcpp와 플래닝 환경에 관한 planning_scene과 interface가 추가되어 있다.

앞선 글에서 말한 MTC에 필요한 task, solvers, stages도 추가되어 있다.

ROS1과 다른 점은 헤더 부분에 전처리를 사용했다는 점.

 

static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial");
namespace mtc = moveit::task_constructor;

class MTCTaskNode
{
public:
  MTCTaskNode(const rclcpp::NodeOptions& options);

  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();

  void doTask();

  void setupPlanningScene();

private:
  // Compose an MTC task from a series of stages.
  mtc::Task createTask();
  mtc::Task task_;
  rclcpp::Node::SharedPtr node_;
};

MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options)
  : node_{ std::make_shared<rclcpp::Node>("mtc_node", options) }
{
}

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface()
{
  return node_->get_node_base_interface();
}

당연히 Logger를 하나 만들어주고

moveit::task_constructor를 namespace 변수로 선언해줬다. ros1에서는 못보던거니 c++ 상위 버전에서만 가능한가보다.

​

MTC Task Node 클래스를 확인할 수 있다.

실제 노드 생성 과정에서는 특정 option을 선택할 수 있게 해놨다.

추가로 node base interface를 가져오는 함수를 선언한다. 이는 추후에 executor에 사용된다.

void MTCTaskNode::doTask()
{
  task_ = createTask();

  try
  {
    task_.init();
  }
  catch (mtc::InitStageException& e)
  {
    RCLCPP_ERROR_STREAM(LOGGER, e);
    return;
  }

  if (!task_.plan(5))
  {
    RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed");
    return;
  }
  task_.introspection().publishSolution(*task_.solutions().front());

  auto result = task_.execute(*task_.solutions().front());
  if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
  {
    RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed");
    return;
  }

  return;
}

MTC는 기본적으로 property들과 stages를 추가해서 task를 만든다.

task 를 만들고, plan()함수를 통해 task를 생성한다. 5개의 성공하는 플랜을 찾으면 멈춘다.

그 다음은 rvzi한테 solution을 publish한다. 마지막으로 excute 한다.

mtc::Task MTCTaskNode::createTask()
{
  moveit::task_constructor::Task task;
  task.stages()->setName("demo task");
  task.loadRobotModel(node_);

  const auto& arm_group_name = "panda_arm";
  const auto& hand_group_name = "hand";
  const auto& hand_frame = "panda_hand";

  // Set task properties
  task.setProperty("group", arm_group_name);
  task.setProperty("eef", hand_group_name);
  task.setProperty("ik_frame", hand_frame);

auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>();

task 다음에는 solver를 추가할 수 있다.

간단하게는 sampling_planner, interpolation_planner, cartesian_planner가 있다.

solver 다음에는 robot을 움직일 stage를 추가해주면 된다.

MopveTo stage를 사용한다. stage 자체에서는 planner를 설정해줄 수 있는 모양.

​

launch 파일 작성 시 URDF, SRDF, OMPL 파라미터들을 불러올 수도 있지만, moveit config utils를 사용하면 쉽게 가능하다.

​

이 다음은 다양한 stage들을 선언하고, 설정하고 task 에 add 해주는 작업들을 진행한다.

또한 중간 중간에 Container들이 들어가기도, wrapper가 들어가기도 한다.

​

 

동작	요소
그리퍼 열기	MoveTo stage
pick 하기 위해 움직이기	Connect stage
물체 집어들기	Serial Container
물체 접근하기	MoveRelative stage
집는 자세 만들기	GenerateGraspPose stage
집는 자세 IK	ComputeIK stage (Wrapper)
PlanningScene 변경	ModifyPlanningScene stage
그리퍼 닫기	MoveTo stage
물체 붙이기	ModifyPlanningScene stage
물체 들어올리기	MoveRelative stage
위치로 움직이기	Connect stage
물체 놓기	Serial Container
놓는 자세 만들기	GeneratePlacePose stage
충돌감지 끄기	ModifyPlanningScene stage
물체 떨어트리기	ModifyPlanningScene stage
로봇 빼기	MoveRelative stage
홈 자세로 오기	MoveTo stage

위와 같은 Stage, Connector, Wrapper가 만들어져서 하나의 Task 를 수행하게 된다.