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_pose;
00024 bool enable_logging_packets;
00025
00026 n.getParam("uri", uri);
00027 n.getParam("tf_prefix", tf_prefix);
00028 n.param("roll_trim", roll_trim, 0.0);
00029 n.param("pitch_trim", pitch_trim, 0.0);
00030 n.param("enable_logging", enable_logging, true);
00031 n.param("enable_parameters", enable_parameters, true);
00032 n.param("use_ros_time", use_ros_time, true);
00033 n.param("enable_logging_imu", enable_logging_imu, true);
00034 n.param("enable_logging_temperature", enable_logging_temperature, true);
00035 n.param("enable_logging_magnetic_field", enable_logging_magnetic_field, true);
00036 n.param("enable_logging_pressure", enable_logging_pressure, true);
00037 n.param("enable_logging_battery", enable_logging_battery, true);
00038 n.param("enable_logging_pose", enable_logging_pose, false);
00039 n.param("enable_logging_packets", enable_logging_packets, true);
00040
00041
00042 ROS_INFO("wait_for_service /add_crazyflie");
00043 ros::ServiceClient addCrazyflieService = n.serviceClient<crazyflie_driver::AddCrazyflie>("/add_crazyflie");
00044 addCrazyflieService.waitForExistence();
00045 ROS_INFO("found /add_crazyflie");
00046 crazyflie_driver::AddCrazyflie addCrazyflie;
00047 addCrazyflie.request.uri = uri;
00048 addCrazyflie.request.tf_prefix = tf_prefix;
00049 addCrazyflie.request.roll_trim = roll_trim;
00050 addCrazyflie.request.pitch_trim = pitch_trim;
00051 addCrazyflie.request.enable_logging = enable_logging;
00052 addCrazyflie.request.enable_parameters = enable_parameters;
00053 addCrazyflie.request.use_ros_time = use_ros_time;
00054 addCrazyflie.request.enable_logging_imu = enable_logging_imu;
00055 addCrazyflie.request.enable_logging_temperature = enable_logging_temperature;
00056 addCrazyflie.request.enable_logging_magnetic_field = enable_logging_magnetic_field;
00057 addCrazyflie.request.enable_logging_pressure = enable_logging_pressure;
00058 addCrazyflie.request.enable_logging_battery = enable_logging_battery;
00059 addCrazyflie.request.enable_logging_pose = enable_logging_pose;
00060 addCrazyflie.request.enable_logging_packets = enable_logging_packets;
00061
00062 std::vector<std::string> genericLogTopics;
00063 n.param("genericLogTopics", genericLogTopics, std::vector<std::string>());
00064 std::vector<int> genericLogTopicFrequencies;
00065 n.param("genericLogTopicFrequencies", genericLogTopicFrequencies, std::vector<int>());
00066
00067 if (genericLogTopics.size() == genericLogTopicFrequencies.size())
00068 {
00069 size_t i = 0;
00070 for (auto& topic : genericLogTopics)
00071 {
00072 crazyflie_driver::LogBlock logBlock;
00073 logBlock.topic_name = topic;
00074 logBlock.frequency = genericLogTopicFrequencies[i];
00075 n.getParam("genericLogTopic_" + topic + "_Variables", logBlock.variables);
00076 addCrazyflie.request.log_blocks.push_back(logBlock);
00077 ++i;
00078 }
00079 }
00080 else
00081 {
00082 ROS_ERROR("Cardinality of genericLogTopics and genericLogTopicFrequencies does not match!");
00083 }
00084
00085
00086 addCrazyflieService.call(addCrazyflie);
00087
00088 return 0;
00089 }