int main(int argc, char **argv)
{
//ROS declaration
ros::init(argc, argv, "motorGo");
ros::NodeHandle my_node;
ros::Rate loop_rate(10);
ros::NodeHandle priv_node("~");
Robot platform(priv_node);
Scribe scribe(my_node, "cmd_vel", platform);
Poete poete(my_node, "odom");
//Lifter lift(my_node, priv_node);
Filter filt;
tf::TransformBroadcaster odom_broadcaster;
nav_msgs::Odometry nav;
int flag=0;
//Creation de Platform et test de verbose option
if ( argc > 1 ) {// argc should be 2 for correct execution
for(int i =0;i<argc;i++&&flag==0){
if (strcmp(argv[i],"--verbose")&&flag==0){
std::cout<<"VERBOSE MODE ON"<<std::endl;
scribe.setVerbose();
poete.setVerbose();
platform.setVerbose();
flag=1;
}
}
}
while(ros::ok()){
platform.odometry();
//if we read correctly
if(platform.getControl().getReadState()){
nav=platform.getOdom();
if(platform.getControl().getReadState()==true){
filt.add(nav);
nav=filt.filtering();
poete.publish(nav);
publishTransform(nav, odom_broadcaster);
}
}
ros::spinOnce();
loop_rate.sleep();
}
ros::spin();
}
TEST(SimpleFilter, callbackTypes)
{
Helper h;
Filter f;
f.registerCallback(boost::bind(&Helper::cb0, &h, _1));
f.registerCallback<const Msg&>(boost::bind(&Helper::cb1, &h, _1));
f.registerCallback<MsgConstPtr>(boost::bind(&Helper::cb2, &h, _1));
f.registerCallback<const ros::MessageEvent<Msg const>&>(boost::bind(&Helper::cb3, &h, _1));
f.registerCallback<Msg>(boost::bind(&Helper::cb4, &h, _1));
f.registerCallback<const MsgPtr&>(boost::bind(&Helper::cb5, &h, _1));
f.registerCallback<MsgPtr>(boost::bind(&Helper::cb6, &h, _1));
f.registerCallback<const ros::MessageEvent<Msg>&>(boost::bind(&Helper::cb7, &h, _1));
f.add(Filter::EventType(boost::make_shared<Msg>()));
EXPECT_EQ(h.counts_[0], 1);
EXPECT_EQ(h.counts_[1], 1);
EXPECT_EQ(h.counts_[2], 1);
EXPECT_EQ(h.counts_[3], 1);
EXPECT_EQ(h.counts_[4], 1);
EXPECT_EQ(h.counts_[5], 1);
EXPECT_EQ(h.counts_[6], 1);
EXPECT_EQ(h.counts_[7], 1);
}