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

removed unused functions and variables

This commit is contained in:
dave 2017-12-08 19:44:24 -05:00
parent 74f7f740e0
commit 60aef71f38
10 changed files with 43 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -59,7 +59,6 @@ add_executable(rosbuzz_node src/rosbuzz.cpp
src/roscontroller.cpp
src/buzz_utility.cpp
src/buzzuav_closures.cpp
src/uav_utility.cpp
src/buzz_update.cpp)
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} buzz buzzdbg pthread)
add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp)

3
include/buzzuav_closures.h
View File

@ -2,7 +2,6 @@
#include <buzz/buzzvm.h>
#include <stdio.h>
#include "uav_utility.h"
#include "mavros_msgs/CommandCode.h"
#include "mavros_msgs/Mavlink.h"
#include "ros/ros.h"
@ -46,8 +45,6 @@ void parse_gpslist();
int buzzuav_takepicture(buzzvm_t vm);
/* Returns the current command from local variable*/
int getcmd();
/*Sets goto position */
void set_goto(double pos[]);
/*Sets goto position from rc client*/
void rc_set_goto(int id, double latitude, double longitude, double altitude);
/*Sets gimbal orientation from rc client*/

16
include/roscontroller.h
View File

@ -20,12 +20,12 @@
#include "mavros_msgs/ParamGet.h"
#include "geometry_msgs/PoseStamped.h"
#include "std_msgs/Float64.h"
#include "std_msgs/String.h"
#include <sensor_msgs/LaserScan.h>
#include <rosbuzz/neigh_pos.h>
#include <sstream>
#include <buzz/buzzasm.h>
#include "buzz_utility.h"
#include "uav_utility.h"
#include <stdlib.h>
#include <string.h>
#include <math.h>
@ -65,7 +65,9 @@ private:
{
}
};
typedef struct num_robot_count Num_robot_count; // not useful in cpp
typedef struct num_robot_count Num_robot_count;
Num_robot_count count_robots;
struct gps
{
@ -115,13 +117,13 @@ private:
bool rcclient;
bool xbeeplugged = false;
bool multi_msg;
Num_robot_count count_robots;
ros::ServiceClient mav_client;
ros::ServiceClient xbeestatus_srv;
ros::ServiceClient capture_srv;
ros::Publisher payload_pub;
ros::Publisher MPpayload_pub;
ros::Publisher neigh_pos_pub;
ros::Publisher uavstate_pub;
ros::Publisher localsetpoint_nonraw_pub;
ros::ServiceServer service;
ros::Subscriber current_position_sub;
@ -160,7 +162,7 @@ private:
/*Initialize publisher and subscriber, done in the constructor*/
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
std::string current_mode;
/*Obtain data from ros parameter server*/
void Rosparameters_get(ros::NodeHandle& n_c_priv);
@ -174,6 +176,10 @@ private:
/*Neighbours pos publisher*/
void neighbours_pos_publisher();
/*UAVState publisher*/
void uavstate_publisher();
/*BVM message payload publisher*/
void send_MPpayload();
/*Prepare messages and publish*/
@ -190,11 +196,11 @@ private:
/*Set the current position of the robot callback*/
void set_cur_pos(double latitude, double longitude, double altitude);
/*convert from spherical to cartesian coordinate system callback */
float constrainAngle(float x);
void gps_rb(GPS nei_pos, double out[]);
void gps_ned_cur(float& ned_x, float& ned_y, GPS t);
void gps_ned_home(float& ned_x, float& ned_y);
void gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon,
double gps_r_lat);

8
include/uav_utility.h
View File

@ -1,8 +0,0 @@
#ifndef UAV_UTILITY_H
#define UAV_UTILITY_H
extern void uav_setup();
extern void uav_done();
#endif

3
log.txt
View File

@ -1,3 +0,0 @@
2017-01-08-14-24-24 enter TranslateFunc
2017-01-08-14-24-24 start to read frames!

30
readme.md
View File

@ -75,37 +75,41 @@ Run
===
To run the ROSBuzz package using the launch file, execute the following:
$ roslaunch rosbuzz rosbuzzm100.launch
$ roslaunch rosbuzz rosbuzz.launch
Note : Before launching the ROSBuzz node, verify all the parameter in the launch file.
Note : Before launching the ROSBuzz node, verify all the parameters in the launch file. A launch file using gdb is available also (rosbuzzd.launch).
* Buzz scripts: Several behavioral scripts are included in the "buzz_Scripts" folder, such as "graphformGPS.bzz" uses in the ICRA publication below and the "testaloneWP.bzz" to control a single drone with a ".csv" list of waypoints. The script "empty.bzz" is a template script.
Publisher
=========
* Messages from Buzz (BVM):
The package publishes mavros_msgs/Mavlink message with a topic name of "outMavlink".
The package publishes mavros_msgs/Mavlink message "outMavlink".
* Command to the flight controller:
The package publishes geometry_msgs/PoseStamped message "setpoint_position/local".
Subscribers
-----------
* Current position of the Robot:
The package subscribes' to sensor_msgs/NavSatFix message with a topic name of "current_pos".
The package subscribes to sensor_msgs/NavSatFix message "global_position/global", to a std_msgs/Float64 message "global_position/rel_alt" and to a geometry_msgs/PoseStamped message "local_position/pose".
* Messages to Buzz (BVM):
The package subscribes' to mavros_msgs/Mavlink message with a topic name of "inMavlink".
The package subscribes to mavros_msgs/Mavlink message with a topic name of "inMavlink".
* Battery status:
The package subscribes' to mavros_msgs/BatteryStatus message with a topic name of "battery_state".
* Status:
The package subscribes to mavros_msgs/BatteryStatus message "battery" and to either a mavros_msgs/ExtendedState message "extended_state" or a mavros_msgs/State message "state".
Service
-------
* Remote Controller command:
The package offers service using mavros_msgs/CommandInt service with name "rc_cmd".
* Remote Controller:
The package offers a mavros_msgs/CommandLong service "buzzcmd" to control its state.
Client
References
------
* ROS and Buzz : consensus-based behaviors for heterogeneous teams. Submitted to the Internaional Conference on Robotics and Automation (September 2017). 6pgs. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843
* Flight controller client:
This package is a client of mavros_msgs/CommandInt service with name "fc_cmd".
* Over-The-Air Updates for Robotic Swarms. Accepted by IEEE Software (September 2017 - pending minor revision). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G.

38
src/buzz_utility.cpp
View File

@ -450,44 +450,6 @@ struct buzzswarm_elem_s
};
typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
void check_swarm_members(const void* key, void* data, void* params)
{
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
int* status = (int*)params;
if (*status == 3)
return;
fprintf(stderr, "CHECKING SWARM :%i, member: %i, age: %i \n", buzzdarray_get(e->swarms, 0, uint16_t), *(uint16_t*)key,
e->age);
if (buzzdarray_size(e->swarms) != 1)
{
fprintf(stderr, "Swarm list size is not 1\n");
*status = 3;
}
else
{
int sid = 1;
if (!buzzdict_isempty(VM->swarms))
{
if (*buzzdict_get(VM->swarms, &sid, uint8_t) && buzzdarray_get(e->swarms, 0, uint16_t) != sid)
{
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", sid, buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
}
if (buzzdict_size(VM->swarms) > 1)
{
sid = 2;
if (*buzzdict_get(VM->swarms, &sid, uint8_t) && buzzdarray_get(e->swarms, 0, uint16_t) != sid)
{
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", sid, buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
}
}
}
/*Step through the buzz script*/
void update_sensors()
{

32
src/buzzuav_closures.cpp
View File

@ -94,22 +94,6 @@ void setWPlist(string path)
WPlistname = path + "include/graphs/waypointlist.csv";
}
/*----------------------------------------/
/ Compute GPS destination from current position and desired Range and Bearing
/----------------------------------------*/
void gps_from_rb(double range, double bearing, double out[3])
{
double lat = RAD2DEG(cur_pos[0]);
double lon = RAD2DEG(cur_pos[1]);
out[0] = asin(sin(lat) * cos(range / EARTH_RADIUS) + cos(lat) * sin(range / EARTH_RADIUS) * cos(bearing));
out[1] = lon + atan2(sin(bearing) * sin(range / EARTH_RADIUS) * cos(lat),
cos(range / EARTH_RADIUS) - sin(lat) * sin(out[0]));
out[0] = RAD2DEG(out[0]);
out[1] = RAD2DEG(out[1]);
out[2] = height; // constant height.
}
float constrainAngle(float x)
{
x = fmod(x, 2 * M_PI);
@ -118,6 +102,9 @@ float constrainAngle(float x)
return x;
}
/*----------------------------------------/
/ Compute Range and Bearing from 2 GPS set of coordinates
/----------------------------------------*/
void rb_from_gps(double nei[], double out[], double cur[])
{
double d_lon = nei[1] - cur[1];
@ -129,11 +116,6 @@ void rb_from_gps(double nei[], double out[], double cur[])
out[2] = 0.0;
}
// Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests
double hcpos1[4] = { 45.564489, -73.562537, 45.564140, -73.562336 };
double hcpos2[4] = { 45.564729, -73.562060, 45.564362, -73.562958 };
double hcpos3[4] = { 45.564969, -73.562838, 45.564636, -73.563677 };
void parse_gpslist()
{
// Open file:
@ -468,7 +450,6 @@ float* getgimbal()
string getuavstate()
{
static buzzvm_t VM = buzz_utility::get_vm();
std::stringstream state_buff;
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
buzzvm_gload(VM);
buzzobj_t uav_state = buzzvm_stack_at(VM, 1);
@ -481,13 +462,6 @@ int getcmd()
return cur_cmd;
}
void set_goto(double pos[])
{
goto_pos[0] = pos[0];
goto_pos[1] = pos[1];
goto_pos[2] = pos[2];
}
int bzz_cmd()
{
int cmd = buzz_cmd;

26
src/roscontroller.cpp
View File

@ -71,7 +71,6 @@ roscontroller::~roscontroller()
/* Cleanup */
buzz_utility::buzz_script_destroy();
/* Stop the robot */
uav_done();
log.close();
}
@ -364,7 +363,7 @@ used
void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle)
{
m_sMySubscriptions.clear();
std::string gps_topic, gps_type;
std::string gps_topic;
if (node_handle.getParam("topics/gps", gps_topic))
;
else
@ -452,6 +451,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c)
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", 5);
uavstate_pub = n_c.advertise<std_msgs::String>("uavstate", 5);
localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
/* Service Clients*/
@ -549,6 +549,16 @@ void roscontroller::neighbours_pos_publisher()
neigh_pos_pub.publish(neigh_pos_array);
}
/*----------------------------------------------------
/ Publish current UAVState from Buzz script
/----------------------------------------------------*/
void roscontroller::uavstate_publisher()
{
std_msgs::String uavstate_msg;
uavstate_msg.data = buzzuav_closures::getuavstate();
uavstate_pub.publish(uavstate_msg);
}
/*--------------------------------------------------------
/ Functions handling the local MAV ROS flight controller
/-------------------------------------------------------*/
@ -877,11 +887,6 @@ void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, GPS t)
gps_convert_ned(ned_x, ned_y, t.longitude, t.latitude, cur_pos.longitude, cur_pos.latitude);
}
void roscontroller::gps_ned_home(float& ned_x, float& ned_y)
{
gps_convert_ned(ned_x, ned_y, cur_pos.longitude, cur_pos.latitude, home.longitude, home.latitude);
}
void roscontroller::gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon,
double gps_r_lat)
{
@ -931,13 +936,6 @@ void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg)
{
// ROS_INFO("Altitude out: %f", cur_rel_altitude);
fcu_timeout = TIMEOUT;
// double lat = std::floor(msg->latitude * 1000000) / 1000000;
// double lon = std::floor(msg->longitude * 1000000) / 1000000;
/*if(cur_rel_altitude<1.2){
home[0]=lat;
home[1]=lon;
home[2]=cur_rel_altitude;
}*/
set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude); // msg->altitude);
buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_rel_altitude); // msg->altitude);
}

25
src/uav_utility.cpp
View File

@ -1,25 +0,0 @@
#include "uav_utility.h"
#include "stdio.h"
/****************************************/
/****************************************/
/****************************************/
/*To do !*/
/****************************************/
void uav_setup()
{
}
/****************************************/
/*To do !*/
/****************************************/
void uav_done()
{
fprintf(stdout, "Robot stopped.\n");
}
/****************************************/
/****************************************/