MW-AHRS X1 raspberrypi4 ros2 foxy문의 > Module_Forum

• 목록

구매한 Sensor: MW-AHRS X1

사용 환경: ros2 foxy, raspberrypi4 8gb, ubuntu server 20.04

개발 언어: python

ubuntu@raspberrypi:~$ colcon build
[1.332s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/ubuntu/install/stella_ahrs' in the environment variable AMENT_PREFIX_PATH doesn't exist
[1.333s] WARNING:colcon.colcon_ros.prefix_path.catkin:The path '/home/ubuntu/install/stella_ahrs' in the environment variable CMAKE_PREFIX_PATH doesn't exist
Starting >>> stella_ahrs
--- stderr: stella_ahrs
/home/ubuntu/2th_NtrexAHRS_lib_ROS/ros2_example/stella_ahrs/serial/list_ports_linux.cc: In function ‘std::vector<std::__cxx11::basic_string<char> > glob(const std::vector<std::__cxx11::basic_string<char> >&)’:
/home/ubuntu/2th_NtrexAHRS_lib_ROS/ros2_example/stella_ahrs/serial/list_ports_linux.cc:66:40: warning: comparison of integer expressions of different signedness: ‘int’ and ‘size_t’ {aka ‘long unsigned in’} [-Wsign-compare]
  66 |    for(int path_index = 0; path_index < glob_results.gl_pathc; path_index++)
      |                            ~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~
/home/ubuntu/2th_NtrexAHRS_lib_ROS/ros2_example/stella_ahrs/serial/list_ports_linux.cc:57:9: warning:variable ‘glob_retval’ set but not used [-Wunused-but-set-variable]
  57 |    int glob_retval = glob(patterns[0].c_str(), 0, NULL, &glob_results);
      |        ^~~~~~~~~~~
/home/ubuntu/2th_NtrexAHRS_lib_ROS/ros2_example/stella_ahrs/serial/list_ports_linux.cc: In function ‘std::string format(const char*, ...)’:
/home/ubuntu/2th_NtrexAHRS_lib_ROS/ros2_example/stella_ahrs/serial/list_ports_linux.cc:246:31: warning: comparison of integer expressions of different signedness: ‘int’ and ‘size_t’ {aka ‘long unsigned int’} [-Wsign-compare]
  246 |        else if( return_value >= buffer_size_bytes )
      |                  ~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~
In file included from /home/ubuntu/2th_NtrexAHRS_lib_ROS/ros2_example/stella_ahrs/src/listener.cpp:1:
/home/ubuntu/2th_NtrexAHRS_lib_ROS/ros2_example/stella_ahrs/include/mw/mw_ahrs.hpp:41:13: warning: ‘run_bool’ defined but not used [-Wunused-variable]
  41 | static bool run_bool = false;
      |            ^~~~~~~~
/home/ubuntu/2th_NtrexAHRS_lib_ROS/ros2_example/stella_ahrs/include/mw/mw_ahrs.hpp:38:14: warning: ‘mag_value’ defined but not used [-Wunused-variable]
  38 |              mag_value[3] = {
      |              ^~~~~~~~~
/home/ubuntu/2th_NtrexAHRS_lib_ROS/ros2_example/stella_ahrs/include/mw/mw_ahrs.hpp:35:14: warning: ‘deg_value’ defined but not used [-Wunused-variable]
  35 |              deg_value[3] = {
      |              ^~~~~~~~~
/home/ubuntu/2th_NtrexAHRS_lib_ROS/ros2_example/stella_ahrs/include/mw/mw_ahrs.hpp:32:14: warning: ‘gyr_value’ defined but not used [-Wunused-variable]
  32 |              gyr_value[3] = {
      |              ^~~~~~~~~
/home/ubuntu/2th_NtrexAHRS_lib_ROS/ros2_example/stella_ahrs/include/mw/mw_ahrs.hpp:29:14: warning: ‘acc_value’ defined but not used [-Wunused-variable]
  29 | static float acc_value[3] = {
      |              ^~~~~~~~~
/home/ubuntu/2th_NtrexAHRS_lib_ROS/ros2_example/stella_ahrs/mw/mw_ahrs.cpp: In static member function ‘static void* ntrex::MwAhrsRosDriver::MwAhrsRead(void*)’:
/home/ubuntu/2th_NtrexAHRS_lib_ROS/ros2_example/stella_ahrs/mw/mw_ahrs.cpp:48:3: warning: no return statement in function returning non-void [-Wreturn-type]
  48 |  }
      |  ^
/home/ubuntu/2th_NtrexAHRS_lib_ROS/ros2_example/stella_ahrs/mw/mw_ahrs.cpp:12:43: warning: unused parameter ‘arg’ [-Wunused-parameter]
  12 |  void *MwAhrsRosDriver::MwAhrsRead(void *arg)
      |                                    ~~~~~~^~~
/usr/bin/ld: CMakeFiles/stella_ahrs_node.dir/mw/mw_ahrs.cpp.o: in function `ntrex::MwAhrsRosDriver::MwAhrsRosDriver(char const*, int, int)':
mw_ahrs.cpp:(.text+0x660): undefined reference to `MW_AHRS_Serial_Connect(char const*, unsigned int, unsigned int)'
collect2: error: ld returned 1 exit status
make[2]: *** [CMakeFiles/stella_ahrs_node.dir/build.make:232: stella_ahrs_node] Error 1
make[1]: *** [CMakeFiles/Makefile2:78: CMakeFiles/stella_ahrs_node.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
---
Failed  <<< stella_ahrs [25.5s, exited with code 2]

Summary: 0 packages finished [26.4s]
  1 package failed: stella_ahrs
  1 package had stderr output: stella_ahrs

https://github.com/ntrexlab/2th_NtrexAHRS_lib_ROS/tree/main 해당 깃을 바탕으로 빌드를 시도하는데 되지 않습니다. 깃 첨부파일의 MW_AHRS_arm64.a 로 변경하여 진행하였으나 완료되지 않습니다.
그래서 직접 pyserial로 코드를 만들어 값을 읽으려 하는데 도움을 요청드립니다.
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
import serial
import time

class ImuPublisher(Node):
    def __init__(self):
        super().__init__('imu_publisher')
        self.publisher_ = self.create_publisher(Imu, 'imu_data', 10)
        self.timer = self.create_timer(0.1, self.timer_callback)  # 0.1초마다 콜백 호출
        self.ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=1)  # IMU Serial 포트 설정

    def request_data(self):
        # IMU에 요청 패킷 보내기
        request_packet = bytearray([0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])  # Command와 Index 설정
        self.ser.write(request_packet)

    def parse_response(self, raw_data):
        try:
            response = raw_data.decode('utf-8').strip().split()
            if len(response) < 6:
                self.get_logger().warning("응답 데이터가 충분하지 않습니다: %s", response)
                return None, None

            # 가속도와 각속도 데이터 추출
            acceleration = (float(response[0]), float(response[1]), float(response[2]))
            angular_velocity = (float(response[3]), float(response[4]), float(response[5]))
            return acceleration, angular_velocity
        except (IndexError, ValueError) as e:
            self.get_logger().error("응답 데이터를 파싱하는 데 실패했습니다: %s", e)
            return None, None

    def timer_callback(self):
        self.request_data()  # IMU에 데이터 요청
        time.sleep(0.1)  # 요청 후 약간의 대기

        # IMU로부터 응답 수신
        raw_response = self.ser.readline()  # 한 줄 읽기
        self.get_logger().info("Raw IMU Response: %s" % raw_response.decode('utf-8'))

        # 빈 응답 처리
        if raw_response in [b'\x00\n', b'\n', b'\r\n', b'']:
            self.get_logger().warning("유효하지 않은 응답: %s" % raw_response.decode('utf-8'))
            return

        # 응답 파싱 및 토픽 게시
        acceleration, angular_velocity = self.parse_response(raw_response)
        if acceleration and angular_velocity:
            imu_msg = Imu()
            imu_msg.linear_acceleration.x = acceleration[0]
            imu_msg.linear_acceleration.y = acceleration[1]
            imu_msg.linear_acceleration.z = acceleration[2]
            imu_msg.angular_velocity.x = angular_velocity[0]
            imu_msg.angular_velocity.y = angular_velocity[1]
            imu_msg.angular_velocity.z = angular_velocity[2]
            self.publisher_.publish(imu_msg)
            self.get_logger().info("IMU Data Published: %s" % imu_msg)

    def destroy_node(self):
        self.ser.close()  # Serial 포트 닫기
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    imu_publisher = ImuPublisher()
   
    try:
        rclpy.spin(imu_publisher)
    except KeyboardInterrupt:
        pass
    finally:
        imu_publisher.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

파이썬으로 다음과 같이 간단한 파이씨리얼과 토픽예제를 만들었는데 메뉴얼의 내용대로 값을 요청해도 받지 못합니다.
usb, rs232통신으로 값을 받고자 하는데
 request_packet = bytearray([0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])  # Command와 Index 설정
        self.ser.write(request_packet)

패킷을 다음과 같이 정하여 보내면 응답이 와야하는데 오지 않습니다.

제공해주시는 git이 정상적으로 동작하거나 제가 구현하고자 하는 pyserial이 동작해야 합니다.