0편 – [robot arm, manipulator 프로젝트][ubuntu 20.04, ros2, gazebo][0] – ros2, gazebo 설치 및 예제 실행
1편 – [robot arm, manipulator 프로젝트][ubuntu 20.04, ros2, gazebo][1] – ros2 publisher subscriber c++, python 구현 및 실행
2편 – [robot arm, manipulator 프로젝트][ubuntu 20.04, ros2, gazebo][2] – ros2 와 아두이노 통신 (micro ros arduino), OpenCR, robotis, dynamixel 모터 개발 환경 구성 및 예제
3편 – [robot arm, manipulator 프로젝트][ubuntu 20.04, ros2, gazebo][3] – ros2 와 아두이노(OpenCR)로 여러개의 dynamixel 모터 제어 및 엔코더 값 받기, robotis
computer 에서 OpenCR 을 거쳐서 모터를 제어하려고 하였지만 OpenCR 의 부하가 커질 뿐 아니라 로보티즈에서도 로봇을 만들 때 u2d2 를 통해 모터를 제어하고 있는 것으로 확인하여 시스템 구성도를 수정하여 진행
따라서, 2편, 3편은 생략하여도 된다.
1.
mkdir ros2_ws && cd ros2_ws
mkdir src && cd src
git clone -b ros2 https://github.com/ROBOTIS-GIT/DynamixelSDK.git
git clone -b ros2 https://github.com/ROBOTIS-GIT/dynamixel-workbench.git
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake test_dynamixel
cd ~/ros2_ws
colcon build이후 computer-u2d2-dynamixel motor 를 연결한 뒤에
ls /dev/tty*
sudo usermod -aG dialout <username>
test_dynamixel/src 폴더에 test_move_vel.cpp, test_move_pos.cpp 파일 추가
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/int16.hpp"
#include "dynamixel_sdk/dynamixel_sdk.h"
rclcpp::Node::SharedPtr node = nullptr;
using namespace dynamixel;
// Control table address
#define ADDR_OPERATING_MODE 11 // 제어모드
#define ADDR_TORQUE_ENABLE 64 // Torque on/off 제어
#define ADDR_GOAL_POSITION 116 // 위치 제어
#define ADDR_PRESENT_POSITION 132 // 현재 위치
// Protocol version
#define PROTOCOL_VERSION 2.0 // Default Protocol version of DYNAMIXEL X series.
// Default setting
#define DXL1_ID 13 // DXL1 ID
#define DXL2_ID 14 // DXL2 ID
#define BAUDRATE 57600 // Default Baudrate of DYNAMIXEL X series
#define DEVICE_NAME "/dev/ttyUSB0" // Port name
#define VALUE 2048 // 초기 목표 위치 (중간값)
#define OPERATING_VALUE 3 // 제어모드, 위치제어: 3
PortHandler *portHandler;
PacketHandler *packetHandler;
void topic_callback(const std_msgs::msg::Int16::SharedPtr msg)
{
uint8_t dxl_error = 0;
// 목표 위치 설정 (msg->data를 클램핑하여 사용)
int target_position = msg->data;
if (target_position < 0) target_position = 0;
if (target_position > 4095) target_position = 4095;
// 모터 위치 설정
packetHandler->write4ByteTxRx(portHandler, DXL1_ID, ADDR_GOAL_POSITION, target_position, &dxl_error);
RCLCPP_INFO(node->get_logger(), "Set Position: %d", target_position);
}
int main(int argc, char **argv)
{
uint8_t dxl_error = 0;
rclcpp::init(argc, argv);
node = rclcpp::Node::make_shared("Dynamixel");
// PortHandler와 PacketHandler 초기화
portHandler = dynamixel::PortHandler::getPortHandler(DEVICE_NAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
// 포트 열기
if (!portHandler->openPort())
{
RCLCPP_ERROR(node->get_logger(), "Failed to open the port!");
return -1;
}
// Baudrate 설정
if (!portHandler->setBaudRate(BAUDRATE))
{
RCLCPP_ERROR(node->get_logger(), "Failed to set the baudrate!");
return -1;
}
// 제어모드를 위치 제어로 설정
packetHandler->write1ByteTxRx(portHandler, DXL1_ID, ADDR_OPERATING_MODE, OPERATING_VALUE, &dxl_error);
if (dxl_error != 0)
{
RCLCPP_ERROR(node->get_logger(), "Failed to set operating mode!");
return -1;
}
// Torque Enable
packetHandler->write1ByteTxRx(portHandler, DXL1_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error);
if (dxl_error != 0)
{
RCLCPP_ERROR(node->get_logger(), "Failed to enable torque!");
return -1;
}
RCLCPP_INFO(node->get_logger(), "Dynamixel initialized in position control mode.");
// 초기 목표 위치 설정
packetHandler->write4ByteTxRx(portHandler, DXL1_ID, ADDR_GOAL_POSITION, VALUE, &dxl_error);
// ROS2 구독자 생성
auto subscription = node->create_subscription<std_msgs::msg::Int16>(
"/cmd_position", 10, topic_callback);
// ROS2 스핀
rclcpp::spin(node);
// 종료 시 Torque Disable
packetHandler->write1ByteTxRx(portHandler, DXL1_ID, ADDR_TORQUE_ENABLE, 0, &dxl_error);
rclcpp::shutdown();
portHandler->closePort();
return 0;
}위 코드가 test_move_pos.cpp
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/int16.hpp"
#include "dynamixel_sdk/dynamixel_sdk.h"
rclcpp::Node::SharedPtr node = nullptr;
using namespace dynamixel;
// Control table address
#define ADDR_OPERTAING_MODE 11
#define ADDR_TORQUE_ENABLE 64 // Torque on/off 제어
#define ADDR_GOAL_VELOCITY 104 // 속도 제어
#define ADDR_GOAL_POSITION 116 // 위치 제어
#define ADDR_PRESENT_POSITION 132 // 현재 위치
// Protocol version
#define PROTOCOL_VERSION 2.0 // Default Protocol version of DYNAMIXEL X series.
// Default setting
#define DXL1_ID 13 // DXL1 ID
#define DXL2_ID 14 // DXL2 ID
#define BAUDRATE 57600 // Default Baudrate of DYNAMIXEL X series
#define DEVICE_NAME "/dev/ttyUSB0"
#define VALUE 100
#define OPERATING_VALUE 1 // 제어모드, 위치제어: 3, 속도제어: 1
PortHandler *portHandler;
PacketHandler *packetHandler;
void topic_callback(const std_msgs::msg::Int16::SharedPtr msg)
{
uint8_t dxl_error = 0;
packetHandler->write4ByteTxRx(portHandler, DXL1_ID, ADDR_GOAL_VELOCITY, msg->data,&dxl_error);
RCLCPP_INFO(node->get_logger(), "velocity: %d", msg->data);
}
int main(int argc, char **argv)
{
uint8_t dxl_error = 0;
rclcpp::init(argc, argv);
node = rclcpp::Node::make_shared("Dynamixel");
portHandler = dynamixel::PortHandler::getPortHandler(DEVICE_NAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
if(portHandler->openPort() == false)
{
RCLCPP_INFO(node->get_logger(),"Failed to open the port!");
}
if(portHandler->setBaudRate(BAUDRATE) == false)
{
RCLCPP_INFO(node->get_logger(),"Failed to set the baudrate!");
}
packetHandler->write1ByteTxRx(portHandler, DXL1_ID, ADDR_OPERTAING_MODE, OPERATING_VALUE, &dxl_error);
packetHandler->write1ByteTxRx(portHandler, DXL1_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error);
RCLCPP_INFO(node->get_logger(),"start setting");
packetHandler->write4ByteTxRx(portHandler, DXL1_ID, ADDR_GOAL_VELOCITY, VALUE);
auto subscription =
node->create_subscription<std_msgs::msg::Int16>("/cmd_velocty", 10, topic_callback);
rclcpp::spin(node);
packetHandler->write1ByteTxRx(portHandler, DXL1_ID, ADDR_TORQUE_ENABLE, 0, &dxl_error);
rclcpp::shutdown();
return 0;
}위 코드가 test_move_vel.cpp
test_dynamixel/src 폴더에 있는 CMakeLists.txt 파일 수정
cmake_minimum_required(VERSION 3.5)
project(test_dynamixel)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(dynamixel_sdk REQUIRED)
find_package(dynamixel_workbench_toolbox REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
# add_executable(test_move_vel src/test_move_vel.cpp)
# ament_target_dependencies(test_move_vel rclcpp dynamixel_sdk dynamixel_workbench_toolbox std_msgs)
# add_executable(test_move_pos src/test_move_pos.cpp)
# ament_target_dependencies(test_move_pos rclcpp dynamixel_sdk dynamixel_workbench_toolbox std_msgs)
# install(TARGETS
# test_move_pos
# test_move_vel
# DESTINATION lib/${PROJECT_NAME})
set(EXECUTABLES
test_move_vel
test_move_pos
)
foreach(EXEC IN LISTS EXECUTABLES)
add_executable(${EXEC} src/${EXEC}.cpp)
ament_target_dependencies(${EXEC} rclcpp dynamixel_sdk dynamixel_workbench_toolbox std_msgs)
install(TARGETS ${EXEC}
DESTINATION lib/${PROJECT_NAME})
endforeach()
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()
test_dynamixel/src 폴더에 있는 package.xml 파일 수정
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>test_dynamixel</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="jw@todo.todo">jw</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<depend>rclcpp</depend>
<depend>dynamixel_sdk</depend>
<depend>dynamixel_workbench_toolbox</depend>
<depend>std_msgs</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
이제
cd ~/ros2_ws/
colcon build
source /opt/ros/foxy/setup.bash
source ~/ros2_ws/install/local_setup.bash
ros2 run test_dynamixel test_move_pos /dev/ttyUSB0혹은
cd ~/ros2_ws/
colcon build
source /opt/ros/foxy/setup.bash
source ~/ros2_ws/install/local_setup.bash
ros2 run test_dynamixel test_move_vel /dev/ttyUSB0
위 사진과 같이 rqt 를 통해 모터의 속도와 위치를 변화시켜 볼 수 있다.
0 Comments