From 05f51194cc7b70644cd5c9c01b323350ccacec97 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 1 Apr 2017 02:48:37 -0400 Subject: [PATCH] changes to new robot id service --- include/roscontroller.h | 8 ++++++-- src/roscontroller.cpp | 22 +++++++++++++++++----- 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index ee6ffe7..f1ac04c 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -13,6 +13,8 @@ #include "mavros_msgs/Mavlink.h" #include "mavros_msgs/PositionTarget.h" #include "sensor_msgs/NavSatStatus.h" +#include +#include #include #include #include @@ -61,6 +63,7 @@ private: bool rcclient; bool multi_msg; ros::ServiceClient mav_client; + ros::ServiceClient robot_id_client; ros::Publisher payload_pub; ros::Publisher neigh_pos_pub; ros::Publisher localsetpoint_pub; @@ -70,11 +73,12 @@ private: ros::Subscriber payload_sub; ros::Subscriber flight_status_sub; ros::Subscriber obstacle_sub; - ros::Subscriber Robot_id_sub; + //ros::Subscriber Robot_id_sub; /*Commands for flight controller*/ //mavros_msgs::CommandInt cmd_srv; mavros_msgs::CommandLong cmd_srv; - + mavros_msgs::ParamGet::Request robot_id_srv_request; + mavros_msgs::ParamGet::Response robot_id_srv_response; std::vector m_sMySubscriptions; std::map m_smTopic_infos; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 6ab0be0..441a367 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -36,6 +36,18 @@ namespace rosbzz_node{ /rosbuzz_node loop method executed once every step /--------------------------------------------------*/ void roscontroller::RosControllerRun(){ + + while(!robot_id_client.call(robot_id_srv_request,robot_id_srv_response)){ + /*run once*/ + ros::spinOnce(); + /*loop rate of ros*/ + ros::Rate loop_rate(10); + loop_rate.sleep(); + /*sleep for the mentioned loop rate*/ + ROS_ERROR("Waiting for Xbee to respond to get device ID"); + } + robot_id=robot_id_srv_response.value.integer; + /* Set the Buzz bytecode */ if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) { fprintf(stdout, "Bytecode file found and set\n"); @@ -154,7 +166,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); + //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); @@ -164,7 +176,7 @@ namespace rosbzz_node{ arm_client = n_c.serviceClient(armclient); mode_client = n_c.serviceClient(modeclient); mav_client = n_c.serviceClient(fcclient_name); - + robot_id_client = n_c.serviceClient("/Robot_ID_srv"); multi_msg=true; } /*--------------------------------------- @@ -630,10 +642,10 @@ namespace rosbzz_node{ /Obtain robot id by subscribing to xbee robot id topic / TODO: check for integrity of this subscriber call back /----------------------------------------------------*/ - void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){ - robot_id=(int)msg->data; + /*void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){ - } + + }*/ }