Created separate launch file for MIRA framework and took out function…

…ality in scitos_node. Added MIRA units for implementing the proposed watchdog behaviour in issue strands-project#98.

scitos_mira/launch/mira_framework.launch

@@ -0,0 +1,8 @@
+<?xml version="1.0" ?>
+<launch>
+	<!-- scitos_modules argument; must be a space seperated list of hardware modules to load -->
+	<arg name="SCITOS_CONFIG" default="$(find scitos_mira)/resources/SCITOSDriver.xml"/>
+
+	<node pkg="scitos_mira" type="startMiraFramework.sh" args="$(arg SCITOS_CONFIG)" name="startMiraFramework" output="screen">
+	</node>
+</launch>

scitos_mira/launch/scitos_mira.launch

@@ -2,7 +2,6 @@
 <launch>
 	<!-- scitos_modules argument; must be a space seperated list of hardware modules to load -->
 	<arg name="SCITOS_MODULES" default="Drive Charger EBC Display Head"/>
-	<arg name="SCITOS_CONFIG" default="$(find scitos_mira)/resources/SCITOSDriver.xml"/>
 
 	<!-- Remote Launching -->
     <arg name="machine" default="localhost"/>
@@ -11,7 +10,6 @@
     <machine name="$(arg machine)" address="$(arg machine)" env-loader="$(optenv ROS_ENV_LOADER )" user="$(arg user)" default="true" />
 
 	<node pkg="scitos_mira" type="scitos_node" name="scitos_node" output="screen" respawn="true">
-		<param name="config_file" value="$(arg SCITOS_CONFIG)" type="string"/>
 		<param name="scitos_modules" value="$(arg SCITOS_MODULES)" />
 	</node>
 

scitos_mira/scripts/startMiraFramework.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+
+source ~/.bashrc
+
+##############################################################################
+
+while [ true ] ; do
+	echo "Starting MIRA framework at `date`"
+	mira $1 -p1234
+	sleep 5
+done

scitos_mira/src/BridgeWatchdogClient.C

@@ -0,0 +1,50 @@
+/**
+ * @file BridgeWatchdogClient.C
+ *    Will continuously send pings to a channel to notify a BridgeWatchdogServer that everything is okay.
+ *
+ * @author Christian Reuther
+ * @date   2015/02/12
+ */
+
+#include <fw/Unit.h>
+#include <serialization/Serialization.h>
+
+namespace mira { 
+
+////////////////////////////////////////////////////////////////////////////////
+
+class BridgeWatchdogClient : public Unit
+{
+	MIRA_OBJECT(BridgeWatchdogClient)
+
+public:
+	// Spin with 100 milliseconds precision. This can be altered using the serialization interface
+	BridgeWatchdogClient() : Unit(Duration::milliseconds(100)) {
+		mMaxPingAge = Duration::seconds(2);
+	}
+
+	template<typename Reflector> void reflect(Reflector& r)
+	{
+		// Reflect the basic service methods of the base class mira::Unit.
+		Unit::reflect(r);
+	}
+
+protected:
+	virtual void initialize() {
+		mWatchdogChannel = publish<std::string>("/BridgeWatchdog");
+	}
+
+	virtual void process(const Timer& timer) {
+		// Will get the current timestamp
+		mWatchdogChannel.post("OK");
+	}
+
+protected:
+	Channel<std::string> mWatchdogChannel;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+
+}
+
+MIRA_CLASS_SERIALIZATION(mira::BridgeWatchdogClient, mira::Unit);

scitos_mira/src/BridgeWatchdogServer.C

@@ -0,0 +1,75 @@
+/**
+ * @file BridgeWatchdogServer.C
+ *    Will call emergency stop if there is no update to the "/BridgeWatchdog" channel for a certain time.
+ *
+ * @author Christian Reuther
+ * @date   2015/02/12
+ */
+
+#include <fw/Unit.h>
+#include <serialization/Serialization.h>
+
+namespace mira { 
+
+////////////////////////////////////////////////////////////////////////////////
+
+class BridgeWatchdogServer : public Unit
+{
+	MIRA_OBJECT(BridgeWatchdogServer)
+
+public:
+	// Spin with 100 milliseconds precision
+	BridgeWatchdogServer() : Unit(Duration::milliseconds(100)) {
+		mMaxPingAge = Duration::seconds(2);
+	}
+
+	template<typename Reflector> void reflect(Reflector& r)
+	{
+		// Reflect the basic service methods of the base class mira::Unit.
+		Unit::reflect(r);
+
+		// Set it as a property so it can be changed at runtime
+		r.property("MaxPingAge", mMaxPingAge, "Maximum duration between pings before emergencyStop is triggered", Duration::seconds(2));
+	}
+
+protected:
+	virtual void initialize() {
+		mWatchdogChannel = subscribe<std::string>("/BridgeWatchdog");
+
+		// Definitely wait until robot service is available.
+		waitForService("/robot/Robot", Duration::seconds(10));
+	}
+
+	virtual void process(const Timer& timer) {
+		const Time now = Time::now();
+
+		// Get data from watchdog channel and check the timestamp
+		try {
+			auto read = mWatchdogChannel.read();
+			if(Duration(now - read->timestamp).totalMilliseconds() > mMaxPingAge)
+				triggerEmergencyStop();
+		} catch(...) {
+			// Something went wrong, we should definitely stop just in case
+			triggerEmergencyStop();
+		}
+	}
+
+protected: 
+	void triggerEmergencyStop() {
+		MIRA_LOG(ERROR) << "BridgeWatchdogServer: Triggered emergency stop!";
+
+		auto rpcFuture = callService<void>("/robot/Robot", "emergencyStop");
+		rpcFuture.wait();
+		rpcFuture.get(); // get an exception if it occurred - nothing we can do though
+	}
+
+protected:
+	Channel<std::string> mWatchdogChannel;
+	Duration mMaxPingAge;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+
+}
+
+MIRA_CLASS_SERIALIZATION(mira::BridgeWatchdogServer, mira::Unit);

scitos_mira/src/scitos_node.cpp

@@ -6,7 +6,6 @@
 #include <boost/thread.hpp>
 #include <boost/algorithm/string.hpp>
 
-
 #include <string>
 #include <vector>
 
@@ -39,29 +38,8 @@ int main(int argc, char **argv) {
 
 	MIRA_LOGGER.registerSink(RosLogSink());
 
-	std::string  config_file, port_number, scitos_modules;
+	std::string scitos_modules;
 	std::vector<std::string> args;
-
-	if (argc < 2) { // no arguments, so use ROS parameters.
-
-	  if (ros::param::get("~config_file", config_file))  {
-		args.push_back(std::string("-c"));
-		args.push_back(config_file);
-	  } else {
-		ROS_ERROR("Can't read parameter 'config_file'");
-		return 1;
-	  }
-	  if (ros::param::get("~server_port", port_number))  {
-		args.push_back(std::string("-p"));
-		args.push_back(port_number);
-		ROS_INFO_STREAM("Loading with MIRA multiprocess communication support on port " << port_number);
-	  } else {
-		ROS_INFO("Not loading with MIRA multiprocess support.");
-	  }
-	} else {
-	  for (int i=1; i<argc;i++)
-		args.push_back(std::string(argv[i]));
-	}
 
 	mira::Framework framework(args, true);
 
@@ -78,12 +56,12 @@ int main(int argc, char **argv) {
 		    "list of SCITOS hardware modules to interface into ROS"); 
 	  return 1;
 	}
-
 
 	ROS_INFO_STREAM("Creating G5 with modules [" << scitos_modules << "]");
 	std::vector<std::string> modules;
 	boost::split(modules, scitos_modules, boost::is_any_of("\t "));
 	ScitosG5 s(modules);
+
 	ROS_INFO("Going into main loop.");
 	s.spin();
 	return 0;