Spiri
/
services-ros1-xbee_mav
12
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

robot id service

This commit is contained in:
vivek-shankar 2017-04-01 02:49:25 -04:00
parent 52ba435ecc
commit a96b6abda5
5 changed files with 24 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -57,8 +57,8 @@ include_directories(
add_executable(xbee_mav src/Xbee.cpp src/XBeeFrame.cpp src/SerialDevice.cpp src/CommunicationManager.cpp src/PacketsHandler) add_executable(xbee_mav src/Xbee.cpp src/XBeeFrame.cpp src/SerialDevice.cpp src/CommunicationManager.cpp src/PacketsHandler)
target_link_libraries(xbee_mav ${catkin_LIBRARIES}) target_link_libraries(xbee_mav ${catkin_LIBRARIES})
#add_executable(config src/main.cpp src/XBeeModule.cpp src/XMLConfigParser.cpp) add_executable(config src/main.cpp src/XBeeModule.cpp src/XMLConfigParser.cpp)
#target_link_libraries(config ${catkin_LIBRARIES}) target_link_libraries(config ${catkin_LIBRARIES})
#add_executable(test_controller src/TestController.cpp) #add_executable(test_controller src/TestController.cpp)
#target_link_libraries(test_controller ${catkin_LIBRARIES}) #target_link_libraries(test_controller ${catkin_LIBRARIES})

6
Resources/XBee_Config.xml
View File

@ -2,13 +2,13 @@
<XBeeConfig> <XBeeConfig>
<Settings> <Settings>
<Parameter Command="CM" Description="Channel Mask">00FFFFFFFFFFFF7FFF</Parameter> <Parameter Command="CM" Description="Channel Mask">00FFFFFFFFFFF7FFFF</Parameter>
<Parameter Command="HP" Description="Preamble ID">1</Parameter> <Parameter Command="HP" Description="Preamble ID">1</Parameter>
<Parameter Command="ID" Description="Network ID">5FFF</Parameter> <Parameter Command="ID" Description="Network ID">5FFF</Parameter>
<Parameter Command="MT" Description="Broadcast Multi-Transmits">1</Parameter> <Parameter Command="MT" Description="Broadcast Multi-Transmits">3</Parameter>
<Parameter Command="PL" Description="TX Power Level">4</Parameter> <Parameter Command="PL" Description="TX Power Level">4</Parameter>
<Parameter Command="RR" Description="Unicast Retries">A</Parameter> <Parameter Command="RR" Description="Unicast Retries">A</Parameter>
<Parameter Command="CE" Description="Routing/Messaging Mode">2</Parameter> <Parameter Command="CE" Description="Routing/Messaging Mode">0</Parameter>
<Parameter Command="BH" Description="Broadcast Hops">0</Parameter> <Parameter Command="BH" Description="Broadcast Hops">0</Parameter>
<Parameter Command="NH" Description="Network Hops">7</Parameter> <Parameter Command="NH" Description="Network Hops">7</Parameter>
<Parameter Command="MR" Description="Mesh Unicast Retries">1</Parameter> <Parameter Command="MR" Description="Mesh Unicast Retries">1</Parameter>

7
include/CommunicationManager.h
View File

@ -15,7 +15,8 @@
#include<mavros_msgs/CommandInt.h> #include<mavros_msgs/CommandInt.h>
#include<mavros_msgs/Mavlink.h> #include<mavros_msgs/Mavlink.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <mavros_msgs/ParamGet.h>
#include <mavros_msgs/ParamValue.h>
#include"PacketsHandler.h" #include"PacketsHandler.h"
#include"SerialDevice.h" #include"SerialDevice.h"
@ -86,6 +87,7 @@ private:
void Process_In_Fragments(); void Process_In_Fragments();
void Process_In_Packets(); void Process_In_Packets();
void Process_Command_Responses(); void Process_Command_Responses();
bool Get_ID(mavros_msgs::ParamGet::Request& req, mavros_msgs::ParamGet::Response& res);
Mist::Xbee::SerialDevice serial_device_; Mist::Xbee::SerialDevice serial_device_;
Mist::Xbee::PacketsHandler packets_handler_; Mist::Xbee::PacketsHandler packets_handler_;
@ -97,9 +99,10 @@ private:
ros::NodeHandle node_handle_; ros::NodeHandle node_handle_;
ros::Subscriber mavlink_subscriber_; ros::Subscriber mavlink_subscriber_;
ros::Publisher mavlink_publisher_; ros::Publisher mavlink_publisher_;
ros::Publisher Robot_Id_Publisher_; //ros::Publisher Robot_Id_Publisher_;
ros::ServiceClient mav_dji_client_; ros::ServiceClient mav_dji_client_;
ros::ServiceServer mav_dji_server_; ros::ServiceServer mav_dji_server_;
ros::ServiceServer Robot_Id_;
std_msgs::UInt8 device_id_out; std_msgs::UInt8 device_id_out;
std::shared_ptr<std::thread> service_thread_; // TO DO delete !? std::shared_ptr<std::thread> service_thread_; // TO DO delete !?
}; };

18
src/CommunicationManager.cpp
View File

@ -143,7 +143,7 @@ void CommunicationManager::Run_In_Swarm_Mode()
std::string in_messages_topic; std::string in_messages_topic;
bool success_1 = false; bool success_1 = false;
bool success_2 = false; bool success_2 = false;
Robot_Id_ = node_handle_.advertiseService("/Robot_ID_srv", &CommunicationManager::Get_ID, this);
if (node_handle_.getParam("Xbee_In_From_Buzz", out_messages_topic)) if (node_handle_.getParam("Xbee_In_From_Buzz", out_messages_topic))
{ {
mavlink_subscriber_ = node_handle_.subscribe(out_messages_topic.c_str(), 1000, mavlink_subscriber_ = node_handle_.subscribe(out_messages_topic.c_str(), 1000,
@ -157,9 +157,9 @@ void CommunicationManager::Run_In_Swarm_Mode()
{ {
mavlink_publisher_ = node_handle_.advertise<mavros_msgs::Mavlink>( mavlink_publisher_ = node_handle_.advertise<mavros_msgs::Mavlink>(
in_messages_topic.c_str(), 1000); in_messages_topic.c_str(), 1000);
Robot_Id_Publisher_= node_handle_.advertise<std_msgs::UInt8>( //Robot_Id_Publisher_= node_handle_.advertise<std_msgs::UInt8>(
"/device_id_xbee_", 1000); // "/device_id_xbee_", 1000);
device_id_out.data = packets_handler_.get_device_id(); //device_id_out.data = packets_handler_.get_device_id();
success_2 = true; success_2 = true;
} }
else else
@ -167,10 +167,11 @@ void CommunicationManager::Run_In_Swarm_Mode()
if (success_1 && success_2) if (success_1 && success_2)
{ {
ros::Rate loop_rate(LOOP_RATE); ros::Rate loop_rate(LOOP_RATE);
while (ros::ok()) while (ros::ok())
{ {
Robot_Id_Publisher_.publish(device_id_out); //Robot_Id_Publisher_.publish(device_id_out);
Process_In_Standard_Messages(); Process_In_Standard_Messages();
Process_In_Fragments(); Process_In_Fragments();
Process_In_Acks_and_Pings(); Process_In_Acks_and_Pings();
@ -544,6 +545,13 @@ void CommunicationManager::Process_Command_Responses()
} }
} }
bool CommunicationManager::Get_ID (mavros_msgs::ParamGet::Request& req, mavros_msgs::ParamGet::Response& res){
mavros_msgs::ParamValue id;
id.integer=packets_handler_.get_device_id();
res.value= id;
res.success=true;
return true;
}
} }

2
src/XMLConfigParser.cpp
View File

@ -24,7 +24,7 @@ XMLConfigParser::~XMLConfigParser()
//***************************************************************************** //*****************************************************************************
bool XMLConfigParser::Load_Config() bool XMLConfigParser::Load_Config()
{ {
const std::string FILE_NAME = "/home/vivek/catkin_ws/src/xbee_ros_node/Resources/XBee_Config.xml"; const std::string FILE_NAME = "/home/vivek/catkin_ws/src/xbee/Resources/XBee_Config.xml";
if (Check_Config_File_Exists(FILE_NAME)) if (Check_Config_File_Exists(FILE_NAME))
{ {