Skip to content

Commit

Permalink
Ros: Fixed subscribing
Browse files Browse the repository at this point in the history
  • Loading branch information
@goeblr
goeblr committed Feb 7, 2018
1 parent 463b7ac commit b8a7da0
Show file tree
Hide file tree
Showing 5 changed files with 130 additions and 7 deletions.
4 changes: 2 additions & 2 deletions rosserial_libs/ros_lib/ros/node_handle.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -348,8 +348,8 @@ namespace ros {
}

/* Register a new subscriber */
template<typename MsgT>
bool subscribe(Subscriber< MsgT> & s){
template<typename SubscriberT>
bool subscribe(SubscriberT & s){
for(int i = 0; i < MAX_SUBSCRIBERS; i++){
if(subscribers[i] == 0){ // empty slot
subscribers[i] = (Subscriber_*) &s;
Expand Down
35 changes: 34 additions & 1 deletion rosserial_libs/ros_lib/ros/subscriber.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,10 +54,43 @@ namespace ros {
const char * topic_;
};

/* Bound function subscriber. */
template<typename MsgT, typename ObjT = void>
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<typename MsgT>
class Subscriber: public Subscriber_{
class Subscriber<MsgT, void> : public Subscriber_
{
public:
typedef void(*CallbackT)(const MsgT&);
MsgT msg;
Expand Down
66 changes: 62 additions & 4 deletions src/RosInterface/RosInterface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,10 +11,12 @@

#include "RosInterface.h"

#include <utilities/RosWrapper.h>
#include <supra_msgs/get_nodes.h>
#include <supra_msgs/get_node_parameters.h>
#include <supra_msgs/set_node_parameter.h>
//#include <utilities/RosWrapper.h>
//#include <supra_msgs/get_nodes.h>
//#include <supra_msgs/get_node_parameters.h>
//#include <supra_msgs/set_node_parameter.h>
//#include <supra_msgs/freeze.h>
//#include <supra_msgs/sequence.h>

#include <iostream>
#include <memory>
Expand Down Expand Up @@ -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);
Expand All @@ -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<supra_msgs::sequenceRequest, supra_msgs::sequenceResponse> servSequence
(supra_msgs::SEQUENCE, (RosInterface::sequenceCallback));
bool sequenceAdvertisement = wr.getNodeHandle()->advertiseService(servSequence);
logging::log_error_if(!sequenceAdvertisement, "Error advertising service ", "sequence");
wr.subscribe<supra_msgs::freeze, RosInterface>("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_msgs::freeze>("supra_freeze", RosInterface::freezeCallback);
#endif

wr.spinEndless();
Expand Down Expand Up @@ -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))
Expand Down Expand Up @@ -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");
}
}
}
8 changes: 8 additions & 0 deletions src/RosInterface/RosInterface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,8 @@
#include <supra_msgs/get_nodes.h>
#include <supra_msgs/get_node_parameters.h>
#include <supra_msgs/set_node_parameter.h>
#include <supra_msgs/freeze.h>
#include <supra_msgs/sequence.h>

namespace supra
{
Expand Down Expand Up @@ -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);
Expand All @@ -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 <typename ValueType>
static void fillParameterMessage(const ConfigurationDictionary* confDict, const ValueRangeDictionary * ranges, std::string paramName, supra_msgs::parameter* pParam)
Expand Down Expand Up @@ -94,6 +100,8 @@ namespace supra
}
}
}

static bool m_sequenceActive;
};
}

Expand Down
24 changes: 24 additions & 0 deletions src/SupraLib/utilities/RosWrapper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,30 @@ namespace supra
return m_rosNode;
}

#ifdef ROS_ROSSERIAL
/// Subscribe to a topic. Follows ROS conventions
template <class M, class T>
void subscribe(
const std::string & topic,
void(*fp)(const M &))
{
m_subscriber = std::unique_ptr<ros::Subscriber < M, void> >(
new ros::Subscriber <M, void>(topic.c_str(), fp)
);
m_rosNode->subscribe(*m_subscriber);
}
#else
/// Subscribe to a topic. Follows ROS conventions
template <class M>
void subscribe(
const std::string & topic,
void(*fp)(const M &))
{
m_subscriber = std::unique_ptr<ros::Subscriber>(new ros::Subscriber());
*m_subscriber = m_rosNode->subscribe(topic, 1, fp);
}
#endif

/// Subscribe to a topic. Follows ROS conventions
template <class M, class T>
void subscribe(
Expand Down

0 comments on commit b8a7da0

Please sign in to comment.