Go to the documentation of this file.00001
00023 #include "testbb/testbb.h"
00024
00025
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
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
00083
00084
00085
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