AHRS x1 ROS1 드라이버 코드 수정 질문
페이지 정보
본문
사용 환경: ROS1
개발 언어: C++
#include "listener.h"
void *AHRS_thread(void *arg)
{
while (run)
{
if (MW_SerialRecv(&id, &length, data))
{
switch ((int)(unsigned char)data[1])
{
case ACC:
// acc_x = ((int)(unsigned char)data[2] | (int)(unsigned char)data[3] << 8);
// acc_y = ((int)(unsigned char)data[4] | (int)(unsigned char)data[5] << 8);
// acc_z = ((int)(unsigned char)data[6] | (int)(unsigned char)data[7] << 8);
// imu.linear_acceleration.x = acc_x / 1000.0 * 9.8;
// imu.linear_acceleration.y = acc_y / 1000.0 * 9.8;
// imu.linear_acceleration.z = acc_z / 1000.0 * 9.8;
// break;
// 데이터 변환 (16비트 정수로 변환 후 1000.0으로 나눔)
acc_x = (int16_t)((data[2]) | (data[3] << 8)) / 1000.0;
acc_y = (int16_t)((data[4]) | (data[5] << 8)) / 1000.0;
acc_z = (int16_t)((data[6]) | (data[7] << 8)) / 1000.0;
// IMU 메시지에 값 적용 (convertor_g2a를 곱하여 변환)
imu.linear_acceleration.x = acc_x * convertor_g2a;
imu.linear_acceleration.y = acc_y * convertor_g2a;
imu.linear_acceleration.z = acc_z * convertor_g2a;
break;
case GYO:
// gyo_x = ((int)(unsigned char)data[2] | (int)(unsigned char)data[3] << 8);
// gyo_y = ((int)(unsigned char)data[4] | (int)(unsigned char)data[5] << 8);
// gyo_z = ((int)(unsigned char)data[6] | (int)(unsigned char)data[7] << 8);
// imu.angular_velocity.x = gyo_x / 10.0 * 0.01745;
// imu.angular_velocity.y = gyo_y / 10.0 * 0.01745;
// imu.angular_velocity.z = gyo_z / 10.0 * 0.01745;
// break;
// 데이터 변환 (16비트 정수로 변환 후 10.0으로 나눔)
gyo_x = (int16_t)((data[2]) | (data[3] << 8)) / 10.0;
gyo_y = (int16_t)((data[4]) | (data[5] << 8)) / 10.0;
gyo_z = (int16_t)((data[6]) | (data[7] << 8)) / 10.0;
// IMU 메시지에 값 적용 (단위 변환: deg/s → rad/s)
imu.angular_velocity.x = gyo_x * convertor_d2r;
imu.angular_velocity.y = gyo_y * convertor_d2r;
imu.angular_velocity.z = gyo_z * convertor_d2r;
break;
case DEG:
// deg_x = ((int)(unsigned char)data[2] | (int)(unsigned char)data[3] << 8);
// deg_y = ((int)(unsigned char)data[4] | (int)(unsigned char)data[5] << 8);
// deg_z = ((int)(unsigned char)data[6] | (int)(unsigned char)data[7] << 8);
// float x = deg_x / 100.0;
// float y = deg_y / 100.0;
// float z = deg_z / 100.0;
// imu.orientation.w = (COS(z) * COS(y) * COS(x)) + (SIN(z) * SIN(y) * SIN(x));
// imu.orientation.x = (COS(z) * COS(y) * SIN(x)) - (SIN(z) * SIN(y) * COS(x));
// imu.orientation.y = (COS(z) * SIN(y) * COS(x)) + (SIN(z) * COS(y) * SIN(x));
// imu.orientation.z = (SIN(z) * COS(y) * COS(x)) - (COS(z) * SIN(y) * SIN(x));
deg_x = (int16_t)(((int)(unsigned char)data[2] | (int)(unsigned char)data[3] << 8)) / 100.0;
deg_y = (int16_t)(((int)(unsigned char)data[4] | (int)(unsigned char)data[5] << 8)) / 100.0;
deg_z = (int16_t)(((int)(unsigned char)data[6] | (int)(unsigned char)data[7] << 8)) / 100.0;
roll = deg_x * convertor_d2r;
pitch = mag_y * convertor_d2r;
yaw = deg_z * convertor_d2r;
tf_orientation = Euler2Quaternion(roll, pitch, yaw);
imu_yaw_data = deg_z;
imu.orientation.x = tf_orientation.x();
imu.orientation.y = tf_orientation.y();
imu.orientation.z = tf_orientation.z();
imu.orientation.w = tf_orientation.w();
break;
case MAG:
mag_x = (int16_t)(((int)(unsigned char)data[2] | (int)(unsigned char)data[3] << 8)) / 10.0;
mag_y = (int16_t)(((int)(unsigned char)data[4] | (int)(unsigned char)data[5] << 8)) / 10.0;
mag_z = (int16_t)(((int)(unsigned char)data[6] | (int)(unsigned char)data[7] << 8)) / 10.0;
// MagneticField 메시지에 데이터 저장
mag_msg.magnetic_field.x = mag_x / convertor_ut2t;
mag_msg.magnetic_field.y = mag_y / convertor_ut2t;
mag_msg.magnetic_field.z = mag_z / convertor_ut2t;
break;
}
}
}
}
tf2::Quaternion Euler2Quaternion(float roll, float pitch, float yaw)
{
float qx = (sin(roll / 2) * cos(pitch / 2) * cos(yaw / 2)) -
(cos(roll / 2) * sin(pitch / 2) * sin(yaw / 2));
float qy = (cos(roll / 2) * sin(pitch / 2) * cos(yaw / 2)) +
(sin(roll / 2) * cos(pitch / 2) * sin(yaw / 2));
float qz = (cos(roll / 2) * cos(pitch / 2) * sin(yaw / 2)) -
(sin(roll / 2) * sin(pitch / 2) * cos(yaw / 2));
float qw = (cos(roll / 2) * cos(pitch / 2) * cos(yaw / 2)) +
(sin(roll / 2) * sin(pitch / 2) * sin(yaw / 2));
tf2::Quaternion q(qx, qy, qz, qw);
return q;
}
int main(int argc, char **argv)
{
pthread_t thread;
ros::init(argc, argv, "mw_ahrs");
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<sensor_msgs::Imu>("imu", 10);
ros::Publisher mag_pub = n.advertise<sensor_msgs::MagneticField>("magnetic_field", 10);
MW_SerialOpen("/dev/ttyUSB0", 115200);
pthread_create(&thread, NULL, AHRS_thread, NULL);
imu.orientation_covariance = {0.0025, 0, 0, 0, 0.0025, 0, 0, 0, 0.0025};
imu.angular_velocity_covariance = {0.02, 0, 0, 0, 0.02, 0, 0, 0, 0.02};
imu.linear_acceleration_covariance = {0.04, 0, 0, 0, 0.04, 0, 0, 0, 0.04};
imu.linear_acceleration.x = 0;
imu.linear_acceleration.y = 0;
imu.linear_acceleration.z = 0;
imu.angular_velocity.x = 0;
imu.angular_velocity.y = 0;
imu.angular_velocity.z = 0;
imu.orientation.w = 0;
imu.orientation.x = 0;
imu.orientation.y = 0;
imu.orientation.z = 0;
mag_msg.magnetic_field.x = 0.0;
mag_msg.magnetic_field.y = 0.0;
mag_msg.magnetic_field.z = 0.0;
ros::Rate rate(1000);
while (ros::ok())
{
imu.header.frame_id = "imu_link";
imu.header.stamp = ros::Time::now();
chatter_pub.publish(imu);
mag_pub.publish(mag_msg); // 자기장 메세지
ros::spinOnce();
rate.sleep();
}
run = CLOCK_THREAD_CPUTIME_ID;
pthread_join(thread,NULL);
return 0;
}
listener.cpp를 ROS2 드라이버의 포멧형식에 맞게 수정해서 imu토픽 테스트 중인데
센서 값이 전체적으로 업데이트 속도가 느리고, 가속도 값이 변환이 잘 안되는 것 같네요..
추가로 수정해야 될 사항이나 잘못된 부분이 있나요?
전체 코드는 파일로 첨부했습니다.
첨부파일
-
ROS1 x1동작테스트.mkv (4.1M)
3회 다운로드 | DATE : 2025-02-13 14:13:09
- 이전글x1 ROS1 드라이버 개발 코드 검토 문의 25.02.19
- 다음글AHRS x1 우분투 및 ROS1 동작 지원 여부 25.02.07
댓글목록
최고관리자님의 댓글
최고관리자 작성일
안녕하세요.
아이디어 로봇입니다.
해당 문제의 경우 우선 문제가 어떻게 발생되는지 자세하게 파악이 필요할 것 같습니다.
정확한 원인과 문제 파악을 위해 속도 지연 및 가속도 문제가 발생할 때의 로그와 값을 영상으로 공유하여 주실 수 있으신가요?
로아스john님의 댓글의 댓글
로아스john 작성일
동작 테스트 영상 첨부하였고
처음엔 센서 기준 x축으로 앞뒤로 기울였고, y축으로 양옆으로 기울였고, z축으로 회전했습니다
지속적으로 센서를 움직이고있는데 데이터 출력 지연이 발생하고, orientation x, y, z, w(쿼터니언)이 축 회전 방향으로 값이 정상적으로 안들어오네요
예를 들어 x축으로 기울였을때
orientation:
x: 0.01579766906797886 <- 값이 변동 하지 않습니다.
또한 쿼터니언 범위도 -1.0 ~ 1.0까지 출력되지도 않네요.
로아스john님의 댓글의 댓글
로아스john 작성일지금 다시 기존 ROS 1 드라이버 (수정하기전) 코드로 rivz 테스트 중인데 데이터 지연만 조금 있는것 같고, 값은 맞게 들어오는 것 같습니다.
최고관리자님의 댓글
최고관리자 작성일
영상을 확인해본 결과 imu 토픽에서 angular_velocity값이 먼저 변하고 몇 초 뒤 orientation 값이 변하는 현상이 발생하는 것으로 보입니다.
AHRS_thread 내에 연산에서 지연이 나타나면 orientation을 계산하기 위한 값을 받지 못하기 때문에 이러한 현상이 발생할 수 있을 것 같습니다.
먼저 case DEG:에서 pitch 계산 부분에 오타로 mag 값을 가져오는 부분이 있습니다.
roll = deg_x * convertor_d2r;
pitch = mag_y * convertor_d2r;
yaw = deg_z * convertor_d2r;
잘못된 값이 들어와 지연이 발생하였을 가능성이 있습니다.
오타의 문제가 아니라면 AHRS_thread에서 ACC, GYO, DEG, MAG 계산에서 각각 지연이 얼마나 되는지 디버그 코드를 추가하시면 어느 부분에서 지연이 발생하는지 확인하실 수 있으실 것 같습니다.
그리고 AHRS_thread에서 기존 코드와는 다르게 convertor_g2a와 같은 변수를 외부에서 가져오는 부분이 있는 것으로 확인됩니다.
thread 내에서 외부 변수를 가져오는 과정에서 지연이 발생할 가능성이 있기 때문에 외부 변수 부분을 제거하고 지연이 발생하는지 확인하여 주세요.
또 다른 가능성으로는 ros::Rate rate(1000); 코드에서 초당 1000번으로 실행되는 부분이 있습니다.
너무 과한 rate는 네트워크 부하로 인한 지연을 발생시킬 수도 있습니다.
로아스john님의 댓글의 댓글
로아스john 작성일
답변 감사합니다.
현재 ROS1 드라이버 패키지 수정 및 테스트중에 /imu 토픽 값이 모두 0으로 나오는 증상이 발생하여
ROS1 드라이버 동작 커밋 부분으로 롤백 해서 실행했는데도 안되네요.
여분의 x1 센서 1개로 테스트 해보고, 다른 우분투에서도 테스트 해보니 ROS1에서 /imu 토픽값이 0이 나옵니다.
윈도우나 ROS2는 잘 작동합니다. 윈도우에서도 경우도 Comport를 잘못잡아서 3~4번 재연결한적이 몇 번정도 있었습니다.
혹시 해결방안이 있는지 여쭙고 싶습니다.
최고관리자님의 댓글의 댓글
최고관리자 작성일/imu 토픽 값이 모두 0으로 나오는 증상이 처음 발생하였을 때 수정하신 부분이 어디일까요?