Adding simple autorecover - hopefully all of my build errors are fixed

can_msgs/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(catkin REQUIRED
 add_message_files(DIRECTORY msg
   FILES
     Frame.msg
+    CanState.msg
 )
 
 generate_messages(

can_msgs/msg/CanState.msg

@@ -0,0 +1,13 @@
+# CanState.msg
+#
+# Message denoting the state of the CAN Bus
+#
+# @author Joe Adkisson
+# @version 0.1
+
+uint8  driver_state
+
+# enumeration values for status:
+uint8 CLOSED=0
+uint8 OPEN=1
+uint8 READY=2

socketcan_bridge/CMakeLists.txt

@@ -1,4 +1,5 @@
 cmake_minimum_required(VERSION 2.8.3)
+add_compile_options(-std=c++11)
 project(socketcan_bridge)
 
 find_package(catkin REQUIRED
@@ -14,6 +15,7 @@ catkin_package(
   LIBRARIES
     socketcan_to_topic
     topic_to_socketcan
+    socketcan_recover_ctrl
   CATKIN_DEPENDS
     can_msgs
     roscpp
@@ -48,12 +50,24 @@ add_dependencies(topic_to_socketcan
   ${catkin_EXPORTED_TARGETS}
 )
 
+# socketcan_recover_ctrl
+add_library(socketcan_recover_ctrl
+  src/socketcan_recover_ctrl.cpp
+)
+target_link_libraries(socketcan_recover_ctrl
+  ${catkin_LIBRARIES}
+)
+add_dependencies(socketcan_recover_ctrl
+  ${catkin_EXPORTED_TARGETS}
+)
+
 # socketcan_to_topic_node
 add_executable(socketcan_to_topic_node
   src/socketcan_to_topic_node.cpp
 )
 target_link_libraries(socketcan_to_topic_node
   socketcan_to_topic
+  socketcan_recover_ctrl
   ${catkin_LIBRARIES}
 )
 
@@ -64,6 +78,7 @@ add_executable(topic_to_socketcan_node
 )
 target_link_libraries(topic_to_socketcan_node
   topic_to_socketcan
+  socketcan_recover_ctrl
   ${catkin_LIBRARIES}
 )
 
@@ -74,16 +89,20 @@ add_executable(socketcan_bridge_node
 target_link_libraries(socketcan_bridge_node
   topic_to_socketcan
   socketcan_to_topic
+  socketcan_recover_ctrl
   ${catkin_LIBRARIES}
 )
 
+
 install(
   TARGETS
     socketcan_bridge_node
+    socketcan_recover_ctrl
     socketcan_to_topic
     socketcan_to_topic_node
     topic_to_socketcan
     topic_to_socketcan_node
+
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -109,6 +128,7 @@ if(CATKIN_ENABLE_TESTING)
   target_link_libraries(test_conversion
     topic_to_socketcan
     socketcan_to_topic
+    socketcan_recover_ctrl
     ${catkin_LIBRARIES}
   )
 
@@ -131,4 +151,6 @@ if(CATKIN_ENABLE_TESTING)
     ${catkin_LIBRARIES}
   )
 
+
+
 endif()

socketcan_bridge/include/socketcan_bridge/socketcan_recover_ctrl.h

@@ -0,0 +1,59 @@
+#ifndef SOCKETCAN_RECOVER_CTRL_H
+#define SOCKETCAN_RECOVER_CTRL_H
+
+#include <can_msgs/CanState.h>
+#include <socketcan_interface/socketcan.h>
+#include <ros/ros.h>
+
+namespace socketcan_bridge
+{
+
+class SocketCANRecoverCtrl
+{
+public:
+
+	/**
+     * @brief Constructor for the recovery control class
+     *
+     * Initializes the publisher, and sets up a timer to periodically check the bus state
+     */
+    SocketCANRecoverCtrl(ros::NodeHandle* nh, ros::NodeHandle* nh_param, boost::shared_ptr<can::DriverInterface> driver);
+
+    ~SocketCANRecoverCtrl()
+    {
+        timer_.stop();
+    }
+
+private:
+
+	/**
+ 	* @brief Publishes the status of the bus
+ 	*/
+    void publishStatus(const can::State & state);
+
+    /**
+     * @brief Recover the bus from an error state
+     *
+     * Calls the driver's recover() function
+     */
+    void recover();
+
+    /**
+     * @brief Checks the state of the bus, if !statie.isReady() then the
+     * recover timer is started to fire in 5secs, otherwise we stop the timer
+     */
+    void stateCallback(const can::State & s);
+
+
+    ros::Publisher state_pub_;
+    boost::shared_ptr<can::DriverInterface> driver_;
+    ros::WallTimer timer_;
+
+    can::StateInterface::StateListener::Ptr state_listener_;
+
+};
+
+};  // namespace socketcan_bridge
+
+
+#endif

socketcan_bridge/src/socketcan_bridge_node.cpp

@@ -28,6 +28,7 @@
 #include <ros/ros.h>
 #include <socketcan_bridge/topic_to_socketcan.h>
 #include <socketcan_bridge/socketcan_to_topic.h>
+#include <socketcan_bridge/socketcan_recover_ctrl.h>
 #include <socketcan_interface/threading.h>
 #include <string>
 
@@ -60,6 +61,10 @@ int main(int argc, char *argv[])
   socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, driver);
   to_topic_bridge.setup();
 
+    // Recovery service
+  socketcan_bridge::SocketCANRecoverCtrl recover_ctrl(&nh, &nh_param, driver);
+
+
   ros::spin();
 
   driver->shutdown();

socketcan_bridge/src/socketcan_recover_ctrl.cpp

@@ -0,0 +1,81 @@
+#include <socketcan_bridge/socketcan_recover_ctrl.h>
+
+namespace socketcan_bridge
+{
+
+    /**
+     * @brief Constructor for the recovery control class
+     *
+     * Initializes the publisher, and sets up a timer to periodically check the bus state
+     */
+    SocketCANRecoverCtrl::SocketCANRecoverCtrl(ros::NodeHandle* nh, ros::NodeHandle* nh_param,
+        boost::shared_ptr<can::DriverInterface> driver) : driver_(driver)
+    {
+        state_pub_ = nh->advertise<can_msgs::CanState>("can_state", 1, true);
+
+        timer_ = nh->createWallTimer(ros::WallDuration(5), [this](const ros::WallTimerEvent& event) {recover();}, true, false);
+
+        state_listener_ = driver_->createStateListener(
+            can::StateInterface::StateDelegate(this, &SocketCANRecoverCtrl::stateCallback));
+
+    }
+
+    /**
+     * @brief Checks the state of the bus, if !statie.isReady() then the
+     * recover timer is started to fire in 5secs, otherwise we stop the timer
+     */
+    void SocketCANRecoverCtrl::stateCallback(const can::State & state) {
+        publishStatus(state);
+        if(!state.isReady())
+        {            
+            timer_.start();
+        }
+        else
+        {
+            timer_.stop();
+        }
+    }
+
+    /**
+     * @brief Recover the bus from an error state
+     *
+     * Calls the driver's recover() function
+     */
+    void SocketCANRecoverCtrl::recover() 
+    {
+        timer_.stop();
+        if(driver_->recover()) {
+            ROS_INFO("CAN driver timed out, successfully recovered");
+        } 
+        else 
+        {
+            ROS_WARN("CAN driver timed out, recovery failed");
+            timer_.start();
+        }
+    }
+
+    /**
+     * @brief Publishes the status of the bus
+     */
+    void SocketCANRecoverCtrl::publishStatus(const can::State & state) {
+        can_msgs::CanState state_msg;
+        switch(state.driver_state) {
+            case can::State::open:
+                state_msg.driver_state = can_msgs::CanState::OPEN;
+                break;
+            case can::State::closed:
+                state_msg.driver_state = can_msgs::CanState::CLOSED;
+                break;
+            case can::State::ready:
+                state_msg.driver_state = can_msgs::CanState::READY;
+                break;
+            default:
+                state_msg.driver_state = can_msgs::CanState::CLOSED;
+                break;
+        }
+
+        state_pub_.publish(state_msg);
+    }
+
+
+};  // namespace socketcan_bridge

socketcan_bridge/src/socketcan_to_topic_node.cpp

@@ -27,6 +27,7 @@
 
 #include <ros/ros.h>
 #include <socketcan_bridge/socketcan_to_topic.h>
+#include <socketcan_bridge/socketcan_recover_ctrl.h>
 #include <socketcan_interface/threading.h>
 #include <socketcan_interface/string.h>
 #include <string>
@@ -56,6 +57,8 @@ int main(int argc, char *argv[])
 
   socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, driver);
   to_topic_bridge.setup();
+  // Recovery service
+  socketcan_bridge::SocketCANRecoverCtrl recover_ctrl(&nh, &nh_param, driver);
 
   ros::spin();
 

socketcan_bridge/src/topic_to_socketcan_node.cpp

@@ -27,6 +27,7 @@
 
 #include <ros/ros.h>
 #include <socketcan_bridge/topic_to_socketcan.h>
+#include <socketcan_bridge/socketcan_recover_ctrl.h>
 #include <socketcan_interface/threading.h>
 #include <socketcan_interface/string.h>
 #include <string>
@@ -57,6 +58,10 @@ int main(int argc, char *argv[])
   socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, driver);
   to_socketcan_bridge.setup();
 
+
+  // Recovery service
+  socketcan_bridge::SocketCANRecoverCtrl recover_ctrl(&nh, &nh_param, driver);
+
   ros::spin();
 
   driver->shutdown();

socketcan_interface/include/socketcan_interface/socketcan.h

@@ -149,7 +149,7 @@ class SocketCANInterface : public AsioDriver<boost::asio::posix::stream_descript
             ret = true;
         }
         if( internal_error & CAN_ERR_RESTARTED){
-            str += "ontroller restarted;";
+            str += "controller restarted;";
             ret = true;
         }
         return ret;