Unverified Commit 10596ad2 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub

Merge pull request #1 from ksyy/master

Changed namespace rosSubscribe to rosQueuedSubscribe
parents cf2d21a3 4290b807
import rospy
from std_srvs.srv import Trigger, TriggerResponse, SetBool, SetBoolResponse
from sot_hpp_msgs.srv import PlugSot, PlugSotResponse, SetString, SetJointNames
from std_srvs.srv import Trigger, TriggerResponse, SetBool, SetBoolResponse, Empty, EmptyResponse
from sot_hpp_msgs.srv import PlugSot, PlugSotResponse, SetString, SetJointNames, GetInt, GetIntResponse, SetInt, SetIntRequest, SetIntResponse
from dynamic_graph_bridge_msgs.srv import RunCommand
class RosInterface(object):
......@@ -10,7 +10,8 @@ class RosInterface(object):
rospy.Service('/sot/run_pre_action', PlugSot, self.runPreAction)
rospy.Service('/sot/request_hpp_topics', Trigger, self.requestHppTopics)
rospy.Service('/sot/clear_queues', Trigger, self.clearQueues)
rospy.Service('/sot/read_queue', SetBool, self.readQueue)
rospy.Service('/sot/read_queue', SetInt, self.readQueue)
rospy.Service('/sot/stop_reading_queue', Empty, self.stopReadingQueue)
self.runCommand = rospy.ServiceProxy ('/run_command', RunCommand)
self.supervisor = supervisor
......@@ -98,13 +99,23 @@ class RosInterface(object):
def readQueue(self, req):
if self.supervisor is not None:
self.supervisor.readQueue(req.data)
self.supervisor.readQueue( req.data)
else:
cmd = "supervisor.readQueue(" + str(req.data) + ")"
answer = self.runCommand (cmd)
print cmd
print answer
return SetBoolResponse (True, "ok")
return SetIntResponse ()
def stopReadingQueue(self, req):
if self.supervisor is not None:
self.supervisor.stopReadingQueue ()
else:
cmd = "supervisor.stopReadingQueue()"
answer = self.runCommand (cmd)
print cmd
print answer
return EmptyResponse ()
def requestHppTopics(self, req):
handlers = {
......
......@@ -222,8 +222,14 @@ class Supervisor(object):
self.rosexport.clearQueue(s)
def readQueue(self, read):
print "Read queues:", read
self.rosexport.readQueue (read)
if read < 0:
print "ReadQueue argument should be >= 0"
return
t = self.sotrobot.device.control.time
self.rosexport.readQueue (t + read)
def stopReadingQueue(self):
self.rosexport.readQueue (-1)
def plugSot(self, id, check = False):
if check and not self.isSotConsistentWithCurrent (id):
......
......@@ -19,7 +19,7 @@ namespace dynamicgraph
namespace command
{
namespace rosSubscribe
namespace rosQueuedSubscribe
{
Clear::Clear
(RosQueuedSubscribe& entity, const std::string& docstring)
......@@ -152,7 +152,7 @@ namespace dynamicgraph
(RosQueuedSubscribe& entity, const std::string& docstring)
: Command
(entity,
boost::assign::list_of (Value::BOOL),
boost::assign::list_of (Value::INT),
docstring)
{}
......@@ -162,7 +162,7 @@ namespace dynamicgraph
static_cast<RosQueuedSubscribe&> (owner ());
std::vector<Value> values = getParameterValues ();
bool read = values[0].value ();
int read = values[0].value ();
entity.readQueue (read);
return Value ();
......@@ -179,7 +179,7 @@ namespace dynamicgraph
: dynamicgraph::Entity(n),
nh_ (rosInit (true)),
bindedSignal_ (),
readQueue_ (false)
readQueue_ (-1)
{
std::string docstring =
"\n"
......@@ -192,7 +192,7 @@ namespace dynamicgraph
" - topic: the topic name in ROS.\n"
"\n";
addCommand ("add",
new command::rosSubscribe::Add
new command::rosQueuedSubscribe::Add
(*this, docstring));
docstring =
"\n"
......@@ -202,7 +202,7 @@ namespace dynamicgraph
" - name of the signal to remove (see method list for the list of signals).\n"
"\n";
addCommand ("rm",
new command::rosSubscribe::Rm
new command::rosQueuedSubscribe::Rm
(*this, docstring));
docstring =
"\n"
......@@ -211,7 +211,7 @@ namespace dynamicgraph
" No input:\n"
"\n";
addCommand ("clear",
new command::rosSubscribe::Clear
new command::rosQueuedSubscribe::Clear
(*this, docstring));
docstring =
"\n"
......@@ -220,7 +220,7 @@ namespace dynamicgraph
" No input:\n"
"\n";
addCommand ("list",
new command::rosSubscribe::List
new command::rosQueuedSubscribe::List
(*this, docstring));
docstring =
"\n"
......@@ -230,7 +230,7 @@ namespace dynamicgraph
" - name of the signal (see method list for the list of signals).\n"
"\n";
addCommand ("clearQueue",
new command::rosSubscribe::ClearQueue
new command::rosQueuedSubscribe::ClearQueue
(*this, docstring));
docstring =
"\n"
......@@ -240,17 +240,17 @@ namespace dynamicgraph
" - name of the signal (see method list for the list of signals).\n"
"\n";
addCommand ("queueSize",
new command::rosSubscribe::QueueSize
new command::rosQueuedSubscribe::QueueSize
(*this, docstring));
docstring =
"\n"
" Whether signals should read values from the queues.\n"
" Whether signals should read values from the queues, and when.\n"
"\n"
" Input is:\n"
" - boolean.\n"
" - int (dynamic graph time at which the reading begin).\n"
"\n";
addCommand ("readQueue",
new command::rosSubscribe::ReadQueue
new command::rosQueuedSubscribe::ReadQueue
(*this, docstring));
}
......@@ -318,15 +318,14 @@ namespace dynamicgraph
return -1;
}
void RosQueuedSubscribe::readQueue (bool read)
void RosQueuedSubscribe::readQueue (int beginReadingAt)
{
for (std::map<std::string, bindedSignal_t>::const_iterator it =
// Prints signal queues sizes
/*for (std::map<std::string, bindedSignal_t>::const_iterator it =
bindedSignal_.begin (); it != bindedSignal_.end (); it++) {
std::cout << it->first << " : " << it->second->size() << '\n';
}
std::cout << std::endl;
// TODO ensure that the signal are synchronised
readQueue_ = read;
}*/
readQueue_ = beginReadingAt;
}
std::string RosQueuedSubscribe::getDocString () const
......
......@@ -23,12 +23,12 @@ namespace dynamicgraph
namespace command
{
namespace rosSubscribe
namespace rosQueuedSubscribe
{
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
# define ROS_SUBSCRIBE_MAKE_COMMAND(CMD) \
# define ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(CMD) \
class CMD : public Command \
{ \
public: \
......@@ -37,17 +37,17 @@ namespace dynamicgraph
virtual Value doExecute (); \
}
ROS_SUBSCRIBE_MAKE_COMMAND(Add);
ROS_SUBSCRIBE_MAKE_COMMAND(Clear);
ROS_SUBSCRIBE_MAKE_COMMAND(List);
ROS_SUBSCRIBE_MAKE_COMMAND(Rm);
ROS_SUBSCRIBE_MAKE_COMMAND(ClearQueue);
ROS_SUBSCRIBE_MAKE_COMMAND(QueueSize);
ROS_SUBSCRIBE_MAKE_COMMAND(ReadQueue);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Add);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Clear);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(List);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Rm);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(ClearQueue);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(QueueSize);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(ReadQueue);
#undef ROS_SUBSCRIBE_MAKE_COMMAND
#undef ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND
} // end of namespace errorEstimator.
} // end of namespace rosQueuedSubscribe.
} // end of namespace command.
class RosQueuedSubscribe;
......@@ -128,7 +128,7 @@ namespace dynamicgraph
std::string list ();
void clear ();
void clearQueue (const std::string& signal);
void readQueue (bool read);
void readQueue (int beginReadingAt);
std::size_t queueSize (const std::string& signal) const;
template <typename T>
......@@ -162,7 +162,7 @@ namespace dynamicgraph
ros::NodeHandle& nh_;
std::map<std::string, bindedSignal_t> bindedSignal_;
bool readQueue_;
int readQueue_;
// Signal<bool, int> readQueue_;
template <typename T>
......
......@@ -47,7 +47,7 @@ namespace dynamicgraph
bs->subscriber =
boost::make_shared<ros::Subscriber>
(rosSubscribe.nh ().subscribe (topic, 1, callback));
(rosSubscribe.nh ().subscribe (topic, 50, callback)); // Keep 50 messages in queue, but only 20 are sent every 100ms
RosQueuedSubscribe::bindedSignal_t bindedSignal (bs);
rosSubscribe.bindedSignal ()[signal] = bindedSignal;
......@@ -73,7 +73,7 @@ namespace dynamicgraph
template <typename T>
T& BindedSignal<T>::reader (T& data, int time)
{
if (!entity->readQueue_) {
if (entity->readQueue_ == -1 || time < entity->readQueue_) {
data = last;
} else {
qmutex.lock();
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment