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();
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, "ekf_localization_node");
ros::NodeHandle node;
ros::NodeHandle priv_node("~");
if(argc != 2)
std::cout << "No Input Image!" << std::endl;
double spin_rate;
priv_node.param("spin",spin_rate, 20.0);
EKFnode k(node,spin_rate);
ros::spin();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "boost_coarse_gps_odometry_node");
// Use message_filters to combine and sync two topics
ros::NodeHandle nh;
ros::NodeHandle priv_node("~");
priv_node.param<std::string>("frame_id", frame_id, "odom/coarse_gps");
priv_node.param<std::string>("child_frame_id", child_frame_id, "base_link");
priv_node.param<bool>("publish_tf", publish_tf, true);
message_filters::Subscriber<nav_msgs::Odometry> utm_sub(nh, "odometry/utm", 10);
// message_filters::Subscriber<nav_msgs::Odometry> orient_sub(nh, "odometry/filtered_imu_encoders", 10);
message_filters::Subscriber<sensor_msgs::Imu> orient_sub(nh, "gps/imu/data", 10);
// typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry, nav_msgs::Odometry> MySyncPolicy;
typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry, sensor_msgs::Imu> MySyncPolicy;
// ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), utm_sub, orient_sub);
sync.registerCallback(boost::bind(&coarseOdomCallback, _1, _2));
odom_pub = nh.advertise<nav_msgs::Odometry>("odometry/coarse_gps", 1, false);
tf::TransformBroadcaster odom_broadcaster;
ros::Time last_time = ros::Time::now();
ros::Rate r(20.0);
while (ros::ok())
{
last_time = odom.header.stamp;
ros::spinOnce();
// Check for not same topic
if (last_time != odom.header.stamp)
{
// perform subscribed callback again and wait until it has to publish again
// ROS_INFO_STREAM("Publishing odom!");
odom_pub.publish(odom);
if (publish_tf)
{
// ROS_INFO_STREAM("Publishing tf!");
odom_broadcaster.sendTransform(odom_tf);
}
}
else
{
ROS_INFO_STREAM("Same odometry/utm topic as before!");
}
// ros::spinOnce();
r.sleep();
}
return 0;
}
