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

Added topic parametrization from launch file

This commit is contained in:
isvogor 2017-02-15 18:08:41 -05:00
parent c20422dc7e
commit 17f974367d
5 changed files with 89 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
include/roscontroller.h
View File

@ -66,6 +66,9 @@ private:
/*Commands for flight controller*/
mavros_msgs::CommandInt cmd_srv;
std::vector<std::string> m_sMySubscriptions;
std::map<std::string, std::string> m_smTopic_infos;
void Initialize_pub_sub(ros::NodeHandle n_c);
@ -110,8 +113,11 @@ private:
/*battery status callback*/
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
/*flight extended status callback*/
void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
/*flight status callback*/
void flight_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
void flight_status_update(const mavros_msgs::State::ConstPtr& msg);
/*current position callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
@ -125,6 +131,9 @@ private:
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
void GetSubscriptionParameters(ros::NodeHandle node_handle);
void Subscribe(ros::NodeHandle n_c);
};
}

16
launch/rosbuzz.launch
View File

@ -1,19 +1,19 @@
<!-- Launch file for ROSBuzz -->
<launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="$(env ROS_WS)/src/ROSBuzz/launch/robot_config/$(env ROBOT).yaml"/>
<param name="program_mode" value="production "/>
<!-- Move this to yaml -->
<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/ROSBuzz/src/test1.bzz" />
<rosparam file="$(env ROS_WS)/src/ROSBuzz/launch/launch_config/$(env ROBOT).yaml"/>
<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/test1.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<!-- Move this to yaml -->
<param name="fcclient_name" value="/mavros/cmd/command_int" />
<!-- <param name="fcclient_name" value="/mavros/cmd/command_int" /> -->
<param name="fcclient_name" value="/dji_mavcmd" />
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="3"/>
<param name="No_of_Robots" value="2"/>
<param name="No_of_Robots" value="3"/>
<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
</node>
</launch>

4
launch/rosbuzzm100.launch
View File

@ -2,7 +2,7 @@
<launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<param name="bzzfile_name" value="../ROS_WS/src/ROSBuzz/src/test1.bzz" />
<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/test1.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<param name="fcclient_name" value="/dji_mavcmd" />
@ -10,7 +10,7 @@
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="3"/>
<param name="No_of_Robots" value="3"/>
<param name="stand_by" value="../ROS_WS/src/ROSBuzz/src/stand_by.bo"/>
<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
</node>

69
src/roscontroller.cpp
View File

@ -139,15 +139,41 @@ namespace rosbzz_node{
/*Obtain standby script to run during update*/
n_c.getParam("stand_by", stand_by);
GetSubscriptionParameters(n_c);
// initialize topics to null?
}
void roscontroller::GetSubscriptionParameters(ros::NodeHandle node_handle){
//todo: make it as an array in yaml?
m_sMySubscriptions.clear();
std::string gps_topic, gps_type;
node_handle.getParam("topics/gps", gps_topic);
node_handle.getParam("type/gps", gps_type);
m_smTopic_infos.insert(pair <std::string, std::string>(gps_topic, gps_type));
std::string battery_topic, battery_type;
node_handle.getParam("topics/battery", battery_topic);
node_handle.getParam("type/battery", battery_type);
m_smTopic_infos.insert(pair <std::string, std::string>(battery_topic, battery_type));
std::string status_topic, status_type;
node_handle.getParam("topics/status", status_topic);
node_handle.getParam("type/status", status_type);
m_smTopic_infos.insert(pair <std::string, std::string>(status_topic, status_type));
}
void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){
/*subscribers*/
current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this);
battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this);
Subscribe(n_c);
//current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this);
//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_status_update,this);
//flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this);
obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
/*publishers*/
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
@ -157,6 +183,26 @@ namespace rosbzz_node{
multi_msg=true;
}
void roscontroller::Subscribe(ros::NodeHandle n_c){
for(std::map<std::string, std::string>::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it){
if(it->second == "mavros_msgs/ExtendedState"){
flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this);
}
else if(it->second == "mavros_msgs/State"){
flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this);
}
else if(it->second == "mavros_msgs/BatteryStatus"){
battery_sub = n_c.subscribe(it->first, 1000, &roscontroller::battery, this);
}
else if(it->second == "sensor_msgs/NavSatFix"){
current_position_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this);
}
std::cout << "Subscribed to: " << it->first << endl;
}
}
void roscontroller::Compile_bzz(){
/*Compile the buzz code .bzz to .bo*/
stringstream bzzfile_in_compile;
@ -485,8 +531,21 @@ namespace rosbzz_node{
buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining);
//ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, msg->current, msg ->remaining);
}
/*flight status update*/
void roscontroller::flight_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg){
/*flight extended status update*/
void roscontroller::flight_status_update(const mavros_msgs::State::ConstPtr& msg){
// http://wiki.ros.org/mavros/CustomModes
std::cout << "Message: " << msg->mode << std::endl;
if(msg->mode == "GUIDED")
buzzuav_closures::flight_status_update(1);
else if (msg->mode == "LAND")
buzzuav_closures::flight_status_update(4);
else // ground standby = LOITER?
buzzuav_closures::flight_status_update(5);//?
}
/*flight extended status update*/
void roscontroller::flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg){
buzzuav_closures::flight_status_update(msg->landed_state);
}
/*current position callback*/

2
src/test1.bzz
View File

@ -1,7 +1,7 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz"
include "/home/ivan/dev/buzz/src/include/vec2.bzz"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater