Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

integrated all topics & services in yaml

This commit is contained in:
dave 2018-09-26 22:15:40 -04:00
parent c5f1135e6e
commit a50d88d44e
5 changed files with 105 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
include/roscontroller.h
View File

@ -142,19 +142,9 @@ private:
bool debug = false;
bool setmode = false;
std::string bzzfile_name;
std::string fcclient_name;
std::string armclient;
std::string modeclient;
std::string rcservice_name;
std::string bcfname, dbgfname;
std::string out_payload;
std::string in_payload;
std::string stand_by;
std::string xbeesrv_name;
std::string capture_srv_name;
std::string setpoint_name;
std::string stream_client_name;
std::string setpoint_nonraw;
// ROS service, publishers and subscribers
ros::ServiceClient mav_client;
@ -195,7 +185,7 @@ private:
ros::ServiceClient mode_client;
// Initialize publisher and subscriber, done in the constructor
void Initialize_pub_sub(ros::NodeHandle& n_c);
void Initialize_pub_sub(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
std::string current_mode;
@ -280,6 +270,7 @@ private:
/*Robot independent subscribers*/
void Subscribe(ros::NodeHandle& n_c);
void PubandServ(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);

17
launch/launch_config/topics.yaml
View File

@ -3,11 +3,20 @@ topics:
battery : battery
status : state
estatus : extended_state
fcclient: cmd/command
setpoint: setpoint_position/local
armclient: cmd/arming
modeclient: set_mode
localpos: local_position/pose
stream: set_stream_rate
altitude: global_position/rel_alt
obstacles: obstacles
inpayload: inMavlink
outpayload: outMavlink
gridn: grid
bstate: bvmstate
npose: neighbours_pos
fstatus: fleet_status
services:
fcclient: cmd/command
armclient: cmd/arming
modeclient: set_mode
stream: set_stream_rate
rcservice: buzzcmd
netstatus: network_status

5
launch/rosbuzz.launch
View File

@ -13,14 +13,9 @@
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
<param name="rcclient" value="true" />
<param name="debug" value="$(arg debug)" />
<param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/>
<param name="out_payload" value="outMavlink"/>
<param name="xbee_plugged" value="$(arg xbee_plugged)"/>
<param name="name" value="$(arg name)"/>
<param name="xbee_status_srv" value="network_status"/>
<param name="setmode" value="$(arg setmode)"/>
<param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
</node>

5
launch/rosbuzzd.launch
View File

@ -13,14 +13,9 @@
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" launch-prefix="gdb -ex run --args">
<rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
<param name="rcclient" value="true" />
<param name="debug" value="$(arg debug)" />
<param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/>
<param name="out_payload" value="outMavlink"/>
<param name="xbee_plugged" value="$(arg xbee_plugged)"/>
<param name="name" value="$(arg name)"/>
<param name="xbee_status_srv" value="network_status"/>
<param name="setmode" value="$(arg setmode)"/>
<param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
</node>

155
src/roscontroller.cpp
View File

@ -22,7 +22,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
// Obtain parameters from ros parameter server
Rosparameters_get(n_c_priv);
// Initialize publishers, subscribers and client
Initialize_pub_sub(n_c);
Initialize_pub_sub(n_c, n_c_priv);
// Compile the .bzz file to .basm, .bo and .bdbg
std::string fname = Compile_bzz(bzzfile_name);
bcfname = fname + ".bo";
@ -255,31 +255,6 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
ROS_ERROR("Provide a setmode mode in Launch file");
system("rosnode kill rosbuzz_node");
}
// Obtain rc service option from parameter server
if (n_c.getParam("rcclient", rcclient))
{
if (rcclient == true)
{
if (!n_c.getParam("rcservice_name", rcservice_name))
{
ROS_ERROR("Provide a name topic name for rc service in Launch file");
system("rosnode kill rosbuzz_node");
}
}
else if (rcclient == false)
{
ROS_INFO("RC service is disabled");
}
}
else
{
ROS_ERROR("Provide a rc client option: yes or no in Launch file");
system("rosnode kill rosbuzz_node");
}
// Obtain out payload name
n_c.getParam("out_payload", out_payload);
// Obtain in payload name
n_c.getParam("in_payload", in_payload);
// Obtain standby script to run during update
n_c.getParam("stand_by", stand_by);
@ -300,8 +275,6 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
system("rosnode kill rosbuzz_node");
}
}
else
n_c.getParam("xbee_status_srv", xbeesrv_name);
if (!n_c.getParam("capture_image_srv", capture_srv_name))
{
@ -313,7 +286,7 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle)
/*
/Obtains publisher, subscriber and services from yml config file
/ Obtains subscribers names from yaml config file
/-----------------------------------------------------------------------------------*/
{
std::string topic;
@ -348,74 +321,122 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle)
node_handle.getParam("topics/altitude", topic);
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "std_msgs/Float64"));
// Obtain required topic and service names from the parameter server
if (node_handle.getParam("topics/fcclient", fcclient_name))
;
node_handle.getParam("topics/inpayload", topic);
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs::Mavlink"));
}
void roscontroller::PubandServ(ros::NodeHandle& n_c, ros::NodeHandle& node_handle)
/*
/ Obtains publishers and services names from yaml config file
/-----------------------------------------------------------------------------------*/
{
std::string topic;
if (node_handle.getParam("services/fcclient", topic))
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(topic);
else
{
ROS_ERROR("Provide a fc client name in Launch file");
ROS_ERROR("Provide a fc client name in YAML file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("topics/setpoint", setpoint_name))
;
if (node_handle.getParam("topics/setpoint", topic))
localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(topic, 5);
else
{
ROS_ERROR("Provide a Set Point name in Launch file");
ROS_ERROR("Provide a Set Point name in YAML file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("topics/armclient", armclient))
;
if (node_handle.getParam("services/armclient", topic))
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(topic);
else
{
ROS_ERROR("Provide an arm client name in Launch file");
ROS_ERROR("Provide an arm service name in YAML file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("topics/modeclient", modeclient))
;
if (node_handle.getParam("services/modeclient", topic))
{
if(setmode)
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(topic);
}
else
{
ROS_ERROR("Provide a mode client name in Launch file");
ROS_ERROR("Provide a set mode service name in YAML file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("topics/stream", stream_client_name))
;
if (node_handle.getParam("services/stream", topic))
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(topic);
else
{
ROS_ERROR("Provide a mode client name in Launch file");
ROS_ERROR("Provide a set stream rate service name in YAML file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("services/rcservice", topic))
service = n_c.advertiseService(topic, &roscontroller::rc_callback, this);
else
{
ROS_ERROR("Provide a remote controler service name in YAML file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("services/netstatus", topic))
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(topic);
else
{
ROS_ERROR("Provide a network status service name in YAML file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("topics/outpayload", topic))
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(topic, 5);
else
{
ROS_ERROR("Provide a Mavlink MSG out topic name in YAML file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("topics/fstatus", topic))
MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>(topic, 5);
else
{
ROS_ERROR("Provide a fleet status out topic name in YAML file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("topics/npose", topic))
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>(topic, MAX_NUMBER_OF_ROBOTS);
else
{
ROS_ERROR("Provide a Neighbor pose out topic name in YAML file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("topics/bstate", topic))
bvmstate_pub = n_c.advertise<std_msgs::String>(topic, 5);
else
{
ROS_ERROR("Provide a BVM state out topic name in YAML file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("topics/gridn", topic))
grid_pub = n_c.advertise<nav_msgs::OccupancyGrid>(topic, 5);
else
{
ROS_ERROR("Provide a grid topic name in YAML file");
system("rosnode kill rosbuzz_node");
}
}
void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c)
void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
/*
/ Create all required subscribers, publishers and ROS service clients
/-------------------------------------------------------*/
{
// subscribers
// Subscribers
Subscribe(n_c);
payload_sub = n_c.subscribe(in_payload, MAX_NUMBER_OF_ROBOTS, &roscontroller::payload_obt, this);
// Publishers and service Clients
// publishers
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 5);
MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>("fleet_status", 5);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", MAX_NUMBER_OF_ROBOTS);
bvmstate_pub = n_c.advertise<std_msgs::String>("bvmstate", 5);
grid_pub = n_c.advertise<nav_msgs::OccupancyGrid>("grid", 5);
localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
// Service Clients
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
if(setmode)
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
if (rcclient)
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
PubandServ(n_c, n_c_priv);
ROS_INFO("Ready to receive Mav Commands from RC client");
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
capture_srv = n_c.serviceClient<mavros_msgs::CommandBool>(capture_srv_name);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
multi_msg = true;
}
@ -455,6 +476,10 @@ void roscontroller::Subscribe(ros::NodeHandle& n_c)
{
obstacle_sub = n_c.subscribe(it->first, 5, &roscontroller::obstacle_dist_callback, this);
}
else if (it->second == "mavros_msgs::Mavlink")
{
payload_sub = n_c.subscribe(it->first, MAX_NUMBER_OF_ROBOTS, &roscontroller::payload_obt, this);
}
std::cout << "Subscribed to: " << it->first << endl;
}