Turtlebot3의 teleoperation 커멘드를 통해 발행되는 topic 구독

Jeong · April 21, 2025, 5:54am

/*******************************************************************************

Definition for TurtleBot3Core ‘run()’ function
*******************************************************************************/
void TurtleBot3Core::run()
{
static uint32_t pre_time_to_control_motor; //모터 제어 주기 관리용 타임스탬프 저장 static 변수, 한번 선언되면 함수가 여러번 호출되어도 값 유지됨

// Check connection state with ROS2 node
update_connection_state_with_ros2_node(); // ROS2 노드와 연결 상태를 확인하고 control_item.is_connect_ros2_node 갱신

/* For diagnosis */
// diagnosis는 시스템의 상태를 점검하기 위한 노드이다
// Show LED status
diagnosis.showLedStatus(get_connection_state_with_ros2_node()); // get_connection_state_with_ros2_node는 ROS2 노드와 연결 상태 반환함, 즉 연결이 끊기면 경고용 LED 패턴 표시
// Update Voltage
diagnosis.updateVoltageCheck(true);
// Check push button pressed for simple test drive
test_motors_with_buttons(diagnosis.getButtonPress(3000)); // 사용자가 버튼을 3초 눌렀을 때, 테스트용으로 모터 작동 함수

/* For sensing and run buzzer */
// Update the IMU unit
sensors.updateIMU();
// Update sonar data
// TODO: sensors.updateSonar(t);
// Run buzzer if there is still melody to play.
sensors.onMelody();

/* For getting command from rc100 */
// Receive data from RC100
controllers.getRCdata(goal_velocity_from_rc100); // RC100 리모컨에서 받은 데이터를 goal_velocity_from_rc100 배열에 저장

/* For processing DYNAMIXEL slave function */
// Update control table of OpenCR to communicate with ROS2 node
update_imu(INTERVAL_MS_TO_UPDATE_CONTROL_ITEM); // control_item의 imu 관련 값들 업데이트 함수
update_times(INTERVAL_MS_TO_UPDATE_CONTROL_ITEM); // control_item의 time 관련 값 업데이트
update_gpios(INTERVAL_MS_TO_UPDATE_CONTROL_ITEM); // gpios 관련 값 업데이트
update_motor_status(INTERVAL_MS_TO_UPDATE_CONTROL_ITEM); // control_item의 모터 상태 값 업데이트 함수
update_battery_status(INTERVAL_MS_TO_UPDATE_CONTROL_ITEM); // control_item의 배터리 상태 값 업데이트 함수
update_analog_sensors(INTERVAL_MS_TO_UPDATE_CONTROL_ITEM); // control_item의 센서 값 업데이트 함수
update_joint_status(INTERVAL_MS_TO_UPDATE_CONTROL_ITEM);

// Packet processing with ROS2 Node.
dxl_slave.processPacket(); // Dynamixel 프로토콜 기반으로 ROS2 노드로부터 명령 패킷을 수신하고, 응답을 보냄(통신 루틴)

/* For controlling DYNAMIXEL motors (Wheels) /
/ 바퀴 모터를 일정 주기로 제어하는 부분 */
if (millis()-pre_time_to_control_motor >= INTERVAL_MS_TO_CONTROL_MOTOR) // 마지막 제어시간으로부터 INTERVAL_MS_TO_CONTROL_MOTOR 만큼 시간이 지났다면 모터 제어 수행
{
pre_time_to_control_motor = millis(); // 제어 시간 업데이트
if(get_connection_state_with_ros2_node() == false){
memset(goal_velocity_from_cmd, 0, sizeof(goal_velocity_from_cmd)); // ROS2 노드와 연결이 끊기면 모터 명령 0, memset 함수 이용
}
update_goal_velocity_from_3values(); // RC100, 버튼, ROS2 에서 받은 커멘드 명령을 합쳐서 최종 목표 속도 계산하는 함수
if(get_connection_state_with_motors() == true){
// motor_driver 객체의 control_motors 함수로 모터 제어
motor_driver.control_motors(p_tb3_model_info->wheel_separation, goal_velocity[VelocityType::LINEAR], goal_velocity[VelocityType::ANGULAR]);
}
}
}

/*******************************************************************************

Function definition for updating velocity values
to be used for control of DYNAMIXEL(motors).
*******************************************************************************/
// RC100, 버튼, ROS2 에서 받은 커멘드 명령을 합쳐서 최종 목표 속도 계산하는 함수, run 함수에서 호출되어서 사용됨
void update_goal_velocity_from_3values(void)
{
goal_velocity[VelocityType::LINEAR] = goal_velocity_from_button[VelocityType::LINEAR] + goal_velocity_from_cmd[VelocityType::LINEAR] + goal_velocity_from_rc100[VelocityType::LINEAR];
goal_velocity[VelocityType::ANGULAR] = goal_velocity_from_button[VelocityType::ANGULAR] + goal_velocity_from_cmd[VelocityType::ANGULAR] + goal_velocity_from_rc100[VelocityType::ANGULAR];

sensors.setLedPattern(goal_velocity[VelocityType::LINEAR], goal_velocity[VelocityType::ANGULAR]);
}

turtlebot3 waffle_pi를 사용해서 터틀봇 예제 프로젝트를 진행하고 있는데 OpenCR 펌웨어 코드를 공부하는 과정에서 질문이 생겨 문의 드립니다. 위의 코드는 OpenCR 펌웨어 코드에서 turtlebot.cpp 에서 정의 된 run() 함수와 update_goal_velocity_from_3values() 함수의 코드 부분입니다. 여기서 motor_driver 의 control_motors 함수로 직선,회전 goal_velocity를 넘겨주기 위해서 goal_velocity를 update_goal_velocity_from_3values() 함수에서 계산해주게 되는데

여기서
goal_velocity_from_button는 펌웨어 코드 확인용 코드에서 가져오는 OpenCR의 버튼을 눌렀을 때 생성되는 목표 속도에 대한 값이고,
goal_velocity_from_rc100는 RC100 조이스틱의 커멘드에 의해서 생성되는 목표 속도에 대한 값으로 해석했습니다.
그리고
goal_velocity_from_cmd 를 turtlebot3 패키지 중 teleoperation 노드를 실행했을 때 키보드 입력에 의해 생성되는 커멘드 토픽에 의해 결정되는 목표 속도 값이라고 생각하는데 이때 이 goal_velocity_from_cmd 가 아래의 콜백함수 코드에 의해서 메모리에 저장된다고 해석했습니다.

/*******************************************************************************

Callback function definition to be used in communication with the ROS2 node.
******************************************************************************/
// Dynamixel 프로토콜 기반으로 ROS2 노드에서 보낸 데이터를 처리하는 콜백 함수, begin에서 호출되어서 사용됨
static void dxl_slave_write_callback_func(uint16_t item_addr, uint8_t &dxl_err_code, void arg)
{
(void)arg; // 추가 인자이지만 여기에서는 사용하지 않아서 (void)arg로 무시함

// 선택지가 주어질 때, 그에 대한 선택을 해야하는 경우 switch 문 자주 사용됨
switch(item_addr) // item_addr 은 데이터 주소, 어떤 데이터 항목을 쓸 건지 알려줌
{
case ADDR_MODEL_INFORM:
control_items.model_inform = p_tb3_model_info->model_info;
dxl_err_code = DXL_ERR_ACCESS;
break;

// ROS2 에서 디버그 모드 플래그 값을 변경하려고 데이터 전송 시
case ADDR_DEBUG_MODE:
if (control_items.debug_mode == true)
DEBUG_PRINTLN(“Debug Mode : Enabled”);
else
DEBUG_PRINTLN(“Debug Mode : Disabled”);
break;

// 로봇의 부저 제어하는 명령
case ADDR_SOUND:
sensors.makeMelody(control_items.buzzer_sound); // control_items.buzzer_sound 값을 받아서 부저 울림
break;

// IMU 재보정을 명령
case ADDR_IMU_RECALIBRATION:
if(control_items.imu_recalibration == true){
sensors.calibrationGyro(); // 자이로센서 보정 수행 후,
control_items.imu_recalibration = false; // False로 다시 초기화
}
break;

// 모터 토크(힘) 켜고 끄는 명령
case ADDR_MOTOR_TORQUE:
if(get_connection_state_with_motors() == true)
motor_driver.set_torque(control_items.motor_torque_enable_state); // 모터 토크 Enable/Disable 수행
break;

// ROS2의 teleoperation으로부터 전송된, cmd_vel 의 선속도 값 저장
case ADDR_CMD_VEL_LINEAR_X:
// m/s 단위로 변환 후 최소, 최대 속도 사이의 값이 들어오면 그걸 저장
// dxl_slave가 받은 cmd_vel 값을 자동으로 control_items 메모리에 반영
goal_velocity_from_cmd[VelocityType::LINEAR] = constrain((float)(control_items.cmd_vel_linear[0]*0.01f), min_linear_velocity, max_linear_velocity);
break;

// ROS2의 teleoperation으로부터 전송된, cmd_vel 의 회전속도 값 저장
case ADDR_CMD_VEL_ANGULAR_Z:
// 단위 변환 및 속도 제한 적용
goal_velocity_from_cmd[VelocityType::ANGULAR] = constrain((float)(control_items.cmd_vel_angular[2]*0.01f), min_angular_velocity, max_angular_velocity);
break;

case ADDR_PROFILE_ACC_L:
case ADDR_PROFILE_ACC_R:
if(get_connection_state_with_motors() == true)
// dxl_slave가 받은 값을 자동으로 control_items 메모리에 반영
motor_driver.write_profile_acceleration(control_items.profile_acceleration[MortorLocation::LEFT], control_items.profile_acceleration[MortorLocation::RIGHT]);
break;

이 코드에서 goal_velocity_from_cmd의 Linear(x 방향 선속도)와 Angular(z축 회전속도) 값을 결정하게 되는데 이걸 결정하기 위해서 각각 control_items의 cmd_vel_linear[0] 과 cmd_vel_angular[2] 를 불러옵니다.

여기서 제가 궁금한 것은 이 cmd_vel_linear[0] 과 cmd_vel_angular[2]의 값이 어디서 결정되어서 저장되는지 알고 싶습니다. 제 생각에는 turtlebot3의 teleoperation에 의해 생성되는 cmd_vel/Twist 토픽을 구독해서 값이 결정되는 것 같은데 그 토픽을 구독하는 부분을 코드에서 찾을수가 없었습니다.

turtlebot3의 teleoperation에 의해 토픽이 생성되는 과정과 그걸 구독해서 목표 속도를 결정하는 과정에 대해서 코드적인 부분에서 알고 싶습니다.