Release: v0.2.0 (#442)

* Feature: sound interface (#436) * Added sound_play sounds to safety * Removed sound scheduler * Add sound_play_node to launch file * Several small fixes * Fix clang format * Remove sound scheduler exec depend * Clang tidy fixes * Feature/pm246 stairs in sim (#439) * Fix undefined value in xacro when launching on exo. * Add stairs to simulation Co-authored-by: Olav de Haas <[email protected]> * Feature/pm 249 add rockerswitch input (#438) * Added rocker switch variable to the gait instruction msg generated by the (mock) input device. The message can send a 1 which means that the rocker switch has been incremented and a -1 which means that the rocker switch has been decremented. * Added two more variables to the gait instruction message to distinguish increment or decrement of the rocker switch. * Removed the rocker switch argument and created the increment and decrement possibilities as instruction types. Co-authored-by: Roelemans <[email protected]> Co-authored-by: JorisWeeda <[email protected]>
project-march · Jan 28, 2020 · 9e0a785 · 9e0a785
1 parent e8f9a9f
commit 9e0a785
Show file tree

Hide file tree

Showing 26 changed files with 35 additions and 262 deletions.
diff --git a/march_launch/launch/march.launch b/march_launch/launch/march.launch
@@ -9,6 +9,7 @@
     <arg name="gazebo_ui" default="false" doc="Launches the Gazebo UI."/>
 
     <arg name="ground_gait" default="false" doc="Exoskeleton will ground gait in gazebo_ui if true."/>
+    <arg name="obstacle" default="none" doc="Obstacle to load in the simulation."/>
     <arg name="unpause" default="true" doc="Unpause simulation when controller starts."/>
     <arg name="fixed" default="true" doc="Fixes the exoskeleton in the world in rviz"/>
 
@@ -39,7 +40,7 @@
 
         <include file="$(find march_safety)/launch/march_safety.launch"/>
 
-        <include file="$(find march_sound_scheduler)/launch/march_sound_scheduler.launch"/>
+        <node name="sound_play" pkg="sound_play" type="soundplay_node.py"/>
 
         <include file="$(find march_gait_selection)/launch/march_gait_selection.launch">
             <arg name="gait_directory" value="$(arg gait_directory)"/>
@@ -63,6 +64,7 @@
             <arg name="gazebo_ui" value="$(eval not headless and gazebo_ui)"/>
             <arg name="fixed" value="$(arg fixed)"/>
             <arg name="ground_gait" value="$(arg ground_gait)"/>
+            <arg name="obstacle" value="$(arg obstacle)"/>
         </include>
     </group>
 

diff --git a/march_launch/launch/march_simulation.launch b/march_launch/launch/march_simulation.launch
@@ -6,6 +6,7 @@
     <arg name="unpause" default="$(eval not gazebo_ui)" doc="Unpause simulation when controller starts."/>
     <arg name="fixed" default="true" doc="Fixes the exoskeleton in the world"/>
     <arg name="ground_gait" default="false" doc="Exoskeleton will ground gait in simulation if true."/>
+    <arg name="obstacle" default="none" doc="Obstacle to load in the simulation."/>
     <arg name="gait_directory" default="training-v" doc="Gait files directory to use"/>
     <arg name="input-device" default="false" doc="Launches ros serial node to connect with input device."/>
 

diff --git a/march_launch/package.xml b/march_launch/package.xml
@@ -20,7 +20,6 @@
     <exec_depend>march_gain_scheduling</exec_depend>
     <exec_depend>march_gait_selection</exec_depend>
     <exec_depend>march_safety</exec_depend>
-    <exec_depend>march_sound_scheduler</exec_depend>
     <exec_depend>march_state_machine</exec_depend>
     <exec_depend>xacro</exec_depend>
     <exec_depend>rosserial_python</exec_depend>

diff --git a/march_safety/CMakeLists.txt b/march_safety/CMakeLists.txt
@@ -7,6 +7,7 @@ find_package(catkin REQUIRED COMPONENTS
     march_shared_resources
     roscpp
     sensor_msgs
+    sound_play
     std_msgs
     urdf
 )
@@ -18,6 +19,7 @@ catkin_package(
     march_shared_resources
     roscpp
     sensor_msgs
+    sound_play
     std_msgs
     urdf
 )
@@ -47,7 +49,7 @@ install(TARGETS ${PROJECT_NAME}
 install(TARGETS ${PROJECT_NAME}_node
     RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
-install(DIRECTORY launch
+install(DIRECTORY launch sound
     DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 

diff --git a/march_safety/include/march_safety/input_device_safety.h b/march_safety/include/march_safety/input_device_safety.h
@@ -21,7 +21,6 @@ class InputDeviceSafety : public SafetyType
 private:
   void inputDeviceAliveCallback(const std_msgs::TimeConstPtr& msg);
 
-  ros::NodeHandle n_;
   SafetyHandler* safety_handler_;
   ros::Duration connection_timeout_;
   ros::Time time_last_alive_;

diff --git a/march_safety/include/march_safety/safety_handler.h b/march_safety/include/march_safety/safety_handler.h
@@ -4,31 +4,32 @@
 
 #include <string>
 #include <ros/ros.h>
+#include <sound_play/sound_play.h>
+
 #include <march_shared_resources/Error.h>
-#include <march_shared_resources/Sound.h>
 #include <march_shared_resources/GaitInstruction.h>
 
 class SafetyHandler
 {
 public:
-  SafetyHandler(ros::NodeHandle* n, ros::Publisher* error_publisher, ros::Publisher* sound_publisher,
-                ros::Publisher* gait_instruction_publisher);
+  SafetyHandler(ros::NodeHandle* n, ros::Publisher* error_publisher, ros::Publisher* gait_instruction_publisher,
+                sound_play::SoundClient& sound_client);
 
-  void publishFatal(std::string message);
+  void publishFatal(const std::string& message);
 
-  void publishNonFatal(std::string message);
+  void publishNonFatal(const std::string& message);
 
   void publishErrorMessage(const std::string& message, int8_t error_type) const;
 
   void publishStopMessage() const;
 
-  void publishErrorSound(int8_t error_type) const;
-
 private:
   ros::NodeHandle* n_;
   ros::Publisher* error_publisher_;
-  ros::Publisher* sound_publisher_;
   ros::Publisher* gait_instruction_publisher_;
+
+  sound_play::Sound fatal_sound_;
+  sound_play::Sound non_fatal_sound_;
 };
 
 #endif  // MARCH_SAFETY_SAFETY_HANDLER_H
diff --git a/march_safety/include/march_safety/temperature_safety.h b/march_safety/include/march_safety/temperature_safety.h
@@ -10,7 +10,6 @@
 #include <sensor_msgs/Temperature.h>
 
 #include <march_shared_resources/Error.h>
-#include <march_shared_resources/Sound.h>
 
 #include "march_safety/safety_type.h"
 #include "march_safety/safety_handler.h"

diff --git a/march_safety/package.xml b/march_safety/package.xml
@@ -16,6 +16,7 @@
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>urdf</depend>
+  <depend>sound_play</depend>
 
   <test_depend>rostest</test_depend>
   <test_depend>code_coverage</test_depend>

diff --git a/march_sound_scheduler/sound/fatal.wav → march_safety/sound/fatal.wav b/march_sound_scheduler/sound/fatal.wav → march_safety/sound/fatal.wav
diff --git a/march_sound_scheduler/sound/non-fatal.wav → march_safety/sound/non_fatal.wav b/march_sound_scheduler/sound/non-fatal.wav → march_safety/sound/non_fatal.wav
diff --git a/march_safety/src/input_device_safety.cpp b/march_safety/src/input_device_safety.cpp
@@ -7,7 +7,7 @@ InputDeviceSafety::InputDeviceSafety(ros::NodeHandle* n, SafetyHandler* safety_h
 {
   int milliseconds;
   ros::param::get("~input_device_connection_timeout", milliseconds);
-  double send_errors_interval_param;
+  int send_errors_interval_param;
   ros::param::get("~send_errors_interval", send_errors_interval_param);
   this->send_errors_interval_ = send_errors_interval_param;
   this->connection_timeout_ = ros::Duration(milliseconds / 1000.0);

diff --git a/march_safety/src/safety_handler.cpp b/march_safety/src/safety_handler.cpp
@@ -3,12 +3,13 @@
 
 #include <string>
 
-SafetyHandler::SafetyHandler(ros::NodeHandle* n, ros::Publisher* error_publisher, ros::Publisher* sound_publisher,
-                             ros::Publisher* gait_instruction_publisher)
+SafetyHandler::SafetyHandler(ros::NodeHandle* n, ros::Publisher* error_publisher,
+                             ros::Publisher* gait_instruction_publisher, sound_play::SoundClient& sound_client)
   : n_(n)
   , error_publisher_(error_publisher)
-  , sound_publisher_(sound_publisher)
   , gait_instruction_publisher_(gait_instruction_publisher)
+  , fatal_sound_(sound_client.waveSoundFromPkg("march_safety", "sound/fatal.wav"))
+  , non_fatal_sound_(sound_client.waveSoundFromPkg("march_safety", "sound/non_fatal.wav"))
 {
 }
 
@@ -30,35 +31,19 @@ void SafetyHandler::publishStopMessage() const
   this->gait_instruction_publisher_->publish(gait_instruction_msg);
 }
 
-void SafetyHandler::publishErrorSound(int8_t error_type) const
-{
-  march_shared_resources::Sound sound;
-  sound.header.stamp = ros::Time::now();
-  sound.time = ros::Time::now();
-  if (error_type == march_shared_resources::Error::FATAL)
-  {
-    sound.file_name = "fatal.wav";
-  }
-  else if (error_type == march_shared_resources::Error::NON_FATAL)
-  {
-    sound.file_name = "non-fatal.wav";
-  }
-  this->sound_publisher_->publish(sound);
-}
-
-void SafetyHandler::publishFatal(std::string message)
+void SafetyHandler::publishFatal(const std::string& message)
 {
   ROS_ERROR("%s", message.c_str());
 
   this->publishErrorMessage(message, march_shared_resources::Error::FATAL);
-  this->publishErrorSound(march_shared_resources::Error::FATAL);
+  this->non_fatal_sound_.play();
 }
 
-void SafetyHandler::publishNonFatal(std::string message)
+void SafetyHandler::publishNonFatal(const std::string& message)
 {
   ROS_ERROR("%s", message.c_str());
 
   this->publishStopMessage();
   this->publishErrorMessage(message, march_shared_resources::Error::NON_FATAL);
-  this->publishErrorSound(march_shared_resources::Error::NON_FATAL);
+  this->non_fatal_sound_.play();
 }
diff --git a/march_safety/src/safety_node.cpp b/march_safety/src/safety_node.cpp
@@ -1,21 +1,16 @@
 // Copyright 2018 Project March.
-#include <sstream>
 #include <string>
 #include <vector>
 
 #include <ros/ros.h>
-#include <std_msgs/Float64.h>
-#include <std_msgs/Empty.h>
-#include <sensor_msgs/Temperature.h>
-#include <urdf/model.h>
+#include <sound_play/sound_play.h>
 
 #include "march_shared_resources/Error.h"
-#include "march_shared_resources/Sound.h"
 #include "march_shared_resources/GaitInstruction.h"
 
 #include "march_safety/input_device_safety.h"
-#include "march_safety/temperature_safety.h"
 #include "march_safety/safety_handler.h"
+#include "march_safety/temperature_safety.h"
 
 int main(int argc, char** argv)
 {
@@ -36,11 +31,11 @@ int main(int argc, char** argv)
 
   // Create an error publisher to notify the system (state machine) if something is wrong
   ros::Publisher error_publisher = n.advertise<march_shared_resources::Error>("/march/error", 1000);
-  ros::Publisher sound_publisher = n.advertise<march_shared_resources::Sound>("/march/sound/schedule", 1000);
   ros::Publisher gait_instruction_publisher =
       n.advertise<march_shared_resources::GaitInstruction>("/march/input_device/instruction", 1000);
+  sound_play::SoundClient sound_client(n, "robotsound");
 
-  SafetyHandler safety_handler = SafetyHandler(&n, &error_publisher, &sound_publisher, &gait_instruction_publisher);
+  SafetyHandler safety_handler = SafetyHandler(&n, &error_publisher, &gait_instruction_publisher, sound_client);
 
   std::vector<std::unique_ptr<SafetyType>> safety_list;
   safety_list.push_back(std::unique_ptr<SafetyType>(new TemperatureSafety(&n, &safety_handler, joint_names)));

diff --git a/march_safety/src/temperature_safety.cpp b/march_safety/src/temperature_safety.cpp
@@ -9,7 +9,7 @@
 
 TemperatureSafety::TemperatureSafety(ros::NodeHandle* n, SafetyHandler* safety_handler,
                                      std::vector<std::string> joint_names)
-  : n_(n), safety_handler_(safety_handler), joint_names_(joint_names)
+  : n_(n), safety_handler_(safety_handler), joint_names_(std::move(joint_names))
 {
   ros::param::get("~default_temperature_threshold", this->default_temperature_threshold_);
   ros::param::get("~temperature_thresholds_warning", this->warning_temperature_thresholds_map_);

diff --git a/march_shared_resources/CMakeLists.txt b/march_shared_resources/CMakeLists.txt
@@ -21,7 +21,6 @@ add_message_files(
     LowVoltageNet.msg
     PowerDistributionBoardState.msg
     Setpoint.msg
-    Sound.msg
     Subgait.msg
 )
 

diff --git a/march_shared_resources/msg/GaitInstruction.msg b/march_shared_resources/msg/GaitInstruction.msg
@@ -8,6 +8,9 @@ int8 STOP = 0
 int8 GAIT = 1
 int8 PAUSE = 2
 int8 CONTINUE = 3
+int8 INCREMENT_STEP_SIZE = 4
+int8 DECREMENT_STEP_SIZE = 5
 
 # If the type is a GAIT this is the name of the gait.
 string gait_name
+
diff --git a/march_shared_resources/msg/Sound.msg b/march_shared_resources/msg/Sound.msg
diff --git a/march_shared_resources/msg/Subgait.msg b/march_shared_resources/msg/Subgait.msg
@@ -9,4 +9,4 @@ Setpoint[] setpoints
 
 trajectory_msgs/JointTrajectory trajectory
 duration duration
-Sound[] sounds
+string[] sounds
diff --git a/march_sound_scheduler/CMakeLists.txt b/march_sound_scheduler/CMakeLists.txt
diff --git a/march_sound_scheduler/include/march_sound_scheduler/scheduled_sound.h b/march_sound_scheduler/include/march_sound_scheduler/scheduled_sound.h
diff --git a/march_sound_scheduler/include/march_sound_scheduler/scheduler.h b/march_sound_scheduler/include/march_sound_scheduler/scheduler.h
diff --git a/march_sound_scheduler/launch/march_sound_scheduler.launch b/march_sound_scheduler/launch/march_sound_scheduler.launch