ROS2 moveit2(1)

전에 만들었던 moveit2 환경에서 첫번째 c++ moveit 프로젝트를 만들어 보겠다.

cd ~/ws_moveit/src

ros2 pkg create \
 --build-type ament_cmake \
 --dependencies moveit_ros_planning_interface rclcpp \
 --node-name hello_moveit hello_moveit

package.xml과 CMakeLists.txt에 의존성을 추가하였다.

 

여기서 새로운 소스파일인 ws_moveit/src/hello_moveit/src/hello_moveit.cpp 를 편집기에서 연다.

#include <memory>

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char * argv[])
{
  // Initialize ROS and create the Node
  rclcpp::init(argc, argv);
  auto const node = std::make_shared<rclcpp::Node>(
    "hello_moveit",
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

  // Create a ROS logger
  auto const logger = rclcpp::get_logger("hello_moveit");

  // Next step goes here

  // Shutdown ROS
  rclcpp::shutdown();
  return 0;
}

 

내용을 저장 후 빌드 및 실행을 한다.

colcon build --mixin debug
source install/setup.bash
ros2 run hello_moveit hello_moveit

 

실행했을때 아무런 오류가 뜨지않고 종료된다.

이제 여기에 position 값을 주도록 해보겠다.

좌표값을 주면 moveit2가 역기구학(ik) 계산을 해서 이동경로를 생성한다. 

// Create the MoveIt MoveGroup Interface
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "manipulator");

// Set a target Pose
auto const target_pose = []{
  geometry_msgs::msg::Pose msg;
  msg.orientation.w = 1.0;
  msg.position.x = 0.28;
  msg.position.y = -0.2;
  msg.position.z = 0.5;
  return msg;
}();
move_group_interface.setPoseTarget(target_pose);

// Create a plan to that target pose
auto const [success, plan] = [&move_group_interface]{
  moveit::planning_interface::MoveGroupInterface::Plan msg;
  auto const ok = static_cast<bool>(move_group_interface.plan(msg));
  return std::make_pair(ok, msg);
}();

// Execute the plan
if(success) {
  move_group_interface.execute(plan);
} else {
  RCLCPP_ERROR(logger, "Planning failed!");
}

 

ros2 launch moveit2_tutorials demo.launch.py

ros2 run hello_moveit hello_moveit

좌표값으로 이동하는 로봇팔을 볼 수 있다.(ik)

기존에 만들었던 rb10-1300으로 똑같은 코드를 돌려봤더니 확실히 로봇의 기준좌표가 다르다보니 다른 모양이 나왔는데 이건 조금 더 공부해봐야 알 것 같다.