ROS2 카메라&서보모터 :: object tracking (6)

임선자·2024년 2월 20일

ROS2 - camera tracking

ROS2 - 카메라&서보모터

목록 보기

6/6

- 2024.02.20 -

1. 문제점 분석

ROS2 카메라&서보모터 :: object tracking (5)의 코드는 문제점이 많았다.

우선, 서보모터의 움직이 너무 극단적이었다.

극단적이었다는 것은 즉, 서보모터의 움직임이 원하는 것보다 컸다는 것이다.

서보모터가 한번에 많은 각도를 돌아버리면(=회전속도가 빠를 때) 오버슛이 발생하는 경우가 엄청 많았다.

따라서, 서보모터의 제어값을 할당하는 코드를 조금 수정할 필요를 느꼈다.

서보모터의 움직임을 부드럽게 가져가고 싶어
제어 방법에 대해 구글링을 조금 해보았다.

무한 회전형 서보모터는 정교한 제어가 어렵다고 한다... 사진 출처

또한, 일반 서보모터는 각도 제어가 가능하여 조금 더 세밀한 제어가 가능하지만

무한 회전형 서보모터는 각도 제어가 불가능하여 정확하고 세밀한 제어가 힘들다고 한다.

심지어.... 내가 지금 사용하고 있는 서보모터는 물에 빠진 적이 있어
180, 0의 제어값(다른 방향, 같은 속도)을 줬을 때 회전 속도가 다른 문제점이 있다....

그래서, 코드의 제어값 환산식을 수정해가면서 조금이라도 서보모터의 움직임이 부드러워지도록 하는 것을 목표로 잡게 되었다.

(~~참... 타지에서 한정적인 자원으로 Robotics 학습을 한다는 것이 쉽지가 않습니다....~~)

2. 제어값 수정

먼저 ROS2 카메라&서보모터 :: object tracking (5)에서 설정했던 서보모터 정지 상태의 기준치를 살짝 키워주었다.

이를 통해, 서보모터의 회전량이 많아 다시 반대방향으로 틀어버리는 현상을 줄일 수 있을 거라고 판단하였다.

원래의 정지 상태 기준치 : 220 <= x <= 420
변경한 정지 상태 기준치 : 195 <= x <= 445

두 번째로는, 아두이노 코드에서 제어값을 수정해주었다.

#include <micro_ros_arduino.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/float32.h>

#include <Servo.h>  

rcl_subscription_t subscriber;
std_msgs__msg__Float32 pwm_msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

Servo servo;  

#define PWM_PIN 9 
#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) {
    delay(1); // default 100
  }
}

void subscription_callback(const void *msgin) {
  const std_msgs__msg__Float32 *msg = (const std_msgs__msg__Float32 *)msgin;

  Serial.print("Received PWM: ");
  Serial.println(msg->data);

  int pwm_value = static_cast<int>(msg->data);
  pwm_value = constrain(pwm_value, 0, 180);

  int servo_value;
  
  if (pwm_value == 90) {
    servo_value = 90; 
  } else {
    if (pwm_value < 90) {
      servo_value = static_cast<int>(90 - (pwm_value * 0.5));
    } else {
      servo_value = static_cast<int>((pwm_value * 0.5) + 90);
    }
  }

  servo.write(servo_value);
  
}

void setup() {
  set_microros_transports();

  Serial.begin(115200);

  servo.attach(PWM_PIN);

  allocator = rcl_get_default_allocator();

  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));

  RCCHECK(rclc_subscription_init_default(
      &subscriber,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
      "motor_control"));

  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &pwm_msg, &subscription_callback, ON_NEW_DATA));
}

void loop() {
  delay(1);
  RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10))); // default : 100
}

위의 코드에서 중요하게 봐야할 부분은

  if (pwm_value == 90) {
    servo_value = 90; 
  } else {
    if (pwm_value < 90) {
      servo_value = static_cast<int>(90 - (pwm_value * 0.5));
    } else {
      servo_value = static_cast<int>((pwm_value * 0.5) + 90);
    }
  }

이다.

우분투에서 인식한 객체의 중심이 정지 상태 기준치 내에 있다면,
서보모터에는 pwm 90을 할당한다.

그 외의 경우에는 또 두 가지 경우의 수로 나뉜다.