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 구현 및 실행
1. 아두이노 IDE & OpenCR 개발환경
링크 에서 아두이노 설치 파일 다운
다운로드된 .Appimage 확장자의 파일은 더블 클릭 하거나 터미널에서 ./arduino~~.Appimage 를 입력하면 실행된다.
이후 터미널에서 아래와 같이 아두이노에서 사용할 포트 관련 설정을 포함한 기타 작업을 진행해준다.
wget https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCR/master/99-opencr-cdc.rules
sudo cp ./99-opencr-cdc.rules /etc/udev/rules.d/
sudo udevadm control --reload-rules
sudo udevadm trigger
sudo apt-get install libncurses5-dev:i386
위 사진과 같이 설정의 preferences 에서 additional boards manager URLs 추가
https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCR/master/arduino/opencr_release/package_opencr_index.json
tools->board->boards manager 에서 opencr 보드 검색하여 install
위 사진과 같이 board 와 port 를 설정해주면 연결이 잘 된 것을 볼 수 있다.
2. ROS2 와 아두이노 통신
microROS 활용하여 통신 해보자
2.1. 환경 설정
먼저 본인의 ros2 workspace 에서 작업 시작
cd ~/ros2_ws/src
git clone -b $ROS_DISTRO https://github.com/micro-ROS/micro_ros_setup.git
sudo apt update && rosdep update
rosdep install --from-paths src --ignore-src -y
cd ~/ros2_ws
colcon build
source install/local_setup.bash
cd ~/ros2_ws
ros2 run micro_ros_setup create_agent_ws.sh
ros2 run micro_ros_setup build_agent.sh
source install/local_setup.bash
cd ~/ros2_ws
colcon build
source install/local_setup.bash
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0
위의 사진처럼 된다면 성공
이후 아두이노에서 micro-ros 라이브러리 설치
링크에서 본인 ros 버전에 맞는 zip 파일 다운로드
아두이노 ide 에서 Sketch->include library-> add .ZIP Library 를 통해 다운로드한 라이브러리 추가
2.2. Publisher
file-> example-> micro_ros_arduino 에서 publisher 예제를 컴파일&업로드 한다. 만약 실패한다면
/home/jw/.arduino15/packages/arduino/hardware/avr/1.8.6/
or
/home/jw/.Arduino15/packages/arduino/hardware/sam/1.6.12/위와 같은 경로에서 터미널을 열고
curl https://raw.githubusercontent.com/micro-ROS/micro_ros_arduino/main/extras/patching_boards/platform_arduinocore_sam.txt > platform.txt위 코드를 실행한다. 이후
각각의 새로운 터미널 2개에서
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0
ros2 topic echo /micro_ros_arduino_node_publisher위에 코드를 각각 입력하면 아래와 같은 결과를 얻을 수 있다.
rqt 를 통해서도 확인 할 수 있다.
결국 위에 작업은 추후 모터에서 얻을 엔코더 각도, 각속도, 전류 값을 ros2 로 보내주기 위한 작업이다.
2.3. subscriber
아두이노 ide 에서 file->examples->micro_ros_arduino 에서 subscriber 예제를 업로드 한다.
이때 터미널에서 ros2 run micro_ros_agent micro_ros_agent serial –dev /dev/ttyACM0 는 실행되고 있어야 한다.
rqt 에 plugins -> message publisher 를 열고 Hz 옆에 + 버튼을 눌러준다.
아래에 topic 이 추가되면 expression 에 1 혹은 0을 입력한뒤 오른쪽 마우스 클릭하여 published selected once 를 누르면 1인지 0인지에 따라 보드에서 led 가 켜지거나 꺼진다.
결국 이 작업은 원하는 각도록 보드에 연결된 모터를 제어하기 위해 필요하다.
2.4. sub & pub 동시에
결국에는
ros -> 모터 각도값 -> opencr -> 모터
ros <- opencr<- 모터 엔코더 값, 전류 기타 등등
이러한 형태가 되어야 하기 때문에 sub 과 pub 이 따로따로 수행되면 안된다.
때문에 subscribe 할 때마다 Publish 하는 코드를 작성하고 테스트 하였다.
#include <micro_ros_arduino.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int32.h>
rcl_subscription_t subscriber;
rcl_publisher_t publisher;
std_msgs__msg__Int32 sub_msg;
std_msgs__msg__Int32 pub_msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
#define LED_PIN 13
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
void error_loop() {
while (1) {
digitalWrite(LED_PIN, !digitalRead(LED_PIN));
delay(100);
}
}
void subscription_callback(const void *msgin) {
const std_msgs__msg__Int32 *msg = (const std_msgs__msg__Int32 *)msgin;
// Toggle LED state based on the received message
if (msg->data == 0) {
digitalWrite(22, LOW); // Turn LED OFF
} else {
digitalWrite(22, HIGH); // Turn LED ON
}
// Publish the incremented value
pub_msg.data++;
RCSOFTCHECK(rcl_publish(&publisher, &pub_msg, NULL));
}
void setup() {
set_microros_transports();
pinMode(LED_PIN, OUTPUT);
digitalWrite(LED_PIN, HIGH);
delay(2000);
allocator = rcl_get_default_allocator();
// Initialize support
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
// Create node
RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));
// Create subscriber
RCCHECK(rclc_subscription_init_default(
&subscriber,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
"micro_ros_arduino_subscriber"));
// Create publisher
RCCHECK(rclc_publisher_init_default(
&publisher,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
"micro_ros_arduino_publisher"));
// Create executor
RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator));
RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &subscription_callback, ON_NEW_DATA));
// Initialize publisher message
pub_msg.data = 0;
}
void loop() {
RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
delay(100); // Small delay to avoid busy loop
}
위 코드를 통해 0 혹은 1 값을 sub 하여 led 를 키거나 끄고 이와 동시에 data 를 sub 할 때마다 1 씩 증가하는 data를 pub 하는 것을 확인하였다.
3. 아두이노(OpenCR 보드) 와 모터(Dynamixel) 통신
구체적으로는 로보티즈 회사의 xl430 모터를 다루고 있다.
먼저 로보티즈 사의DYNAMIXEL Wizard 2.0 를 통해 각각의 모터를 돌려보았다.
2개의 xl430 모터의 id 를 확인 하고 다르게 설정해주었다.
#include <DynamixelSDK.h>
// Control Table Address (XL430)
#define ADDR_XL_TORQUE_ENABLE 64 // Torque Enable Address
#define ADDR_XL_GOAL_POSITION 116 // Goal Position Address
#define ADDR_XL_PRESENT_POSITION 132 // Present Position Address
// Data Byte Length
#define LEN_XL_GOAL_POSITION 4
#define LEN_XL_PRESENT_POSITION 4
// Protocol version
#define PROTOCOL_VERSION 2.0 // XL430 uses Protocol 2.0
// Default setting
#define DXL1_ID 13 // Dynamixel#1 ID
#define DXL2_ID 14 // Dynamixel#2 ID
#define BAUDRATE 57600 // Baudrate
#define DEVICENAME "OpenCR_DXL_Port" // Serial Port
#define TORQUE_ENABLE 1 // Value for enabling the torque
#define TORQUE_DISABLE 0 // Value for disabling the torque
#define DXL_MINIMUM_POSITION_VALUE 0 // Minimum position value (in pulse)
#define DXL_MAXIMUM_POSITION_VALUE 4095 // Maximum position value (in pulse)
#define DXL_MOVING_STATUS_THRESHOLD 10 // Moving status threshold
#define CMD_SERIAL Serial
int getch()
{
while (1)
{
if (CMD_SERIAL.available() > 0)
{
break;
}
}
return CMD_SERIAL.read();
}
void setup()
{
CMD_SERIAL.begin(115200);
while (!CMD_SERIAL);
CMD_SERIAL.println("Start...");
// Initialize PortHandler instance
dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
// Initialize PacketHandler instance
dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
// Initialize GroupSyncWrite and GroupSyncRead instances
dynamixel::GroupSyncWrite groupSyncWrite(portHandler, packetHandler, ADDR_XL_GOAL_POSITION, LEN_XL_GOAL_POSITION);
dynamixel::GroupSyncRead groupSyncRead(portHandler, packetHandler, ADDR_XL_PRESENT_POSITION, LEN_XL_PRESENT_POSITION);
int dxl_comm_result;
uint8_t dxl_error;
int32_t dxl1_present_position, dxl2_present_position;
uint8_t param_goal_position[4];
int dxl_goal_position[2] = {DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE};
int index = 0;
// Open port
if (!portHandler->openPort() || !portHandler->setBaudRate(BAUDRATE))
{
CMD_SERIAL.println("Failed to open the port or set baudrate.");
return;
}
CMD_SERIAL.println("Port opened!");
// Enable Torque for both motors
for (int id : {DXL1_ID, DXL2_ID})
{
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, id, ADDR_XL_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS || dxl_error != 0)
{
CMD_SERIAL.print("[ID:"); CMD_SERIAL.print(id); CMD_SERIAL.println("] Torque Enable failed.");
return;
}
}
CMD_SERIAL.println("Motors connected!");
// Add both motors to GroupSyncRead
if (!groupSyncRead.addParam(DXL1_ID) || !groupSyncRead.addParam(DXL2_ID))
{
CMD_SERIAL.println("Failed to add parameters to GroupSyncRead.");
return;
}
while (1)
{
CMD_SERIAL.println("Press any key to continue! (or press 'q' to quit)");
if (getch() == 'q')
break;
// Prepare Goal Position parameters
for (int id : {DXL1_ID, DXL2_ID})
{
param_goal_position[0] = DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[index]));
param_goal_position[1] = DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[index]));
param_goal_position[2] = DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[index]));
param_goal_position[3] = DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[index]));
if (!groupSyncWrite.addParam(id, param_goal_position))
{
CMD_SERIAL.print("[ID:"); CMD_SERIAL.print(id); CMD_SERIAL.println("] Failed to add goal position.");
return;
}
}
// Send Goal Position to both motors
dxl_comm_result = groupSyncWrite.txPacket();
if (dxl_comm_result != COMM_SUCCESS)
{
CMD_SERIAL.println(packetHandler->getTxRxResult(dxl_comm_result));
}
groupSyncWrite.clearParam();
// Wait for motors to reach the goal
do
{
dxl_comm_result = groupSyncRead.txRxPacket();
if (dxl_comm_result != COMM_SUCCESS)
{
CMD_SERIAL.println(packetHandler->getTxRxResult(dxl_comm_result));
break;
}
// Retrieve Present Position for both motors
dxl1_present_position = groupSyncRead.getData(DXL1_ID, ADDR_XL_PRESENT_POSITION, LEN_XL_PRESENT_POSITION);
dxl2_present_position = groupSyncRead.getData(DXL2_ID, ADDR_XL_PRESENT_POSITION, LEN_XL_PRESENT_POSITION);
CMD_SERIAL.print("[ID:"); CMD_SERIAL.print(DXL1_ID);
CMD_SERIAL.print("] Present Position: "); CMD_SERIAL.print(dxl1_present_position);
CMD_SERIAL.print(" [ID:"); CMD_SERIAL.print(DXL2_ID);
CMD_SERIAL.print("] Present Position: "); CMD_SERIAL.println(dxl2_present_position);
} while (abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD ||
abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD);
index = (index == 0) ? 1 : 0; // Toggle goal position
}
// Disable Torque for both motors
for (int id : {DXL1_ID, DXL2_ID})
{
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, id, ADDR_XL_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS || dxl_error != 0)
{
CMD_SERIAL.print("[ID:"); CMD_SERIAL.print(id); CMD_SERIAL.println("] Torque Disable failed.");
}
}
// Close port
portHandler->closePort();
CMD_SERIAL.println("Port closed!");
}
void loop()
{
}4. 최종 ROS2, OpenCR, Dynamixel 모터(xl430) 통신
#include <micro_ros_arduino.h>
#include <DynamixelSDK.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
// Include the required message types
#include <std_msgs/msg/int32.h>
#include <std_msgs/msg/int32_multi_array.h>
rcl_subscription_t subscriber;
rcl_publisher_t publisher;
std_msgs__msg__Int32 sub_msg;
std_msgs__msg__Int32MultiArray pub_msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
#define LED_PIN 13
// Control Table Address (XL430)
#define ADDR_XL_TORQUE_ENABLE 64 // Torque Enable Address
#define ADDR_XL_GOAL_POSITION 116 // Goal Position Address
#define ADDR_XL_PRESENT_POSITION 132 // Present Position Address
// Data Byte Length
#define LEN_XL_GOAL_POSITION 4
#define LEN_XL_PRESENT_POSITION 4
// Protocol version
#define PROTOCOL_VERSION 2.0 // XL430 uses Protocol 2.0
// Default setting
#define DXL1_ID 13 // Dynamixel#1 ID
#define DXL2_ID 14 // Dynamixel#2 ID
#define BAUDRATE 57600 // Baudrate
#define DEVICENAME "OpenCR_DXL_Port" // Serial Port
#define TORQUE_ENABLE 1 // Value for enabling the torque
#define TORQUE_DISABLE 0 // Value for disabling the torque
#define DXL_MINIMUM_POSITION_VALUE 0 // Minimum position value (in pulse)
#define DXL_MAXIMUM_POSITION_VALUE 4095 // Maximum position value (in pulse)
#define DXL_MOVING_STATUS_THRESHOLD 10 // Moving status threshold
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
dynamixel::GroupSyncWrite *groupSyncWrite;
dynamixel::GroupSyncRead *groupSyncRead;
int32_t dxl_present_positions[2];
void error_loop() {
while (1) {
digitalWrite(LED_PIN, !digitalRead(LED_PIN));
delay(100);
}
}
// Helper to clamp values
int clamp(int value, int min_value, int max_value) {
if (value < min_value) return min_value;
if (value > max_value) return max_value;
return value;
}
// Subscription Callback
void subscription_callback(const void *msgin) {
const std_msgs__msg__Int32 *msg = (const std_msgs__msg__Int32 *)msgin;
// Clamp the received value within the allowed range
int target_position = clamp(msg->data, DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE);
// Prepare goal position parameter
uint8_t param_goal_position[4];
param_goal_position[0] = DXL_LOBYTE(DXL_LOWORD(target_position));
param_goal_position[1] = DXL_HIBYTE(DXL_LOWORD(target_position));
param_goal_position[2] = DXL_LOBYTE(DXL_HIWORD(target_position));
param_goal_position[3] = DXL_HIBYTE(DXL_HIWORD(target_position));
// Add goal position for DXL1 and DXL2
groupSyncWrite->addParam(DXL1_ID, param_goal_position);
groupSyncWrite->addParam(DXL2_ID, param_goal_position);
// Write goal positions
groupSyncWrite->txPacket();
groupSyncWrite->clearParam();
// Read current positions
groupSyncRead->txRxPacket();
dxl_present_positions[0] = groupSyncRead->getData(DXL1_ID, ADDR_XL_PRESENT_POSITION, LEN_XL_PRESENT_POSITION);
dxl_present_positions[1] = groupSyncRead->getData(DXL2_ID, ADDR_XL_PRESENT_POSITION, LEN_XL_PRESENT_POSITION);
// Publish the current positions as an array
pub_msg.data.data[0] = dxl_present_positions[0];
pub_msg.data.data[1] = dxl_present_positions[1];
RCSOFTCHECK(rcl_publish(&publisher, &pub_msg, NULL));
}
void setup() {
// Setup ROS transport
set_microros_transports();
pinMode(LED_PIN, OUTPUT);
digitalWrite(LED_PIN, HIGH);
delay(2000);
allocator = rcl_get_default_allocator();
// Initialize support
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
// Create node
RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));
// Create subscriber
RCCHECK(rclc_subscription_init_default(
&subscriber,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
"micro_ros_arduino_subscriber"));
// Create publisher
RCCHECK(rclc_publisher_init_default(
&publisher,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32MultiArray),
"micro_ros_arduino_publisher"));
// Create executor
RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator));
RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &subscription_callback, ON_NEW_DATA));
// Initialize Dynamixel Communication
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
groupSyncWrite = new dynamixel::GroupSyncWrite(portHandler, packetHandler, ADDR_XL_GOAL_POSITION, LEN_XL_GOAL_POSITION);
groupSyncRead = new dynamixel::GroupSyncRead(portHandler, packetHandler, ADDR_XL_PRESENT_POSITION, LEN_XL_PRESENT_POSITION);
if (!portHandler->openPort()) error_loop();
if (!portHandler->setBaudRate(BAUDRATE)) error_loop();
uint8_t dxl_error;
int dxl_comm_result;
// Enable motor torque
for (int id : {DXL1_ID, DXL2_ID}) {
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, id, ADDR_XL_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS || dxl_error != 0) error_loop();
groupSyncRead->addParam(id);
}
// Initialize publisher message
pub_msg.data.data = (int32_t *)malloc(2 * sizeof(int32_t)); // Allocate memory for two integers
pub_msg.data.size = 2; // Array size
pub_msg.data.capacity = 2; // Array capacity
}
void loop() {
RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
delay(100); // Small delay to avoid busy loop
}위에 코드를 통해 sub 하는 값으로 모터를 움직이고, 모터에서 얻은 엔코더 값을 pub 하는 것을 구현하였다.
message publisher 를 통해 xl430 모터의 동작 범위 0~4028 내에서 값을 입력하여 메세지를 보내면 이를 openCR 이 sub 하고 모터가 움직인다. 그리고 해당 모터의 엔코더 값을 읽고 각 모터의 엔코더 값을 pub 한다.
0 Comments