testbb.cpp
Go to the documentation of this file.
00001 
00023 #include "testbb/testbb.h"
00024 
00025 // Register the application
00026 PLUGINLIB_EXPORT_CLASS(testbb::TestBb, micros_swarm::Application)
00027 
00028 namespace testbb{
00029 
00030     TestBb::TestBb() {}
00031 
00032     TestBb::~TestBb() {}
00033 
00034     void TestBb::init() {}
00035 
00036     void TestBb::stop() {}
00037 
00038     void TestBb::baseCallback(const nav_msgs::Odometry& lmsg)
00039     {
00040         static int msg_count = 0;
00041         float x=lmsg.pose.pose.position.x;
00042         float y=lmsg.pose.pose.position.y;
00043 
00044         float vx=lmsg.twist.twist.linear.x;
00045         float vy=lmsg.twist.twist.linear.y;
00046 
00047         micros_swarm::Base l(x, y, 0, vx, vy, 0, 1);
00048         set_base(l);
00049 
00050         msg_count++;
00051         if(msg_count >= 5) {
00052             std::cout<<"shutdown sub"<<std::endl;
00053             sub.shutdown();
00054         }
00055         //std::cout<<"<<<"<<x<<", "<<y<<">>>"<<std::endl;
00056     }
00057     
00058     void TestBb::loop_put(const ros::TimerEvent&)
00059     {   
00060         static int count=0;
00061         std::string robot_id_string="robot_"+boost::lexical_cast<std::string>(get_id());
00062         std_msgs::Int32 val;
00063         val.data = get_id()+count;
00064         bb.put(robot_id_string, val);
00065         count++;
00066     }
00067 
00068     void TestBb::loop_get(const ros::TimerEvent&)
00069     {
00070         std::string robot_id_string="robot_"+boost::lexical_cast<std::string>(get_id());
00071         std_msgs::Int32 val = bb.get(robot_id_string);
00072         std::cout<<robot_id_string<<": "<<val.data<<std::endl;
00073     }
00074     
00075     void TestBb::start()
00076     {
00077         ros::NodeHandle nh;
00078         sub = nh.subscribe("base_pose_ground_truth", 1000, &TestBb::baseCallback, this, ros::TransportHints().udp());
00079         ros::Duration(1).sleep();
00080         bb = micros_swarm::BlackBoard<std_msgs::Int32>(0,0);
00081 
00082         //test put
00083         //timer = nh.createTimer(ros::Duration(0.1), &TestBb::loop_put, this);
00084 
00085         //test get
00086         std::string robot_id_string="robot_" + boost::lexical_cast<std::string>(get_id());
00087         std_msgs::Int32 val;
00088         val.data = get_id();
00089         bb.put(robot_id_string, val);
00090         timer = nh.createTimer(ros::Duration(0.1), &TestBb::loop_get, this);
00091     }
00092 };
00093 


testbb
Author(s):
autogenerated on Thu Jun 6 2019 18:52:39