Skip to content

Commit

Permalink
Create play_tune plugin
Browse files Browse the repository at this point in the history
play_tune: Use tune_id as parameter.

play_tune: Run clang-format

play_tune: Fix buggy conditional

The check `!x > 0` evaluates as `(!x) > 0`, but this was probably intended to be
`!(x > 0)`.

However:

The tune id is unsigned, and tune id 0 means custom tune, which we don't
support, so we can simplify this to just issue an error if tune id is zero.

Check out tune_definition.desc in the PX4 Firmware repo.

Fix mavlink#3.
  • Loading branch information
mortenfyhn committed Jan 28, 2020
1 parent a1dd03c commit 55bd190
Show file tree
Hide file tree
Showing 3 changed files with 59 additions and 0 deletions.
1 change: 1 addition & 0 deletions mavros/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -139,6 +139,7 @@ add_library(mavros_plugins
src/plugins/vfr_hud.cpp
src/plugins/waypoint.cpp
src/plugins/wind_estimation.cpp
src/plugins/play_tune.cpp
)
add_dependencies(mavros_plugins
mavros
Expand Down
3 changes: 3 additions & 0 deletions mavros/mavros_plugins.xml
Original file line number Diff line number Diff line change
Expand Up @@ -77,4 +77,7 @@
<class name="wind_estimation" type="mavros::std_plugins::WindEstimationPlugin" base_class_type="mavros::plugin::PluginBase">
<description>Publish wind estimates</description>
</class>
<class name="play_tune" type="mavros::std_plugins::PlayTunePlugin" base_class_type="mavros::plugin::PluginBase">
<description>Play a tune</description>
</class>
</library>
55 changes: 55 additions & 0 deletions mavros/src/plugins/play_tune.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,55 @@
#include <mavros/mavros_plugin.h>
#include <std_msgs/UInt8.h>

namespace mavros
{
namespace std_plugins
{

class PlayTunePlugin : public plugin::PluginBase
{
public:
PlayTunePlugin() : PluginBase(), nh("~") {}

void initialize(UAS& uas_)
{
PluginBase::initialize(uas_);
sub = nh.subscribe("play_tune", 1, &PlayTunePlugin::callback, this);
}

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

private:
ros::NodeHandle nh;
ros::Subscriber sub;

void callback(const std_msgs::UInt8::ConstPtr& tune_id)
{
ROS_INFO("Got play tune request!");

if (tune_id->data == 0)
{
ROS_ERROR("Tune ID 0 (custom tune) not supported, ignoring.");
return;
}

mavlink::common::msg::PLAY_TUNE msg{};

msg.target_system = m_uas->get_tgt_system();
msg.target_component = m_uas->get_tgt_component();

/* Tune definitions:
https://github.com/PX4/Firmware/blob/
bfab544a64e2dc3b86905771017989502ec40dd2/src/lib/tunes/
tune_definition.desc#L102
*/
msg.tune[0] = tune_id->data;
UAS_FCU(m_uas)->send_message_ignore_drop(msg);
}
};
} // namespace std_plugins
} // namespace mavros

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

0 comments on commit 55bd190

Please sign in to comment.