multi_strategy.cpp
Go to the documentation of this file.
00001 /*
00002  *  This file is part of ucl_drone 2016.
00003  *  For more information, refer
00004  *  to the corresponding header file.
00005  *
00006  *  \author Arnaud Jacques & Alexandre Leclere
00007  *  \date 2016
00008  *
00009  */
00010 
00011 #include <ucl_drone/multi_strategy.h>
00012 
00013 MultiStrategy::MultiStrategy()
00014 {
00015   // Subscriber
00016   ready_channel = nh_.resolveName("ready");  // check which drones are ready
00017   ready_sub = nh_.subscribe(ready_channel, 1, &MultiStrategy::readyCb, this);
00018 
00019   // Publishers
00020   drones_roles_channel = nh_.resolveName("drones_roles");  // broadcast a list of roles
00021   drones_roles_pub = nh_.advertise< ucl_drone::DroneRoles >(drones_roles_channel, 1);
00022 }
00023 
00024 MultiStrategy::~MultiStrategy()
00025 {
00026 }
00027 
00028 void MultiStrategy::init()
00029 {
00030   // TODO: use the vector filled in readyCb
00031   // TODO: add a launch parameter to choose between several multi drone missions
00032 
00033   // Hardcoded:
00034   DroneRole role1("ucl_drone_5");
00035   role1.SetDroneRole(EXPLORE_UNTIL_TARGET);
00036   role_list.push_back(role1);
00037   DroneRole role2("ucl_drone_4");
00038   role2.SetDroneRole(GO_TO, "/ucl_drone_5/target_detected/");
00039   role_list.push_back(role2);
00040 }
00041 
00042 void MultiStrategy::readyCb(const ucl_drone::DroneRole::ConstPtr readyPtr)
00043 {
00044   // TODO: Add to a std::vector the drone name contained in readyPtr
00045 }
00046 
00047 void MultiStrategy::PublishDroneRole()
00048 {
00049   ucl_drone::DroneRoles msg = DroneRole::DroneRolesToMsg(role_list);
00050   drones_roles_pub.publish(msg);
00051 }
00052 
00053 int main(int argc, char **argv)
00054 {
00055   ros::init(argc, argv, "multi_strategy");
00056 
00057   /*if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug))
00058   {
00059     ros::console::notifyLoggerLevelsChanged();
00060   }*/
00061 
00062   MultiStrategy my_custom_strategy;
00063   ROS_DEBUG("end of MultiStrategy initialization");
00064 
00065   ros::Rate r1(3);        // 3Hz
00066   ros::Rate r2(1 / 5.0);  // 1/5Hz
00067 
00068   my_custom_strategy.init();
00069   // ros::spin();
00070   ROS_DEBUG("entering loop 2");
00071   while (ros::ok())
00072   {
00073     TIC(multi);
00074     my_custom_strategy.PublishDroneRole();
00075     TOC(multi, "multi strategy");
00076     ros::spinOnce();
00077     r2.sleep();
00078     ROS_DEBUG("multistrategy has published drone role");
00079   }
00080 
00081   return 0;
00082 }


ucl_drone
Author(s): dronesinma
autogenerated on Sat Jun 8 2019 20:51:53