ROS2 MoveIt2 이용하여 UR3e 의 Link Position 및 Quaternion 획득하기

 

 

지난 글들에서, UR3e 에 Intel RealSense ROS2 패키지를 추가하여 vision을 달고,

고정 위치에 대해 pick and place를 수행해보았습니다.

ROS2 MoveIt2 에서 UR3e + Intel RealSense Pick and Place 예제 수행

 

고정 위치에 대해서가 아닌 vision으로 인식된 목표 위치에 대해서 작동을 하게 만들고자 하기 때문에

Depth camera에 인식된 물체가 real world 좌표로 어디에 위치하는지 변환이 가능해야합니다.

 

이를 위해 가장 먼저 해야할 것은 camera transformation matrix 구성을 해야합니다.

이에 대해서는 다음 글에서 다루도록 하고,

 

camera transformation matrix 를 얻기 위해서는

camera에 보이는 물체 위치의 pixel값 depth값, camera의 현재 위치와 쿼터니언이 필요합니다.

 

저는 로봇의 마지막 관절에 카메라를 달았기 때문에,

로봇이 작동함에 따라 카메라의 위치와 쿼터니언이 계속 변하게 됩니다.

따라서 이 값을 지속적으로 읽어올 필요성이 있습니다.

 

우선적으로 ROS2에서 카메라의 현재 위치와 쿼터니언을 얻는 작업을 먼저 수행해보겠습니다.

 

그전에 먼저 joint state를 얻어보는 예제를 수행해보겠습니다.

여태까지의 과정을 따라오셨다면 별도의 설치 없이 확인이 가능합니다.

첫 번째 터미널에서 다음 명령을 수행합니다.

export COLCON_WS=~/workspace/ros_ur_driver

cd $COLCON_WS

source install/setup.bash

ros2 launch ur_calibration calibration_correction.launch.py robot_ip:=192.168.1.101 target_filename:="${HOME}/my_robot_calibration.yaml"

ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur3e robot_ip:=192.168.1.101 launch_rviz:=false

 

두 번째 터미널에서 다음 명령을 수행합니다.

export COLCON_WS=~/workspace/ros_ur_driver

cd $COLCON_WS

source install/setup.bash

ros2 topic echo /joint_states

 

약 500hz 의 속도로 현재 joint state 가 얻어집니다.

그러나 각 joint의 radian만을 반환합니다.

특정 joint 의 x, y, z position 과 쿼터니언을 얻기 위해서는,

이미 기구학 계산을 편리하게 수행해주는 moveit이 필요합니다.

 

따라서 해당 값을 얻어오는 패키지를 생성하겠습니다.

일단 최종적 목표는 해당 위치를 이용하여 카메라의 현재 위치를 얻는 것이 목표이므로

ur_camera_position 이라는 패키지를 만들어주겠습니다.

 

제가 Github 레포지토리를 이미 만들어놓았고, 이를 이용하면 Camera의 현재 위치를 얻어오는 것이 가능합니다.

cd ~/workspace/ros_ur_driver/src/Universal_Robots_ROS2_Driver

git clone https://github.com/cobang0111/ur_camera_position.git

 

GitHub - cobang0111/ur_camera_position: ROS2 Universal Robot - Get Camera Position

 

그리고 이 패키지의 src 디렉토리에 다음과 같은 내용으로

ur_camera_position.cpp 파일이 존재합니다.

#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#define PI 3.141592

using namespace std::chrono_literals; // For using milliseconds

int main(int argc, char * argv[])
{

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

  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  auto move_group_node =
      rclcpp::Node::make_shared("ur_camera_position", node_options);

  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(move_group_node);
  std::thread([&executor]() { executor.spin(); }).detach();

  static const std::string PLANNING_GROUP_ARM = "ur_manipulator";

  moveit::planning_interface::MoveGroupInterface move_group_arm(
      move_group_node, PLANNING_GROUP_ARM);

  //const moveit::core::JointModelGroup *joint_model_group_arm =
  //    move_group_arm.getCurrentState()->getJointModelGroup(PLANNING_GROUP_ARM);

  static const std::string CAMERA_LINK = "camera_link";

  // Get position and quaternion of camera
  while (rclcpp::ok()) {
    geometry_msgs::msg::PoseStamped current_pose = move_group_arm.getCurrentPose(CAMERA_LINK);
    RCLCPP_INFO(move_group_node->get_logger(), "Camera Position: (%.6f, %.6f, %.6f)",
                current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z);
    RCLCPP_INFO(move_group_node->get_logger(), "Camera Quaternion : (%.6f, %.6f, %.6f, %.6f)",
                current_pose.pose.orientation.x, current_pose.pose.orientation.y,
                current_pose.pose.orientation.z, current_pose.pose.orientation.w);
    rclcpp::sleep_for(std::chrono::seconds(1));
  }

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

 

코드는 정말 간단합니다.

MoveGroupInterface를 이용하여 현재 관절의 정보를 얻어오는 것이 다입니다.

 

만약 camera가 아닌 end effector 의 위치를 얻고싶다면

move_group_arm.getCurrentPose(CAMERA_LINK);

이 코드에서 CAMELA_LINK를 삭제하고

static const std::string CAMERA_LINK = "camera_link";

를 주석 처리하면 됩니다.

 

이제 빌드를 하고 결과를 얻어보겠습니다.

첫 번째 터미널에서 rviz를 실행시켜줍니다.

export COLCON_WS=~/workspace/ros_ur_driver

cd $COLCON_WS

colcon build --packages-select ur_camera_position

source install/setup.bash

ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur3e robot_ip:=192.168.1.101 launch_rviz:=false

 

두 번째 터미널에서 moveit을 실행시켜줍니다.

export COLCON_WS=~/workspace/ros_ur_driver

cd $COLCON_WS

source install/setup.bash

ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur3e launch_rviz:=true robot_ip:=192.168.1.101 reverse_ip:=192.168.1.102

 

그리고 세 번째 터미널에서 새로 설치한 패키지를 실행시켜줍니다.

export COLCON_WS=~/workspace/ros_ur_driver

cd $COLCON_WS

source install/setup.bash

ros2 launch ur_camera_position ur_camera_position_launch.py

 

다음과 같이 현재 로봇의 camera가 위치한 xyz position과 quaternion 값이 1초에 한 번씩 출력됩니다.

 

그럼 이제 다음 글에서 이를 이용해서 camera transformation matrix를 얻어보겠습니다.