Merge pull request #37 from clydemcqueen/indigo-devel

Add support for defining and playing songs

README.md

@@ -43,7 +43,7 @@ _* Not verified. Anyone who is able to verify that this driver works or not is e
 |  Brush motors     | Planned [#15](https://github.com/AutonomyLab/create_autonomy/issues/15) |
 |  LEDs             | Available     |
 |  Digit LEDs       | Available     |
-|  Sound            | Planned [#5](https://github.com/AutonomyLab/create_autonomy/issues/5) |
+|  Sound            | Available     |
 |  Wheeldrop        | Available     |
 |  Bumpers          | Available     |
 |  Cliff sensor     | Planned [#22](https://github.com/AutonomyLab/create_autonomy/issues/22)      |
@@ -196,6 +196,8 @@ Topic       | Description   | Type
 `set_ascii` | Sets the 4 digit LEDs. Accepts 1 to 4 bytes, each representing an ASCII character to be displayed from left to right | [std_msgs/UInt8MultiArray][uint8multiarray]
 `dock` | Activates the demo docking behaviour. Robot enters _Passive_ mode meaning the user loses control (See [OI Spec][oi_spec]) | [std_msgs/Empty][empty]
 `undock` | Switches robot to _Full_ mode giving control back to the user | [std_msgs/Empty][empty]
+`define_song` | Define a song with up to 16 notes. Each note is described by a MIDI note number and a float32 duration in seconds. The longest duration is 255/64 seconds. You can define up to 4 songs (See [OI Spec][oi_spec]) | [ca_msgs/DefineSong][definesong_msg]
+`play_song` | Play a predefined song | [ca_msgs/PlaySong][playsong_msg]
 
 ## Commanding your Create
 
@@ -229,6 +231,8 @@ Contributing to the development and maintenance of _create\_autonomy_ is encoura
 
 * [Michael Browne](http://brownem.engineer/)
     - Confirms driver works with Roomba 700 and 800 series.
+* [Clyde McQueen](https://github.com/clydemcqueen)
+    - Added support for sound ([#37](https://github.com/AutonomyLab/create_autonomy/pull/37)).
 * [Ben Wolsieffer](https://github.com/lopsided98) 
     - Added JointState publisher for wheels ([#26](https://github.com/AutonomyLab/create_autonomy/pull/26)).
     - Added Create 1 description ([#27](https://github.com/AutonomyLab/create_autonomy/pull/27)).
@@ -248,3 +252,6 @@ Contributing to the development and maintenance of _create\_autonomy_ is encoura
 [mode_msg]:  https://github.com/AutonomyLab/create_autonomy/blob/indigo-devel/ca_msgs/msg/Mode.msg
 [chargingstate_msg]:  https://github.com/AutonomyLab/create_autonomy/blob/indigo-devel/ca_msgs/msg/ChargingState.msg
 [jointstate_msg]:  http://docs.ros.org/api/sensor_msgs/html/msg/JointState.html
+[definesong_msg]:  https://github.com/AutonomyLab/create_autonomy/blob/indigo-devel/ca_msgs/msg/DefineSong.msg
+[playsong_msg]:  https://github.com/AutonomyLab/create_autonomy/blob/indigo-devel/ca_msgs/msg/PlaySong.msg
+

ca_driver/include/create_driver/create_driver.h

@@ -19,6 +19,8 @@
 #include "ca_msgs/ChargingState.h"
 #include "ca_msgs/Mode.h"
 #include "ca_msgs/Bumper.h"
+#include "ca_msgs/DefineSong.h"
+#include "ca_msgs/PlaySong.h"
 
 static const double MAX_DBL = std::numeric_limits<double>::max();
 static const double COVARIANCE[36] = {1e-5, 1e-5, 0.0,     0.0,     0.0,     1e-5,
@@ -66,6 +68,8 @@ class CreateDriver
   void setASCIICallback(const std_msgs::UInt8MultiArrayConstPtr& msg);
   void dockCallback(const std_msgs::EmptyConstPtr& msg);
   void undockCallback(const std_msgs::EmptyConstPtr& msg);
+  void defineSongCallback(const ca_msgs::DefineSongConstPtr& msg);
+  void playSongCallback(const ca_msgs::PlaySongConstPtr& msg);
 
   bool update();
   void updateBatteryDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat);
@@ -94,6 +98,8 @@ class CreateDriver
   ros::Subscriber set_ascii_sub_;
   ros::Subscriber dock_sub_;
   ros::Subscriber undock_sub_;
+  ros::Subscriber define_song_sub_;
+  ros::Subscriber play_song_sub_;
 
   ros::Publisher odom_pub_;
   ros::Publisher clean_btn_pub_;

ca_driver/src/create_driver.cpp

@@ -89,6 +89,8 @@ CreateDriver::CreateDriver(ros::NodeHandle& nh)
   set_ascii_sub_ = nh.subscribe("set_ascii", 10, &CreateDriver::setASCIICallback, this);
   dock_sub_ = nh.subscribe("dock", 10, &CreateDriver::dockCallback, this);
   undock_sub_ = nh.subscribe("undock", 10, &CreateDriver::undockCallback, this);
+  define_song_sub_ = nh.subscribe("define_song", 10, &CreateDriver::defineSongCallback, this);
+  play_song_sub_ = nh.subscribe("play_song", 10, &CreateDriver::playSongCallback, this);
 
   // Setup publishers
   odom_pub_ = nh.advertise<nav_msgs::Odometry>("odom", 30);
@@ -222,6 +224,22 @@ void CreateDriver::undockCallback(const std_msgs::EmptyConstPtr& msg)
   robot_->setMode(create::MODE_FULL);
 }
 
+void CreateDriver::defineSongCallback(const ca_msgs::DefineSongConstPtr& msg)
+{
+  if (!robot_->defineSong(msg->song, msg->length, &(msg->notes.front()), &(msg->durations.front())))
+  {
+    ROS_ERROR_STREAM("[CREATE] Failed to define song " << msg->song << " of length " << msg->length);
+  }
+}
+
+void CreateDriver::playSongCallback(const ca_msgs::PlaySongConstPtr& msg)
+{
+  if (!robot_->playSong(msg->song))
+  {
+    ROS_ERROR_STREAM("[CREATE] Failed to play song " << msg->song);
+  }
+}
+
 bool CreateDriver::update()
 {
   publishOdom();

ca_msgs/CMakeLists.txt

@@ -10,7 +10,9 @@ add_message_files(
   FILES
   Bumper.msg
   ChargingState.msg
+  DefineSong.msg
   Mode.msg
+  PlaySong.msg
 )
 
 generate_messages(

ca_msgs/msg/DefineSong.msg

@@ -0,0 +1,4 @@
+uint8 song            # song number [0-3]
+uint8 length          # song length [1-16]
+uint8[] notes         # notes defined by the MIDI note numbering scheme. Notes outside the range of [31-127] are rest notes.
+float32[] durations   # durations in seconds. Maximum duration is 255/64.

ca_msgs/msg/PlaySong.msg

@@ -0,0 +1 @@
+uint8 song      # song number [0-3]