Custom mavros message

Create custom mavros receiver and sender

The aim is to send signals to servo connected to pixhawk, using ROS node.

• In mavros_msgs/msg add custom msg file

GripperServo.msg

time frame_stamp		# Timestamp 
float32 servo_setpoint

• Create gripper_servo.cpp in the path mavros_extras/src/plugins/. This is the receiver node that listen user input and publish to pixhawk.

gripper_servo.cpp

#include <mavros/mavros_plugin.h>
#include <mavros_msgs/GripperServo.h>

namespace mavros
{
namespace extra_plugins
{
class GripperServoPlugin : public plugin::PluginBase
{
public:
    GripperServoPlugin() : PluginBase(),
                                     status_nh("~gripper_servo")
    {
    }

    void initialize(UAS &uas_)
    {
        PluginBase::initialize(uas_);

        status_sub = status_nh.subscribe("gripper_servo", 10, &GripperServoPlugin::status_cb, this);
    }

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

private:
    ros::NodeHandle status_nh;
    ros::Subscriber status_sub;

    /**
	 * @param req	received GripperServo msg
	 */
    void status_cb(const mavros_msgs::GripperServo::ConstPtr &req)
    {
        mavlink::common::msg::GRIPPER_SERVO gripper{};

        gripper.servo_setpoint = req->servo_setpoint;
        
        UAS_FCU(m_uas)->send_message_ignore_drop(gripper);
    }
};
} // namespace extra_plugins
} // namespace mavros

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::GripperServoPlugin, mavros::plugin::PluginBase)

• Edit CMakeList.txt at

mavros_msgs/CMakeList.txt

add_message_files(
....
GripperServo.msg
....
)

mavros_extras/CMakeList.txt

add_library(
...
src/plugins/gripper_servo
.cpp
...
)

• Edit mavros_plugins.xml inside mavros_extras/mavros_plugins.xml

mavros_plugins.xml

<class name="gripper_servo" type="mavros::extra_plugins::GripperServoPlugin" base_class_type="mavros::plugin::PluginBase">
  	<description>Send servo controls to FCU.</description>
</class>

• Edit common.xml inside mavlink/message_definitions/v1.0/

common.xml

<message id="229" name="GRIPPER_SERVO">

   <description> Send gripper control generated from off board cobntroller to on board Pixhawk</description>
   <field type="uint64_t" name="time_usec" units="us">Timestamp (synced to UNIX time or since system boot).</field>
   <field type="float" name="servo_setpoint">actuator controls</field>
</message>

Inside px4

• Edit common.xml inside Firmware/mavlink/include/mavlink/v2.0/message_definitions/

common.xml

 <!--Gripper servo control -->
<message id="229" name="GRIPPER_SERVO">
  <description> Send gripper control generated from off board cobntroller to on board Pixhawk</description>
  <field type="uint64_t" name="time_usec" units="us">Timestamp (synced to UNIX time or since system boot).</field>
  <field type="float" name="servo_setpoint">actuator controls</field>
</message>

• Remove common and standard directories from Firmware/mavlink/include/mavlink/v2.0

• Generate standard and common directories using mavlink-generator located in ros pkgmavlink/pymavlink

$ python mavgenerate.py

At XML, "Browse" to [Firmware/mavlink/include/mavlink/v2.0/message_definitions/standard.xml].
At Out, "Browse" to [Firmware/mavlink/include/mavlink/v2.0].
Select Language "C"
Select Protocol "2.0"
Check Validate

• Make uOrb message Firmware/msg/gripper_servo.msg

uint64 timestamp
float32 servo_setpoint

1. Edit CMakeList.txt inside Firmware/msg/

set(
...
     gripper_servo.msg
     )

1. Edit mavlink_receiver.h located at Firmware/src/modules/mavlink/

mavlink_receiver.h

 #include <uORB/topics/gripper_servo.h>
.....
class MavlinkReceiver
{
 	void handle_message_gripper_servo(mavlink_message_t *msg);
......
 	orb_advert_t _gripper_servo_pub{nullptr};
}

1. Edit mavlink_receiver.cpp located at Firmware/src/modules/mavlink/

MavlinkReceiver::handle_message(mavlink_message_t *msg)
{
  switch (msg->msgid) {

	case MAVLINK_MSG_ID_GRIPPER_SERVO:
		 handle_message_gripper_servo(msg);
         break;
        ....
}

add function

void
MavlinkReceiver::handle_message_gripper_servo(mavlink_message_t *msg)
{
    mavlink_gripper_servo_t man;
    mavlink_msg_gripper_servo_decode(msg, &man);

    struct gripper_servo_s key;
    memset(&key, 0, sizeof(key));

    key.timestamp = hrt_absolute_time();
    key.servo_setpoint = man.servo_setpoint;

    if (_gripper_servo_pub == nullptr) {
        _gripper_servo_pub = orb_advertise(ORB_ID(gripper_servo), &key);

    } else {
        orb_publish(ORB_ID(gripper_servo), _gripper_servo_pub, &key);
    }

}