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

fix namespace use

This commit is contained in:
dave 2017-04-24 17:55:54 -04:00
parent 61d1cab41b
commit bb1ec20e37
6 changed files with 58 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
include/roscontroller.h
View File

@ -46,7 +46,7 @@ namespace rosbzz_node{
class roscontroller{
public:
roscontroller(ros::NodeHandle n_c);
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
~roscontroller();
//void RosControllerInit();
void RosControllerRun();
@ -129,12 +129,12 @@ 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);
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
/*Obtain data from ros parameter server*/
void Rosparameters_get(ros::NodeHandle n_c);
void Rosparameters_get(ros::NodeHandle& n_c_priv);
/*compiles buzz script from the specified .bzz file*/
void Compile_bzz();
@ -195,7 +195,7 @@ private:
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
/*Get publisher and subscriber from YML file*/
void GetSubscriptionParameters(ros::NodeHandle node_handle);
void GetSubscriptionParameters(ros::NodeHandle& node_handle);
/*Arm/disarm method that can be called from buzz*/
void Arm();
@ -204,7 +204,7 @@ private:
void SetMode(std::string mode, int delay_miliseconds);
/*Robot independent subscribers*/
void Subscribe(ros::NodeHandle n_c);
void Subscribe(ros::NodeHandle& n_c);
//void WaypointMissionSetup(float lat, float lng, float alt);

18
launch/launch_config/m100.yaml
View File

@ -1,13 +1,13 @@
topics:
gps : /global_position
battery : /power_status
status : /flight_status
fcclient : /dji_mavcmd
setpoint : /setpoint_position/local
armclient: /dji_mavarm
modeclient: /dji_mavmode
altitude: /rel_alt
stream: /set_stream_rate
gps : global_position
battery : power_status
status : flight_status
fcclient : dji_mavcmd
setpoint : setpoint_position/local
armclient: dji_mavarm
modeclient: dji_mavmode
altitude: rel_alt
stream: set_stream_rate
type:
gps : sensor_msgs/NavSatFix

18
launch/launch_config/solo.yaml
View File

@ -1,13 +1,13 @@
topics:
gps : /mavros/global_position/global
battery : /mavros/battery
status : /mavros/state
fcclient: /mavros/cmd/command
setpoint: /mavros/setpoint_position/local
armclient: /mavros/cmd/arming
modeclient: /mavros/set_mode
stream: /mavros/set_stream_rate
altitude: /mavros/global_position/rel_alt
gps : mavros/global_position/global
battery : mavros/battery
status : mavros/state
fcclient: mavros/cmd/command
setpoint: mavros/setpoint_position/local
armclient: mavros/cmd/arming
modeclient: mavros/set_mode
stream: mavros/set_stream_rate
altitude: mavros/global_position/rel_alt
type:
gps : sensor_msgs/NavSatFix
# for SITL Solo

5
src/rosbuzz.cpp
View File

@ -16,8 +16,9 @@ int main(int argc, char **argv)
{
/*Initialize rosBuzz node*/
ros::init(argc, argv, "rosBuzz");
ros::NodeHandle n_c("~");
rosbzz_node::roscontroller RosController(n_c);
ros::NodeHandle nh_priv("~");
ros::NodeHandle nh;
rosbzz_node::roscontroller RosController(nh, nh_priv);
/*Register ros controller object inside buzz*/
//buzzuav_closures::set_ros_controller_ptr(&RosController);
RosController.RosControllerRun();

62
src/roscontroller.cpp
View File

@ -5,11 +5,11 @@ namespace rosbzz_node{
/*---------------
/Constructor
---------------*/
roscontroller::roscontroller(ros::NodeHandle n_c)
roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
{
ROS_INFO("Buzz_node");
/*Obtain parameters from ros parameter server*/
Rosparameters_get(n_c);
Rosparameters_get(n_c_priv);
/*Initialize publishers, subscribers and client*/
Initialize_pub_sub(n_c);
/*Compile the .bzz file to .basm, .bo and .bdbg*/
@ -120,7 +120,7 @@ namespace rosbzz_node{
/*--------------------------------------------------------
/ Get all required parameters from the ROS launch file
/-------------------------------------------------------*/
void roscontroller::Rosparameters_get(ros::NodeHandle n_c){
void roscontroller::Rosparameters_get(ros::NodeHandle& n_c){
/*Obtain .bzz file name from parameter server*/
if(n_c.getParam("bzzfile_name", bzzfile_name));
@ -129,11 +129,8 @@ namespace rosbzz_node{
if(n_c.getParam("rcclient", rcclient)){
if(rcclient==true){
/*Service*/
if(n_c.getParam("rcservice_name", rcservice_name)){
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
ROS_INFO("Ready to receive Mav Commands from RC client");
}
else{ROS_ERROR("Provide a name topic name for rc service in Launch file"); system("rosnode kill rosbuzz_node");}
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");}
}
@ -162,7 +159,7 @@ namespace rosbzz_node{
/*-----------------------------------------------------------------------------------
/Obtains publisher, subscriber and services from yml file based on the robot used
/-----------------------------------------------------------------------------------*/
void roscontroller::GetSubscriptionParameters(ros::NodeHandle node_handle){
void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle){
//todo: make it as an array in yaml?
m_sMySubscriptions.clear();
std::string gps_topic, gps_type;
@ -202,55 +199,58 @@ namespace rosbzz_node{
/*--------------------------------------------------------
/ Create all required subscribers, publishers and ROS service clients
/-------------------------------------------------------*/
void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){
void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c){
/*subscribers*/
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("/"+robot_name+in_payload, 1000, &roscontroller::payload_obt,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);
obstacle_sub= n_c.subscribe("guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
/*publishers*/
payload_pub = n_c.advertise<mavros_msgs::Mavlink>("/"+robot_name+out_payload, 1000);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>("/"+robot_name+setpoint_name,1000);
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos",1000);
localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name,1000);
/* Service Clients*/
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>("/"+robot_name+armclient);
mode_client = n_c.serviceClient<mavros_msgs::SetMode>("/"+robot_name+modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>("/"+robot_name+fcclient_name);
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>("/"+robot_name+xbeesrv_name);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>("/"+robot_name+stream_client_name);
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
if(rcclient==true)
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
ROS_INFO("Ready to receive Mav Commands from RC client");
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
users_sub = n_c.subscribe("/"+robot_name+"/users_pos", 1000, &roscontroller::users_pos,this);
users_sub = n_c.subscribe("users_pos", 1000, &roscontroller::users_pos,this);
multi_msg=true;
}
/*---------------------------------------
/Robot independent subscribers
/--------------------------------------*/
void roscontroller::Subscribe(ros::NodeHandle n_c){
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("/"+robot_name+it->first, 100, &roscontroller::flight_extended_status_update, this);
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("/"+robot_name+it->first, 100, &roscontroller::flight_status_update, this);
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("/"+robot_name+it->first, 1000, &roscontroller::battery, this);
battery_sub = n_c.subscribe(it->first, 1000, &roscontroller::battery, this);
}
else if(it->second == "sensor_msgs/NavSatFix"){
current_position_sub = n_c.subscribe("/"+robot_name+it->first, 1000, &roscontroller::current_pos, this);
current_position_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this);
}
else if(it->second == "std_msgs/Float64"){
relative_altitude_sub = n_c.subscribe("/"+robot_name+it->first, 1000, &roscontroller::current_rel_alt, this);
relative_altitude_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_rel_alt, this);
}
std::cout << "Subscribed to: " << "/"+robot_name+it->first << endl;
std::cout << "Subscribed to: " << it->first << endl;
}
}
@ -425,9 +425,9 @@ namespace rosbzz_node{
armstate = 1;
Arm();
ros::Duration(0.5).sleep();
// Registering HOME POINT.
home[0] = cur_pos[0];home[1] = cur_pos[1];home[2] = cur_pos[2];
}
// Registering HOME POINT.
home[0] = cur_pos[0];home[1] = cur_pos[1];home[2] = cur_pos[2];
if(current_mode != "GUIDED")
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
if (mav_client.call(cmd_srv))
@ -751,7 +751,7 @@ namespace rosbzz_node{
moveMsg.pose.orientation.w = 1;
// To prevent drifting from stable position.
if(fabs(x)>0.01 || fabs(y)>0.01) {
if(fabs(x)>0.1 || fabs(y)>0.1) {
localsetpoint_nonraw_pub.publish(moveMsg);
ROS_INFO("Sent local NON RAW position message!");
}

2
src/testflocksim.bzz
View File

@ -16,7 +16,7 @@ CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
TARGET = 12.0
EPSILON = 8.0
EPSILON = 10.0
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {