Hi Ken,
I apologize that my earlier email was not clear. I have attached the two
source files again - "test_policy_synchronizer_publisher.cpp" implements
a simple node that publishes two messages at different frequencies and
"test_policy_synchronizer.cpp" uses the approximate time based
synchronizer. I did not comment out any code in the core ROS libraries.
I commented out a couple of lines in my publisher node that incremented
the sequence number before publishing messages (lines 40 and 50 in
test_policy_synchronizer_publisher.cpp). So now no sequence number is
manipulated in the publisher node. When received messages are filtered
through the synchronizer, I see that the sequence numbers in their
headers are set appropriately. Following is the log of a run:
Publisher log:
[ INFO] [1287423975.468743146]: Starting
test_policy_synchronizer_publisher node.
[ INFO] [1287423975.476386994]: Publishing AHRS data: time =
1287423975.476339, seq = 0
[ INFO] [1287423976.476551394]: Publishing AHRS data: time =
1287423976.476421, seq = 0
[ INFO] [1287423976.476866797]: Publishing Alasca data: time =
1287423976.476793, seq = 0
[ INFO] [1287423977.476562926]: Publishing AHRS data: time =
1287423977.476435, seq = 0
[ INFO] [1287423978.476785581]: Publishing AHRS data: time =
1287423978.476639, seq = 0
[ INFO] [1287423978.477132762]: Publishing Alasca data: time =
1287423978.477045, seq = 0
Synchronizer log:
[ INFO] [1287423977.477327968]: ----------
[ INFO] [1287423977.477628006]: Received message set
[ INFO] [1287423977.477812526]: AHRS: timestamp = 1287423976.476421, seq = 1
[ INFO] [1287423977.477975048]: Alasca: timestamp = 1287423976.476793,
seq = 0
[ INFO] [1287423979.477320422]: ----------
[ INFO] [1287423979.477590568]: Received message set
[ INFO] [1287423979.477731158]: AHRS: timestamp = 1287423978.476639, seq = 3
[ INFO] [1287423979.477852403]: Alasca: timestamp = 1287423978.477045,
seq = 1
I now understand that I should not modify/use the sequence field;
however I am curious about knowing how people have used the sequence
field in the past.
I really appreciate your help.
Thanks,
- Aditya
On 10/18/2010 01:32 PM, Ken Conley wrote:
> Hi Aditya,
>
> The subscriber does not modify the sequence number. There's not enough
> detail in your response as to what code you're referring to expand o
> that.
>
> In general, you shouldn't be using the seq field, and commenting out
> code that manipulates it will cause problems for others who wish to
> use your libraries as you are modifying the core ROS libraries.
>
> HTH,
> Ken
>
> On Mon, Oct 18, 2010 at 10:03 AM, Aditya Gadre<agadre@vt.edu> wrote:
>
>> Hi Ken,
>>
>> Thank you very much for your email. That explains the discrepancy.
>> However, when I comment out the code that increments the sequence
>> number, it stays 0. Does this mean the the sequence number is actually
>> only usable on the subscriber side where it gets set to appropriate value?
>>
>> Thanks,
>> - Aditya
>>
>> On 10/18/2010 12:57 PM, Ken Conley wrote:
>>
>>> Hi Aditya,
>>>
>>> The sequence number is not a user-settable field. We hope to make it
>>> user-settable in the future, but in our previous attempt to do so, the
>>> feedback from ros-users was not to.
>>>
>>> HTH,
>>> Ken
>>>
>>> On Mon, Oct 18, 2010 at 9:28 AM, Aditya Gadre<agadre@vt.edu> wrote:
>>>
>>>
>>>> I have a working test code for policy based (approximate time) synchronizer
>>>> in which one node publishes two data messages and the other node runs a
>>>> synchronizer. One message is published at 1Hz and the other at 0.5Hz. For
>>>> each message, I increment the relevant sequence number in the header.
>>>> However, when these messages are received by the node running the policy
>>>> based synchronizer, the sequence numbers are off by 1, although the time
>>>> stamps match perfectly.
>>>>
>>>> For example, following output shows a simple run -
>>>>
>>>> Publisher log:
>>>> [ INFO] [1287418571.114515026]: Starting test_policy_synchronizer_publisher
>>>> node.
>>>> [ INFO] [1287418571.122496486]: Publishing AHRS data: time =
>>>> 1287418571.122447, seq = 1
>>>> [ INFO] [1287418572.122643642]: Publishing AHRS data: time =
>>>> 1287418572.122516, seq = 2
>>>> [ INFO] [1287418572.122956042]: Publishing Alasca data: time =
>>>> 1287418572.122883, seq = 1
>>>> [ INFO] [1287418573.122685477]: Publishing AHRS data: time =
>>>> 1287418573.122559, seq = 3
>>>> [ INFO] [1287418574.122935718]: Publishing AHRS data: time =
>>>> 1287418574.122809, seq = 4
>>>> [ INFO] [1287418574.123219204]: Publishing Alasca data: time =
>>>> 1287418574.123158, seq = 2
>>>> [ INFO] [1287418581.122753362]: Shutting down
>>>> test_policy_synchronizer_publisher node.
>>>>
>>>> Synchronizer node log:
>>>> [ INFO] [1287418573.123472169]: ----------
>>>> [ INFO] [1287418573.123744969]: Received message set
>>>> [ INFO] [1287418573.123947997]: AHRS: timestamp = 1287418572.122516, seq = 1
>>>> [ INFO] [1287418573.124106258]: Alasca: timestamp = 1287418572.122883, seq =
>>>> 0
>>>> [ INFO] [1287418575.122839197]: ----------
>>>> [ INFO] [1287418575.122900518]: Received message set
>>>> [ INFO] [1287418575.122951782]: AHRS: timestamp = 1287418574.122809, seq = 3
>>>> [ INFO] [1287418575.123003883]: Alasca: timestamp = 1287418574.123158, seq =
>>>> 1
>>>>
>>>> Comparing time stamps of sent and received messages, I see that the sequence
>>>> numbers in the synchronizer node are off by 1 (if sent message seq = 3 then
>>>> received message seq = 2). I am not sure why this discrepancy is popping up.
>>>> I would appreciate any help in this regard! I have attached source code for
>>>> both nodes.
>>>>
>>>> Thank you.
>>>> - Aditya
>>>>
>>>> _______________________________________________
>>>> ros-users mailing list
>>>> ros-users@code.ros.org
>>>> https://code.ros.org/mailman/listinfo/ros-users
>>>>
>>>>
>>>>
>>>>
>>> _______________________________________________
>>> ros-users mailing list
>>> ros-users@code.ros.org
>>> https://code.ros.org/mailman/listinfo/ros-users
>>>
>>>
>>>
>> _______________________________________________
>> ros-users mailing list
>> ros-users@code.ros.org
>> https://code.ros.org/mailman/listinfo/ros-users
>>
>>
> _______________________________________________
> ros-users mailing list
> ros-users@code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
>
>
/*
* test_policy_synchronizer.cpp
*
* Created on: Oct 17, 2010
* Author: agadre
*/
#include "riverine_msgs/AhrsEuler.h"
#include "riverine_msgs/AlascaScan.h"
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
void callback(const riverine_msgs::AhrsEuler::ConstPtr& ahrsEuler,
const riverine_msgs::AlascaScan::ConstPtr& alascaScan)
{
ROS_INFO_STREAM("----------");
ROS_INFO_STREAM("Received message set");
ROS_INFO_STREAM("AHRS: timestamp = " << std::fixed << ahrsEuler->header.stamp.toSec()
<< ", seq = " << ahrsEuler->header.seq);
ROS_INFO_STREAM("Alasca: timestamp = " << std::fixed << alascaScan->header.stamp.toSec()
<< ", seq = " << alascaScan->header.seq);
}
int main(int argc, char* argv[])
{
// init ros
ros::init(argc, argv, "test_policy_synchronizer_node");
ros::NodeHandle nh;
message_filters::Subscriber<riverine_msgs::AhrsEuler>
subAhrsEuler(nh, "ahrs_euler", 10);
message_filters::Subscriber<riverine_msgs::AlascaScan>
subAlascaScan(nh, "alasca_scan", 10);
typedef message_filters::sync_policies::ApproximateTime<riverine_msgs::AhrsEuler, riverine_msgs::AlascaScan>
MyApproxTimeSyncPolicy;
message_filters::Synchronizer<MyApproxTimeSyncPolicy> sync(MyApproxTimeSyncPolicy(10), subAhrsEuler, subAlascaScan);
sync.registerCallback(boost::bind(&callback, _1, _2));
ros::spin();
// done
return 0;
}
/*
* test_policy_synchronizer.cpp
*
* Created on: Oct 17, 2010
* Author: agadre
*/
#include <ros/ros.h>
#include "riverine_msgs/AhrsEuler.h"
#include "riverine_msgs/AlascaScan.h"
int main(int argc, char* argv[])
{
////////////////////
// Initialize ROS
ROS_INFO_STREAM("Starting test_policy_synchronizer_publisher node.");
ros::init(argc, argv, "test_policy_synchronizer_publisher_node");
// Initialize communications
ros::NodeHandle nodeHandle;
// Create publisher
ros::Publisher pubAhrsEuler =
nodeHandle.advertise<riverine_msgs::AhrsEuler>("ahrs_euler", 1);
ros::Publisher pubAlascaScan =
nodeHandle.advertise<riverine_msgs::AlascaScan>("alasca_scan", 1);
// Poll rate
ros::Rate pollRate(1);
////////////////////
// Run main loop
riverine_msgs::AhrsEuler msgAhrsEuler;
riverine_msgs::AlascaScan msgAlascaScan;
bool publishAlasca(false);
while (ros::ok())
{
// publish ahrs
msgAhrsEuler.header.stamp = ros::Time::now();
// ++msgAhrsEuler.header.seq;
ROS_INFO_STREAM("Publishing AHRS data: time = "
<< std::fixed << msgAhrsEuler.header.stamp.toSec()
<< ", seq = "<< msgAhrsEuler.header.seq);
pubAhrsEuler.publish(msgAhrsEuler);
// publish alasca
if(publishAlasca)
{
msgAlascaScan.header.stamp = ros::Time::now();
// ++msgAlascaScan.header.seq;
ROS_INFO_STREAM("Publishing Alasca data: time = "
<< std::fixed << msgAlascaScan.header.stamp.toSec()
<< ", seq = "<< msgAlascaScan.header.seq);
pubAlascaScan.publish(msgAlascaScan);
}
publishAlasca = !publishAlasca;
// Run event loop
ros::spinOnce();
// Make sure we try to maintain the requested poll rate
pollRate.sleep();
}
ROS_INFO_STREAM("Shutting down test_policy_synchronizer_publisher node.");
////////////////////
// done
return 0;
}