diff --git a/rosserial_libs/ros_lib/ros/node_handle.h b/rosserial_libs/ros_lib/ros/node_handle.h index 9babaff..acbad0a 100644 --- a/rosserial_libs/ros_lib/ros/node_handle.h +++ b/rosserial_libs/ros_lib/ros/node_handle.h @@ -348,8 +348,8 @@ namespace ros { } /* Register a new subscriber */ - template - bool subscribe(Subscriber< MsgT> & s){ + template + bool subscribe(SubscriberT & s){ for(int i = 0; i < MAX_SUBSCRIBERS; i++){ if(subscribers[i] == 0){ // empty slot subscribers[i] = (Subscriber_*) &s; diff --git a/rosserial_libs/ros_lib/ros/subscriber.h b/rosserial_libs/ros_lib/ros/subscriber.h index 5464646..0c07cb1 100644 --- a/rosserial_libs/ros_lib/ros/subscriber.h +++ b/rosserial_libs/ros_lib/ros/subscriber.h @@ -54,10 +54,43 @@ namespace ros { const char * topic_; }; + /* Bound function subscriber. */ + template + class Subscriber : public Subscriber_ + { + public: + typedef void(ObjT::*CallbackT)(const MsgT&); + MsgT msg; + + Subscriber(const char * topic_name, CallbackT cb, ObjT* obj, int endpoint = rosserial_msgs::TopicInfo::ID_SUBSCRIBER) : + cb_(cb), + obj_(obj), + endpoint_(endpoint) + { + topic_ = topic_name; + }; + + virtual void callback(unsigned char* data) + { + msg.deserialize(data); + (obj_->*cb_)(msg); + } + + virtual const char * getMsgType() { return this->msg.getType(); } + virtual const char * getMsgMD5() { return this->msg.getMD5(); } + virtual int getEndpointType() { return endpoint_; } + private: + CallbackT cb_; + ObjT* obj_; + int endpoint_; + }; + + /* Standalone function subscriber. */ /* Actual subscriber, templated on message type. */ template - class Subscriber: public Subscriber_{ + class Subscriber : public Subscriber_ + { public: typedef void(*CallbackT)(const MsgT&); MsgT msg; diff --git a/src/RosInterface/RosInterface.cpp b/src/RosInterface/RosInterface.cpp index 857262f..8f04b0a 100644 --- a/src/RosInterface/RosInterface.cpp +++ b/src/RosInterface/RosInterface.cpp @@ -11,10 +11,12 @@ #include "RosInterface.h" -#include -#include -#include -#include +//#include +//#include +//#include +//#include +//#include +//#include #include #include @@ -96,9 +98,12 @@ namespace supra }; #endif + bool supra::RosInterface::m_sequenceActive = false; + void RosInterface::mainLoop(string masterHost) { RosWrapper wr(masterHost); + m_sequenceActive = false; #ifdef ROS_ROSSERIAL GetNodesServer servGetNodes(supra_msgs::GET_NODES, RosInterface::get_nodesCallback); @@ -114,10 +119,18 @@ namespace supra (supra_msgs::SET_NODE_PARAMETER, (RosInterface::set_node_parameterCallback)); bool setNodeParameterAdvertisement = wr.getNodeHandle()->advertiseService(servSetNodeParameter); logging::log_error_if(!setNodeParameterAdvertisement, "Error advertising service ", "set_node_parameter"); + + ros::ServiceServer servSequence + (supra_msgs::SEQUENCE, (RosInterface::sequenceCallback)); + bool sequenceAdvertisement = wr.getNodeHandle()->advertiseService(servSequence); + logging::log_error_if(!sequenceAdvertisement, "Error advertising service ", "sequence"); + wr.subscribe("supra_freeze", RosInterface::freezeCallback); #else auto serviceGetNodes = wr.getNodeHandle()->advertiseService("get_nodes", RosInterface::get_nodesCallback); auto serviceGetNodeParameters = wr.getNodeHandle()->advertiseService("get_node_parameters", RosInterface::get_node_parametersCallback); auto serviceSetNodeParameters = wr.getNodeHandle()->advertiseService("set_node_parameter", RosInterface::set_node_parameterCallback); + auto serviceSequence = wr.getNodeHandle()->advertiseService("sequence", RosInterface::sequenceCallback); + wr.subscribe("supra_freeze", RosInterface::freezeCallback); #endif wr.spinEndless(); @@ -186,6 +199,19 @@ namespace supra #endif } + ServiceReturnType RosInterface::sequenceCallback(ROSSERIAL_REQUESTTYPECONST supra_msgs::sequence::Request & req, supra_msgs::sequence::Response& res) + { + res.success = sequence(req.sequenceActive); +#ifdef ROS_REAL + return res.success; +#endif + } + + void RosInterface::freezeCallback(const supra_msgs::freeze & freezeMsg) + { + freeze(freezeMsg.freezeActive); + } + void RosInterface::fillParameterMessage(const ConfigurationDictionary* confDict, const ValueRangeDictionary * rangeDict, std::string paramName, supra_msgs::parameter* pParam) { if (rangeDict->hasKey(paramName)) @@ -426,4 +452,36 @@ namespace supra } return retVal; } + + bool RosInterface::sequence(bool active) + { + bool ret = false; + if(m_sequenceActive && !active) + { + SupraManager::Get()->stopOutputsSequence(); + m_sequenceActive = false; + ret = true; + } + else if(!m_sequenceActive && active) + { + SupraManager::Get()->startOutputsSequence(); + m_sequenceActive = true; + ret = true; + } + return ret; + } + + void RosInterface::freeze(bool freezeActive) + { + if(freezeActive) + { + SupraManager::Get()->freezeInputs(); + log_info("RosInterface: Freezing inputs"); + } + else if(!freezeActive) + { + SupraManager::Get()->unfreezeInputs(); + log_info("RosInterface: Unfreezing inputs"); + } + } } diff --git a/src/RosInterface/RosInterface.h b/src/RosInterface/RosInterface.h index 175d3b0..bd97420 100644 --- a/src/RosInterface/RosInterface.h +++ b/src/RosInterface/RosInterface.h @@ -25,6 +25,8 @@ #include #include #include +#include +#include namespace supra { @@ -58,6 +60,8 @@ namespace supra static ServiceReturnType get_nodesCallback(ROSSERIAL_REQUESTTYPECONST supra_msgs::get_nodes::Request & req, supra_msgs::get_nodes::Response& res); static ServiceReturnType get_node_parametersCallback(ROSSERIAL_REQUESTTYPECONST supra_msgs::get_node_parameters::Request & req, supra_msgs::get_node_parameters::Response & res); static ServiceReturnType set_node_parameterCallback(ROSSERIAL_REQUESTTYPECONST supra_msgs::set_node_parameter::Request & req, supra_msgs::set_node_parameter::Response & res); + static ServiceReturnType sequenceCallback(ROSSERIAL_REQUESTTYPECONST supra_msgs::sequence::Request & req, supra_msgs::sequence::Response& res); + static void freezeCallback(const supra_msgs::freeze & freezeMsg); static std::string getParameterTypeString(const ValueRangeDictionary * ranges, std::string paramName); static std::string getParameterValueString(const ConfigurationDictionary * config, const ValueRangeDictionary * ranges, std::string paramName); @@ -66,6 +70,8 @@ namespace supra static void fillParameterMessage(const ConfigurationDictionary* confDict, const ValueRangeDictionary * ranges, std::string paramName, supra_msgs::parameter* pParam); static bool convertAndSetParameter(std::string inputID, std::string paramName, std::string newValue); + static bool sequence(bool active); + static void freeze(bool freezeActive); template static void fillParameterMessage(const ConfigurationDictionary* confDict, const ValueRangeDictionary * ranges, std::string paramName, supra_msgs::parameter* pParam) @@ -94,6 +100,8 @@ namespace supra } } } + + static bool m_sequenceActive; }; } diff --git a/src/SupraLib/utilities/RosWrapper.h b/src/SupraLib/utilities/RosWrapper.h index 516eb8b..e2f86a8 100644 --- a/src/SupraLib/utilities/RosWrapper.h +++ b/src/SupraLib/utilities/RosWrapper.h @@ -75,6 +75,30 @@ namespace supra return m_rosNode; } +#ifdef ROS_ROSSERIAL + /// Subscribe to a topic. Follows ROS conventions + template + void subscribe( + const std::string & topic, + void(*fp)(const M &)) + { + m_subscriber = std::unique_ptr >( + new ros::Subscriber (topic.c_str(), fp) + ); + m_rosNode->subscribe(*m_subscriber); + } +#else + /// Subscribe to a topic. Follows ROS conventions + template + void subscribe( + const std::string & topic, + void(*fp)(const M &)) + { + m_subscriber = std::unique_ptr(new ros::Subscriber()); + *m_subscriber = m_rosNode->subscribe(topic, 1, fp); + } +#endif + /// Subscribe to a topic. Follows ROS conventions template void subscribe(