Commit 3f141528 authored by Alexis Nicolin's avatar Alexis Nicolin

ReadQueue now uses the dynamic_graph time to begin unqueueing the information received.

parent 1f9b43c5
import rospy
from std_srvs.srv import Trigger, TriggerResponse, SetBool, SetBoolResponse
from sot_hpp_msgs.srv import PlugSot, PlugSotResponse, SetString, SetJointNames
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/get_sot_time', GetInt, self.getSoTTime)
self.runCommand = rospy.ServiceProxy ('/run_command', RunCommand)
self.supervisor = supervisor
......@@ -104,7 +105,17 @@ class RosInterface(object):
answer = self.runCommand (cmd)
print cmd
print answer
return SetBoolResponse (True, "ok")
return SetIntResponse ()
def getSoTTime(self, req):
if self.supervisor is not None:
return GetIntResponse (self.supervisor.sotrobot.device.control.time)
else:
cmd = "supervisor.sotrobot.device.control.time"
answer = self.runCommand (cmd)
print cmd
print answer
return GetIntResponse (int(answer.result))
def requestHppTopics(self, req):
handlers = {
......
......@@ -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"
......@@ -244,10 +244,10 @@ namespace dynamicgraph
(*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::rosQueuedSubscribe::ReadQueue
......@@ -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
......
......@@ -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