MAVROS에서 PX4로 사용자 정의 메시지 보내기

저작권: 쿼드(QUAD) 드론연구소 https://smartstore.naver.com/maponarooo

MAVROS

이 예에서는 아래 코드를 사용하여 keyboard_command.cpp ( workspace/src/mavros/mavros_extras/src/plugins ) 라는 이름의 새 MAVROS 플러그인을 생성하는 것부터 시작합니다 .

이 코드는 ROS /mavros/keyboard_command/keyboard_sub주제의 'char' 메시지를 구독 하고 이를 MAVLink 메시지로 보냅니다.

 #include <mavros/mavros_plugin.h>
 #include <pluginlib/class_list_macros.h>
 #include <iostream>
 #include <std_msgs/Char.h>

 namespace mavros {
 namespace extra_plugins{

 class KeyboardCommandPlugin : public plugin::PluginBase {
 public:
     KeyboardCommandPlugin() : PluginBase(),
         nh("~keyboard_command")

    { };

     void initialize(UAS &uas_)
     {
         PluginBase::initialize(uas_);
         keyboard_sub = nh.subscribe("keyboard_sub", 10, &KeyboardCommandPlugin::keyboard_cb, this);
     };

     Subscriptions get_subscriptions()
     {
         return {/* RX disabled */ };
     }

 private:
     ros::NodeHandle nh;
     ros::Subscriber keyboard_sub;

    void keyboard_cb(const std_msgs::Char::ConstPtr &req)
     {
         std::cout << "Got Char : " << req->data <<  std::endl;
         mavlink::common::msg::KEY_COMMAND kc {};
         kc.command = req->data;
         UAS_FCU(m_uas)->send_message_ignore_drop(kc);
     }
 };
 }   // namespace extra_plugins
 }   // namespace mavros

PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::KeyboardCommandPlugin, mavros::plugin::PluginBase)

mavros_plugins.xml ( workspace/src/mavros/mavros_extras)을 편집 하고 다음 줄을 추가합니다.

<class name="keyboard_command" type="mavros::extra_plugins::KeyboardCommandPlugin" base_class_type="mavros::plugin::PluginBase">
     <description>Accepts keyboard command.</description>
</class>

CMakeLists.txt ( workspace/src/mavros/mavros_extras )를 편집하고 add_library.
```
add_library( 
...
  src/plugins/keyboard_command.cpp 
)
```

( workspace/src/mavlink/message_definitions/v1.0 )의 common.xml 내 에서 다음 줄을 복사하여 MAVLink 메시지를 추가하세요:

...
  <message id="229" name="KEY_COMMAND">
     <description>Keyboard char command.</description>
     <field type="char" name="command"> </field>
   </message>
...

PX4 변경 사항

common.xml 내부 ( PX4-Autopilot/src/modules/mavlink/mavlink/message_definitions/v1.0 )에 다음과 같이 MAVLink 메시지를 추가합니다(위의 MAVROS 섹션과 동일한 절차):
```
...
  <message id="229" name="KEY_COMMAND">
     <description>Keyboard char command.</description>
     <field type="char" name="command"> </field>
   </message>
...
```
경고
다음 디렉터리의 common.xml 파일이 정확히 동일한 지 확인하세요 .
- PX4-Autopilot/src/modules/mavlink/mavlink/message_definitions/v1.0
- workspace/src/mavlink/message_definitions/v1.0 정확히 동일
(PX4-Autopilot/msg)에서 자신만의 uORB 메시지 파일 key_command.msg를 만드세요 . 이 예의 경우 "key_command.msg"에는 다음 코드만 있습니다.
```
uint64 timestamp # time since system start (microseconds)
char cmd
```
그런 다음 CMakeLists.txt ( PX4-Autopilot/msg )에 다음을 포함합니다.
```
set(
...
     key_command.msg
     )
```

mavlink_receiver.h 편집 ( PX4-Autopilot/src/modules/mavlink 에 있음 )

...
#include <uORB/topics/key_command.h>
...
class MavlinkReceiver
{
...
private:
    void handle_message_key_command(mavlink_message_t *msg);
...
    orb_advert_t _key_command_pub{nullptr};
}

mavlink_receiver.cpp를 편집하십시오 ( PX4-Autopilot/src/modules/mavlink 에 있음 ). 이곳은 PX4가 ROS로부터 전송된 MAVLink 메시지를 수신하고 이를 uORB 주제로 게시하는 곳입니다.

...
void MavlinkReceiver::handle_message(mavlink_message_t *msg)
{
...
 case MAVLINK_MSG_ID_KEY_COMMAND:
        handle_message_key_command(msg);
        break;
...
}
...
void
MavlinkReceiver::handle_message_key_command(mavlink_message_t *msg)
{
    mavlink_key_command_t man;
    mavlink_msg_key_command_decode(msg, &man);

struct key_command_s key = {};

    key.timestamp = hrt_absolute_time();
    key.cmd = man.command;

    if (_key_command_pub == nullptr) {
        _key_command_pub = orb_advertise(ORB_ID(key_command), &key);

    } else {
        orb_publish(ORB_ID(key_command), _key_command_pub, &key);
    }
}

다른 구독자 모듈과 마찬가지로 자신만의 uORB 주제 구독자를 만드세요. 이 예에서는 (/PX4-Autopilot/src/modules/key_receiver)에 모델을 생성해 보겠습니다. 이 디렉터리에 CMakeLists.txt , key_receiver.cpp , Kconfig 세 개의 파일을 만듭니다 . 각각은 다음과 같습니다.

-CMakeLists.txt

px4_add_module(
    MODULE modules__key_receiver
    MAIN key_receiver
    STACK_MAIN 2500
    STACK_MAX 4000
    SRCS
        key_receiver.cpp
    DEPENDS

    )

-key_receiver.cpp

#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <math.h>

#include <uORB/uORB.h>
#include <uORB/topics/key_command.h>

extern "C" __EXPORT int key_receiver_main(int argc, char **argv);

int key_receiver_main(int argc, char **argv)
{
    int key_sub_fd = orb_subscribe(ORB_ID(key_command));
    orb_set_interval(key_sub_fd, 200); // limit the update rate to 200ms

    px4_pollfd_struct_t fds[] = {
        { .fd = key_sub_fd,   .events = POLLIN },
    };

    int error_counter = 0;

    for (int i = 0; i < 10; i++)
    {
        int poll_ret = px4_poll(fds, 1, 1000);

        if (poll_ret == 0)
        {
            PX4_ERR("Got no data within a second");
        }

        else if (poll_ret < 0)
        {
            if (error_counter < 10 || error_counter % 50 == 0)
            {
                PX4_ERR("ERROR return value from poll(): %d", poll_ret);
            }

            error_counter++;
        }

        else
        {
            if (fds[0].revents & POLLIN)
            {
                struct key_command_s input;
                orb_copy(ORB_ID(key_command), key_sub_fd, &input);
                PX4_INFO("Received Char : %c", input.cmd);
             }
        }
    }
    return 0;
}

-Kconfig

 menuconfig MODULES_KEY_RECEIVER
 bool "key_receiver"
 default n
 ---help---
 	Enable support for key_receiver

마지막으로, PX4-Autopilot/boards/ 의 보드에 해당하는 default.px4board 파일 에 모듈을 추가하세요 . 예: - Pixhawk4의 경우 PX4-Autopilot/boards/px4/fmu-v5/default.px4board 에 다음 코드를 추가합니다 . - SITL의 경우 PX4-Autopilot/boards/px4/sitl/ 에 다음 코드를 추가합니다. default.px4board
```
 CONFIG_MODULES_KEY_RECEIVER=y
```

이제 모든 작업을 빌드할 준비가 되었습니다!

빌드

ROS용 빌드

작업공간에서 다음을 실행하십시오: catkin build.
사전에 (/workspace/src/mavros/mavros/launch)에 "px4.launch"를 설정해야 합니다. "px4.launch"를 아래와 같이 편집하세요. USB를 사용하여 컴퓨터를 Pixhawk와 연결하는 경우 아래와 같이 "fcu_url"을 설정해야 합니다. 그러나 CP2102를 사용하여 컴퓨터를 Pixhawk와 연결하는 경우 "ttyACM0"을 "ttyUSB0"으로 바꿔야 합니다. 그리고 SITL을 사용하여 터미널에 연결하는 경우 "/dev/ttyACM0:57600"을 "udp://:14540@127.0.0.1:14557"로 바꿔야 합니다. "gcs_url"을 수정하는 것은 Pixhawk를 UDP로 연결하는 것입니다. 직렬 통신은 MAVROS와 연결을 동시에 허용할 수 없기 때문입니다.

"xxx.xx.xxx.xxx"에 IP 주소를 쓰세요.

...
  <arg name="fcu_url" default="/dev/ttyUSB0:57600" />
  <arg name="gcs_url" default="udp://:14550@xxx.xx.xxx.xxx:14557" />
...

PX4용으로 빌드

이전에 빌드된 PX4-Autopilot 디렉터리를 정리합니다. PX4-Autopilot 디렉토리 루트에서 :
```
make clean
```
PX4-Autopilot을 빌드하고 일반적인 방법으로 업로드하세요 .
예를 들어:
- Pixhawk 4/FMUv5용으로 빌드하려면 PX4-Autopilot 디렉터리 루트에서 다음 명령을 실행하세요:
```
make px4_fmu-v5_default upload
```
- SITL용으로 빌드하려면 PX4-Autopilot 디렉토리의 루트에서 다음 명령을 실행하십시오(jmavsim 시뮬레이션 사용):
```
make px4_sitl jmavsim
```

코드 실행

다음으로 MAVROS 메시지가 PX4로 전송되는지 테스트합니다.

ROS 실행

터미널에서 입력
```
roslaunch mavros px4.launch
```
두 번째 터미널에서 다음을 실행합니다.
```
rostopic pub -r 10 /mavros/keyboard_command/keyboard_sub std_msgs/Char 97
```
이는 메시지 유형 "std_msgs/Char"의 ROS 주제 "/mavros/keyboard_command/keyboard_sub"에 97(ASCII의 'a')을 게시한다는 의미입니다. "-r 10"은 "10Hz"로 지속적으로 게시한다는 의미입니다.

PX4 실행

UDP를 통해 Pixhawk에 접속 (xxx.xx.xxx.xxx를 PX4가 실행되고 있는 실제 IP로 변경)
```
cd PX4-Autopilot/Tools
./mavlink_shell.py xxx.xx.xxx.xxx:14557 --baudrate 57600
```
몇 초 후에 Enter 키를 두 번 누릅니다. 터미널에 아래와 같은 프롬프트가 표시됩니다.
```
nsh>
nsh>
```
구독자 모듈을 실행하려면 "key_receiver"를 입력하세요.
```
nsh> key_receiver
```

ROS 토픽으로부터 성공적으로 메시지가 수신되는지 확인하세요.

PreviousMAVROS 오프보드 제어 예제(C++)Next문제 해결

Last updated 1 year ago