int TonavRos::run(int argc, char* argv[]) { ROS_INFO_STREAM("Tonav ROS - [email protected] \n"); ros::init(argc, argv, "tonav_ros"); if (!parseCommandLineParams(argc, argv)) { printHelp(); return 1; } if (variables_map_.count("help")) { printHelp(); return 1; } ros::NodeHandle node_handle; std::string imu_topic = variables_map_["imu"].as<std::string>(); std::string camera_topic = variables_map_["image"].as<std::string>(); std::string camerainfo_topic = variables_map_["camerainfo"].as<std::string>(); image_transport::ImageTransport transport(node_handle); image_transport::Subscriber camera_subscriber = transport.subscribe(camera_topic, 5, &TonavRos::cameraCallback, this); ros::Subscriber camerainfo_subscriber = node_handle.subscribe(camerainfo_topic, 50, &TonavRos::cameraInfoCallback, this); ros::Subscriber imu_subscriber = node_handle.subscribe(imu_topic, 50, &TonavRos::imuCallback, this); ros::spin(); return 0; }
int TonavRos::run(int argc, char* argv[]) { ROS_INFO_STREAM("Tonav ROS - [email protected] \n"); ros::init(argc, argv, "tonav_ros"); if (!parseCommandLineParams(argc, argv)) { printHelp(); return 1; } if (variables_map_.count("help")) { printHelp(); return 1; } ros::NodeHandle node_handle; tf_broadcaster_.reset(new tf2_ros::TransformBroadcaster); std::string imu_topic = variables_map_["imu"].as<std::string>(); std::string camera_topic = variables_map_["image"].as<std::string>(); std::string camerainfo_topic = variables_map_["camerainfo"].as<std::string>(); std::string calibration_file = variables_map_["calibration"].as<std::string>(); image_transport::ImageTransport transport(node_handle); image_transport::Subscriber camera_subscriber = transport.subscribe(camera_topic, 5, &TonavRos::cameraCallback, this); ros::Subscriber camerainfo_subscriber = node_handle.subscribe(camerainfo_topic, 50, &TonavRos::cameraInfoCallback, this); ros::Subscriber imu_subscriber = node_handle.subscribe(imu_topic, 50, &TonavRos::imuCallback, this); image_publisher_.reset(new image_transport::Publisher(transport.advertise("tonav/image", 5))); Calibration calibration = Calibration::fromPath(calibration_file); tonav_.reset(new Tonav(calibration)); ros::spin(); return 0; }
int main(int argc, const char * argv[]) { bool status = parseCommandLineParams(argc, argv); if (!status) return EXIT_FAILURE; ImageAnnotator annotator; annotator.labelImagesInDirectory(inputdir, csvfile, outputdir); }