diff --git a/include/roscontroller.h b/include/roscontroller.h index 626b202..1a1c30a 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -1,6 +1,7 @@ #pragma once #include #include +#include #include "mavros_msgs/GlobalPositionTarget.h" #include "mavros_msgs/CommandCode.h" #include "mavros_msgs/CommandLong.h" @@ -66,6 +67,7 @@ private: ros::Subscriber payload_sub; ros::Subscriber flight_status_sub; ros::Subscriber obstacle_sub; + ros::Subscriber Robot_id_sub; /*Commands for flight controller*/ //mavros_msgs::CommandInt cmd_srv; mavros_msgs::CommandLong cmd_srv; @@ -137,6 +139,9 @@ private: /* RC commands service */ bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res); + /*robot id sub callback*/ + void set_robot_id(const std_msgs::UInt8::ConstPtr& msg); + void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg); void GetSubscriptionParameters(ros::NodeHandle node_handle); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 600fea5..1b19a30 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -117,7 +117,7 @@ namespace rosbzz_node{ else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");} /*Obtain robot_id from parameter server*/ //n_c.getParam("robot_id", robot_id); - robot_id=(int)buzz_utility::get_robotid(); + //robot_id=(int)buzz_utility::get_robotid(); /*Obtain out payload name*/ n_c.getParam("out_payload", out_payload); /*Obtain in payload name*/ @@ -169,7 +169,7 @@ namespace rosbzz_node{ //battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this); payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this); //flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this); - + Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this); obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this); /*publishers*/ payload_pub = n_c.advertise(out_payload, 1000); @@ -730,6 +730,10 @@ namespace rosbzz_node{ } return true; } + void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){ + robot_id=(int)msg->data; + + } } diff --git a/src/test1.bzz b/src/test1.bzz index 3e9d1fb..78c257e 100644 --- a/src/test1.bzz +++ b/src/test1.bzz @@ -1,7 +1,7 @@ # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." -include "/home/ubuntu/buzz/src/include/vec2.bzz" +include "/home/vivek/ubuntu/buzz/src/include/vec2.bzz" #################################################################################################### # Updater related # This should be here for the updater to work, changing position of code will crash the updater