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; }