Go to the documentation of this file.00001 #include "ros/ros.h"
00002 #include "crazyflie_driver/AddCrazyflie.h"
00003 #include "crazyflie_driver/LogBlock.h"
00004
00005 int main(int argc, char **argv)
00006 {
00007 ros::init(argc, argv, "crazyflie_add", ros::init_options::AnonymousName);
00008 ros::NodeHandle n("~");
00009
00010
00011 std::string uri;
00012 std::string tf_prefix;
00013 double roll_trim;
00014 double pitch_trim;
00015 bool enable_logging;
00016 bool enable_parameters;
00017 bool use_ros_time;
00018 bool enable_logging_imu;
00019 bool enable_logging_temperature;
00020 bool enable_logging_magnetic_field;
00021 bool enable_logging_pressure;
00022 bool enable_logging_battery;
00023 bool enable_logging_packets;
00024
00025 n.getParam("uri", uri);
00026 n.getParam("tf_prefix", tf_prefix);
00027 n.param("roll_trim", roll_trim, 0.0);
00028 n.param("pitch_trim", pitch_trim, 0.0);
00029 n.param("enable_logging", enable_logging, true);
00030 n.param("enable_parameters", enable_parameters, true);
00031 n.param("use_ros_time", use_ros_time, true);
00032 n.param("enable_logging_imu", enable_logging_imu, true);
00033 n.param("enable_logging_temperature", enable_logging_temperature, true);
00034 n.param("enable_logging_magnetic_field", enable_logging_magnetic_field, true);
00035 n.param("enable_logging_pressure", enable_logging_pressure, true);
00036 n.param("enable_logging_battery", enable_logging_battery, true);
00037 n.param("enable_logging_packets", enable_logging_packets, true);
00038
00039
00040 ROS_INFO("wait_for_service /add_crazyflie");
00041 ros::ServiceClient addCrazyflieService = n.serviceClient<crazyflie_driver::AddCrazyflie>("/add_crazyflie");
00042 addCrazyflieService.waitForExistence();
00043 ROS_INFO("found /add_crazyflie");
00044 crazyflie_driver::AddCrazyflie addCrazyflie;
00045 addCrazyflie.request.uri = uri;
00046 addCrazyflie.request.tf_prefix = tf_prefix;
00047 addCrazyflie.request.roll_trim = roll_trim;
00048 addCrazyflie.request.pitch_trim = pitch_trim;
00049 addCrazyflie.request.enable_logging = enable_logging;
00050 addCrazyflie.request.enable_parameters = enable_parameters;
00051 addCrazyflie.request.use_ros_time = use_ros_time;
00052 addCrazyflie.request.enable_logging_imu = enable_logging_imu;
00053 addCrazyflie.request.enable_logging_temperature = enable_logging_temperature;
00054 addCrazyflie.request.enable_logging_magnetic_field = enable_logging_magnetic_field;
00055 addCrazyflie.request.enable_logging_pressure = enable_logging_pressure;
00056 addCrazyflie.request.enable_logging_battery = enable_logging_battery;
00057 addCrazyflie.request.enable_logging_packets = enable_logging_packets;
00058
00059 std::vector<std::string> genericLogTopics;
00060 n.param("genericLogTopics", genericLogTopics, std::vector<std::string>());
00061 std::vector<int> genericLogTopicFrequencies;
00062 n.param("genericLogTopicFrequencies", genericLogTopicFrequencies, std::vector<int>());
00063
00064 if (genericLogTopics.size() == genericLogTopicFrequencies.size())
00065 {
00066 size_t i = 0;
00067 for (auto& topic : genericLogTopics)
00068 {
00069 crazyflie_driver::LogBlock logBlock;
00070 logBlock.topic_name = topic;
00071 logBlock.frequency = genericLogTopicFrequencies[i];
00072 n.getParam("genericLogTopic_" + topic + "_Variables", logBlock.variables);
00073 addCrazyflie.request.log_blocks.push_back(logBlock);
00074 ++i;
00075 }
00076 }
00077 else
00078 {
00079 ROS_ERROR("Cardinality of genericLogTopics and genericLogTopicFrequencies does not match!");
00080 }
00081
00082
00083 addCrazyflieService.call(addCrazyflie);
00084
00085 return 0;
00086 }