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

merge solo-playground

This commit is contained in:
dave 2017-04-14 14:55:29 -04:00
parent fcf9b17f0f 3af2a03ffc
commit 2cafc1cbe1
14 changed files with 519 additions and 188 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -15,7 +15,6 @@ find_package(catkin REQUIRED COMPONENTS
)
##############################
#Generate messages#
##############################
add_message_files(

53
include/roscontroller.h
View File

@ -13,8 +13,13 @@
#include "mavros_msgs/Mavlink.h"
#include "mavros_msgs/PositionTarget.h"
#include "sensor_msgs/NavSatStatus.h"
#include <mavros_msgs/ParamGet.h>
#include <mavros_msgs/ParamValue.h>
#include "mavros_msgs/WaypointPush.h"
#include "mavros_msgs/Waypoint.h"
#include "mavros_msgs/PositionTarget.h"
#include "mavros_msgs/StreamRate.h"
#include "mavros_msgs/ParamGet.h"
#include "geometry_msgs/PoseStamped.h"
#include "std_msgs/Float64.h"
#include <sensor_msgs/LaserScan.h>
#include <rosbuzz/neigh_pos.h>
#include <sstream>
@ -32,6 +37,7 @@
#define UPDATER_MESSAGE_CONSTANT 987654321
#define XBEE_MESSAGE_CONSTANT 586782343
#define XBEE_STOP_TRANSMISSION 4355356352
#define TIMEOUT 60
using namespace std;
@ -56,6 +62,8 @@ private:
}; typedef struct num_robot_count Num_robot_count ;
double cur_pos[3];
double home[3];
double cur_rel_altitude;
uint64_t payload;
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
@ -64,14 +72,18 @@ private:
int robot_id=0;
//int oldcmdID=0;
int rc_cmd;
float fcu_timeout;
int armstate;
int barrier;
int message_number=0;
int no_of_robots=0;
uint8_t no_of_robots=0;
/*tmp to be corrected*/
int no_cnt=0;
int old_val=0;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name;
uint8_t no_cnt=0;
uint8_t old_val=0;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name;
std::string stream_client_name;
std::string relative_altitude_sub_name;
std::string setpoint_nonraw;
bool rcclient;
bool xbeeplugged;
bool multi_msg;
@ -87,7 +99,13 @@ private:
ros::Subscriber payload_sub;
ros::Subscriber flight_status_sub;
ros::Subscriber obstacle_sub;
//ros::Subscriber Robot_id_sub;
ros::Subscriber Robot_id_sub;
ros::Subscriber relative_altitude_sub;
ros::ServiceClient stream_client;
int setpoint_counter;
double my_x = 0, my_y = 0;
/*Commands for flight controller*/
//mavros_msgs::CommandInt cmd_srv;
mavros_msgs::CommandLong cmd_srv;
@ -103,6 +121,8 @@ 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
/*Obtain data from ros parameter server*/
void Rosparameters_get(ros::NodeHandle n_c);
@ -110,7 +130,7 @@ private:
void Compile_bzz();
/*Flight controller service call*/
void flight_controler_service_call();
void flight_controller_service_call();
/*Neighbours pos publisher*/
void neighbours_pos_publisher();
@ -134,6 +154,7 @@ private:
double altitude);
/*convert from spherical to cartesian coordinate system callback */
void cvt_rangebearing_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
void cvt_ned_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
/*battery status callback*/
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
@ -147,6 +168,9 @@ private:
/*current position callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
/*current relative altitude callback*/
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
/*payload callback callback*/
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
@ -166,12 +190,23 @@ private:
void Arm();
/*set mode like guided for solo*/
void SetMode();
void SetMode(std::string mode, int delay_miliseconds);
/*Robot independent subscribers*/
void Subscribe(ros::NodeHandle n_c);
//void WaypointMissionSetup(float lat, float lng, float alt);
void fc_command_setup();
void SetLocalPosition(float x, float y, float z, float yaw);
void SetLocalPositionNonRaw(float x, float y, float z, float yaw);
void SetStreamRate(int id, int rate, int on_off);
void get_number_of_robots();
void GetRobotId();
};
}

5
launch/launch_config/m100.yaml
View File

@ -3,9 +3,14 @@ topics:
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
battery : mavros_msgs/BatteryStatus
status : mavros_msgs/ExtendedState
altitude: std_msgs/Float64

4
launch/launch_config/solo.yaml
View File

@ -3,8 +3,11 @@ topics:
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
@ -12,5 +15,6 @@ type:
# for solo
#battery : mavros_msgs/BatteryStatus
status : mavros_msgs/State
altitude: std_msgs/Float64
environment :
environment : solo-simulator

42
launch/rosbuzz-solo.launch Normal file
View File

@ -0,0 +1,42 @@
<launch>
<!-- RUN mavros -->
<arg name="fcu_url" default="/dev/ttyAMA0:115200" />
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<include file="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/node.launch">
<arg name="pluginlists_yaml" value="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/apm_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
<arg name="log_output" value="$(arg log_output)" />
</include>
<!-- set streaming rate -->
<!-- node pkg="rosservice" type="rosservice" name="freq" args="call /mavros/set_stream_rate 0 10 1" output="screen" / -->
<!-- run xbee -->
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="/home/pi/ros_catkinKin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/src/testflockfev.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="xbee_status_srv" value="/xbee_status"/>
<param name="stand_by" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/src/stand_by.bo"/>
</node>
</launch>

49
launch/rosbuzz-testing-sitl.launch Normal file
View File

@ -0,0 +1,49 @@
<launch>
<!-- RUN mavros
<arg name="fcu_url" default="tcp://127.0.0.1:5760" />
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
<arg name="log_output" value="$(arg log_output)" />
</include>
-->
<!-- run xbee -->
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
<!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testflockfev.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="xbee_status_srv" value="/xbee_status"/>
<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
</node>
<!--
<node pkg="rosloader" type="rosloader" name="rosloader" output="screen"/>
-->
<!-- set streaming rate
/mavros/cmd/arming
<node pkg="rosservice" type="rosservice" name="stream_rate" args="call /mavros/set_stream_rate 0 10 1" />
-->
</launch>

43
launch/rosbuzz-testing.launch Normal file
View File

@ -0,0 +1,43 @@
<launch>
<!-- RUN mavros -->
<arg name="fcu_url" default="/dev/ttyAMA0:115200" />
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
<arg name="log_output" value="$(arg log_output)" />
</include>
<!-- set streaming rate -->
<!-- node pkg="rosservice" type="rosservice" name="freq" args="call /mavros/set_stream_rate 0 10 1" output="screen" / -->
<!-- run xbee -->
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<param name="No_of_dev" type="int" value="3" />
<!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="/home/pi/ros_catkin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/testflockfev.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="3"/>
<param name="No_of_Robots" value="3"/>
<param name="stand_by" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/stand_by.bo"/>
</node>
</launch>

7
launch/rosbuzz.launch
View File

@ -2,14 +2,13 @@
<launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<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" />
<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testmoveto.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="xbee_status_srv" value="/xbee_status"/>
<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
</node>
</node>
</launch>

20
misc/cmdlinectr.sh Normal file
View File

@ -0,0 +1,20 @@
#! /bin/bash
function takeoff {
rosservice call /buzzcmd 0 22 0 0 0 0 0 0 0 0
}
function land {
rosservice call /buzzcmd 0 21 0 0 0 0 0 0 0 0
}
function record {
rosbag record /flight_status /global_position /dji_sdk/local_position /neighbours_pos /power_status /guidance/obstacle_distance /guidance/front_depth_image/compressedDepth /guidance/right_depth_image/compressedDepth /guidance/left_depth_image/compressedDepth /guidance/bottom_depth_image/compressedDepth /guidance/back_depth_image/compressedDepth
}
function clean {
sudo rm /var/log/upstart/robot*
sudo rm /var/log/upstart/x3s*
}
function startrobot {
sudo service robot start
}
function stoprobot {
sudo service robot stop
}

2
src/buzz_update.cpp
View File

@ -454,7 +454,7 @@ void collect_data(){
double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC;
time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0;
//int bytecodesize=(int);
fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)buzz_utility::get_robotid(),neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no);
fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no);
//FILE *Fileptr;
//Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a");
//fprintf(Fileptr,"%i,%i,%i,%i,%i,%u\n",(int)buzz_utility::get_robotid(),neigh,timer_steps,(int)*(size_t*)updater->bcode_size,(int)no_of_robot, *(uint8_t*)updater->update_no);

149
src/buzz_utility.cpp
View File

@ -49,7 +49,7 @@ namespace buzz_utility{
out[3] = (int32_2 & (0xFFFF0000) ) >> 16;
//cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" ";
return out;
}
}
/***************************************************/
/*Appends obtained messages to buzz in message Queue*/
@ -125,21 +125,21 @@ namespace buzz_utility{
/* Add message length to data buffer */
*(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m);
tot += sizeof(uint16_t);
/* Add payload to data buffer */
memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m));
tot += buzzmsg_payload_size(m);
/* Get rid of message */
buzzoutmsg_queue_next(VM);
buzzmsg_payload_destroy(&m);
} while(1);
uint16_t total_size =(ceil((float)tot/(float)sizeof(uint64_t)));
*(uint16_t*)buff_send = (uint16_t) total_size;
uint16_t total_size =(ceil((float)tot/(float)sizeof(uint64_t)));
*(uint16_t*)buff_send = (uint16_t) total_size;
uint64_t* payload_64 = new uint64_t[total_size];
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
free(buff_send);
/*for(int i=0;i<total_size;i++){
@ -148,7 +148,7 @@ namespace buzz_utility{
/* Send message */
return payload_64;
}
/****************************************/
/*Buzz script not able to load*/
/****************************************/
@ -334,7 +334,7 @@ namespace buzz_utility{
/* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
/* Read debug information */
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM);
@ -356,7 +356,7 @@ namespace buzz_utility{
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
return 0;
}
/* Execute the global part of the script */
buzzvm_execute_script(VM);
/* Call the Init() function */
@ -381,7 +381,7 @@ namespace buzz_utility{
/* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
/* Read debug information */
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM);
@ -403,7 +403,7 @@ namespace buzz_utility{
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
return 0;
}
/* Execute the global part of the script */
buzzvm_execute_script(VM);
/* Call the Init() function */
@ -425,6 +425,7 @@ namespace buzz_utility{
void check_swarm_members(const void* key, void* data, void* params) {
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
int* status = (int*)params;
fprintf(stderr, "CHECKING SWARM MEMBERS\n");
if(*status == 3) return;
if(buzzdarray_size(e->swarms) != 1) {
fprintf(stderr, "Swarm list size is not 1\n");
@ -470,83 +471,84 @@ namespace buzz_utility{
buzz_error_info());
buzzvm_dump(VM);
}
/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
/* look into this currently we don't have swarm feature tested */
/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
// usleep(10000);
/*Print swarm*/
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
/* Check swarm state */
/* int status = 1;
buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
/* Check swarm state -- SOMETHING CRASHING HERE!! */
int status = 1;
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
if(status == 1 &&
/* if(status == 1 &&
buzzdict_size(VM->swarmmembers) < 9)
status = 2;
status = 2;*/
buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));
buzzvm_pushi(VM, status);
buzzvm_gstore(VM);*/
buzzvm_gstore(VM);
}
/****************************************/
/*Destroy the bvm and other resorces*/
/****************************************/
/****************************************/
/*Destroy the bvm and other resorces*/
/****************************************/
void buzz_script_destroy() {
if(VM) {
if(VM->state != BUZZVM_STATE_READY) {
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
BO_FNAME,
buzz_error_info());
buzzvm_dump(VM);
}
buzzvm_function_call(VM, "destroy", 0);
buzzvm_destroy(&VM);
free(BO_FNAME);
buzzdebug_destroy(&DBG_INFO);
}
fprintf(stdout, "Script execution stopped.\n");
}
void buzz_script_destroy() {
if(VM) {
if(VM->state != BUZZVM_STATE_READY) {
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
BO_FNAME,
buzz_error_info());
buzzvm_dump(VM);
}
buzzvm_function_call(VM, "destroy", 0);
buzzvm_destroy(&VM);
free(BO_FNAME);
buzzdebug_destroy(&DBG_INFO);
}
fprintf(stdout, "Script execution stopped.\n");
}
/****************************************/
/****************************************/
/****************************************/
/****************************************/
/****************************************/
/*Execution completed*/
/****************************************/
/****************************************/
/*Execution completed*/
/****************************************/
int buzz_script_done() {
return VM->state != BUZZVM_STATE_READY;
}
int buzz_script_done() {
return VM->state != BUZZVM_STATE_READY;
}
int update_step_test(){
int update_step_test() {
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
int a = buzzvm_function_call(VM, "step", 0);
if(a != BUZZVM_STATE_READY){
fprintf(stdout, "step test VM state %i\n",a);
fprintf(stdout, " execution terminated abnormally\n\n");
}
return a == BUZZVM_STATE_READY;
}
int a = buzzvm_function_call(VM, "step", 0);
if(a != BUZZVM_STATE_READY) {
fprintf(stdout, "step test VM state %i\n",a);
fprintf(stdout, " execution terminated abnormally\n\n");
}
return a == BUZZVM_STATE_READY;
}
uint16_t get_robotid(){
/* Get hostname */
char hstnm[30];
gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Knn */
int id = strtol(hstnm + 4, NULL, 10);
//fprintf(stdout, "Robot id from get rid buzz util: %i\n",id);
return (uint16_t)id;
}
/* uint16_t get_robotid() {
// Get hostname
char hstnm[30];
gethostname(hstnm, 30);
// Make numeric id from hostname
// NOTE: here we assume that the hostname is in the format Knn
int id = strtol(hstnm + 4, NULL, 10);
//fprintf(stdout, "Robot id from get rid buzz util: %i\n",id);
return (uint16_t)id;
}*/
buzzvm_t get_vm(){
return VM;
}
buzzvm_t get_vm() {
return VM;
}
void set_robot_var(int ROBOTS){
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
@ -556,4 +558,3 @@ void set_robot_var(int ROBOTS){
}

2
src/buzzuav_closures.cpp
View File

@ -104,7 +104,7 @@ namespace buzzuav_closures{
double d = sqrt(dx*dx+dy*dy); //range
goto_pos[0]=dx;
goto_pos[1]=dy;
goto_pos[2]=0;
goto_pos[2]=height;
/*double b = atan2(dy,dx); //bearing
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
gps_from_rb(d, b, goto_pos);

308
src/roscontroller.cpp
View File

@ -1,5 +1,5 @@
#include "roscontroller.h"
#include <thread>
namespace rosbzz_node{
/*---------------
@ -16,8 +16,16 @@ namespace rosbzz_node{
Compile_bzz();
set_bzz_file(bzzfile_name.c_str());
/*Initialize variables*/
// Solo things
SetMode("LOITER", 0);
armstate = 0;
multi_msg = true;
// set stream rate - wait for the FC to be started
SetStreamRate(0, 10, 1);
/// Get Robot Id - wait for Xbee to be started
GetRobotId();
setpoint_counter = 0;
fcu_timeout = TIMEOUT;
}
/*---------------------
@ -32,23 +40,26 @@ namespace rosbzz_node{
uav_done();
}
void roscontroller::GetRobotId()
{
mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
mavros_msgs::ParamGet::Response robot_id_srv_response;
while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
ros::Duration(0.1).sleep();
ROS_ERROR("Waiting for Xbee to respond to get device ID");
}
robot_id=robot_id_srv_response.value.integer;
//robot_id = 8;
}
/*-------------------------------------------------
/rosbuzz_node loop method executed once every step
/--------------------------------------------------*/
void roscontroller::RosControllerRun(){
mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
mavros_msgs::ParamGet::Response robot_id_srv_response;
while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
/*run once*/
ros::spinOnce();
/*loop rate of ros*/
ros::Rate loop_rate(10);
loop_rate.sleep();
/*sleep for the mentioned loop rate*/
ROS_ERROR("Waiting for Xbee to respond to get device ID");
}
robot_id=robot_id_srv_response.value.integer;
/* Set the Buzz bytecode */
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
fprintf(stdout, "Bytecode file found and set\n");
@ -66,7 +77,7 @@ namespace rosbzz_node{
/*Prepare messages and publish them in respective topic*/
prepare_msg_and_publish();
/*call flight controler service to set command long*/
flight_controler_service_call();
flight_controller_service_call();
/*Set multi message available after update*/
if(get_update_status()){
set_read_update_status();
@ -84,6 +95,10 @@ namespace rosbzz_node{
/*loop rate of ros*/
ros::Rate loop_rate(10);
loop_rate.sleep();
if(fcu_timeout<=0)
buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND);
else
fcu_timeout -= 1/10;
/*sleep for the mentioned loop rate*/
timer_step+=1;
maintain_pos(timer_step);
@ -128,7 +143,7 @@ namespace rosbzz_node{
n_c.getParam("xbee_status_srv", xbeesrv_name);
if(n_c.getParam("xbee_plugged", xbeeplugged));
else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");}
GetSubscriptionParameters(n_c);
// initialize topics to null?
@ -150,19 +165,27 @@ namespace rosbzz_node{
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));
std::string altitude_topic, altitude_type;
node_handle.getParam("topics/altitude", altitude_topic);
node_handle.getParam("type/altitude", altitude_type);
m_smTopic_infos.insert(pair <std::string, std::string>(altitude_topic, altitude_type));
/*Obtain fc client name from parameter server*/
if(node_handle.getParam("topics/fcclient", fcclient_name));
else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}
if(node_handle.getParam("topics/setpoint", setpoint_name));
else {ROS_ERROR("Provide a Set Point name in Launch file"); system("rosnode kill rosbuzz_node");}
if(node_handle.getParam("topics/armclient", armclient));
else {ROS_ERROR("Provide an arm client name in Launch file"); system("rosnode kill rosbuzz_node");}
if(node_handle.getParam("topics/modeclient", modeclient));
else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");}
if(node_handle.getParam("topics/stream", stream_client_name));
else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");}
}
/*--------------------------------------------------------
@ -182,12 +205,14 @@ namespace rosbzz_node{
/*publishers*/
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
localsetpoint_pub = n_c.advertise<mavros_msgs::PositionTarget>("/setpoint_raw/local",1000);
localsetpoint_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name,1000);
/* Service Clients*/
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);
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
multi_msg=true;
}
/*---------------------------------------
@ -208,6 +233,9 @@ namespace rosbzz_node{
else if(it->second == "sensor_msgs/NavSatFix"){
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(it->first, 1000, &roscontroller::current_rel_alt, this);
}
std::cout << "Subscribed to: " << it->first << endl;
}
@ -269,6 +297,7 @@ namespace rosbzz_node{
void roscontroller::Arm(){
mavros_msgs::CommandBool arming_message;
arming_message.request.value = armstate;
ROS_INFO("FC Arm Service called!------------------------------------------------------");
if(arm_client.call(arming_message)) {
if(arming_message.response.success==1)
ROS_INFO("FC Arm Service called!");
@ -278,20 +307,6 @@ namespace rosbzz_node{
ROS_INFO("FC Arm Service call failed!");
}
}
/*---------------------------------------------------------
/ Set mode for the solos
/---------------------------------------------------------*/
void roscontroller::SetMode(){
mavros_msgs::SetMode set_mode_message;
set_mode_message.request.base_mode = 0;
set_mode_message.request.custom_mode = "GUIDED"; // shit!
if(mode_client.call(set_mode_message)) {
ROS_INFO("Set Mode Service call successful!");
} else {
ROS_INFO("Set Mode Service call failed!");
}
}
/*-----------------------------------------------------------------------------------------------------
/Prepare Buzz messages payload for each step and publish
@ -323,7 +338,7 @@ namespace rosbzz_node{
else message_number++;
payload_out.sysid=(uint8_t)robot_id;
payload_out.msgid=(uint32_t)message_number;
/*publish prepared messages in respective topic*/
payload_pub.publish(payload_out);
delete[] out;
@ -371,46 +386,75 @@ namespace rosbzz_node{
/*---------------------------------------------------------------------------------
/Flight controller service call every step if there is a command set from bzz script
/-------------------------------------------------------------------------------- */
void roscontroller::flight_controler_service_call(){
void roscontroller::flight_controller_service_call() {
/* flight controller client call if requested from Buzz*/
/*FC call for takeoff,land, gohome and arm/disarm*/
int tmp = buzzuav_closures::bzz_cmd();
double* goto_pos = buzzuav_closures::getgoto();
if (tmp == buzzuav_closures::COMMAND_TAKEOFF || tmp== buzzuav_closures::COMMAND_LAND || tmp==buzzuav_closures::COMMAND_GOHOME){
double* goto_pos = buzzuav_closures::getgoto();
if (tmp == buzzuav_closures::COMMAND_TAKEOFF || tmp== buzzuav_closures::COMMAND_LAND || tmp==buzzuav_closures::COMMAND_GOHOME) {
cmd_srv.request.param7 = goto_pos[2];
//cmd_srv.request.z = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
else ROS_ERROR("Failed to call service from flight controller");
} else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*FC call for goto*/
cmd_srv.request.command = buzzuav_closures::getcmd();
//std::cout << " CMD: " << buzzuav_closures::getcmd() << std::endl;
// SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND mode
switch(buzzuav_closures::getcmd()){
case mavros_msgs::CommandCode::NAV_LAND:
if(current_mode != "LAND")
{SetMode("LAND", 0);}
if (mav_client.call(cmd_srv)){
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else
{ROS_ERROR("Failed to call service from flight controller");}
armstate = 0;
break;
case mavros_msgs::CommandCode::NAV_TAKEOFF:
if(!armstate){
SetMode("LOITER", 0);
armstate = 1;
Arm();
ros::Duration(0.5).sleep();
}
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))
{ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);}
else
ROS_ERROR("Failed to call service from flight controller");
break;
}
} else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*FC call for goto*/
/*prepare the goto publish message */
cmd_srv.request.param5 = goto_pos[0];
cmd_srv.request.param6 = goto_pos[1];
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
else ROS_ERROR("Failed to call service from flight controller");
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
else ROS_ERROR("Failed to call service from flight controller");
cmd_srv.request.param6 = goto_pos[1];
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv)) {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else
ROS_ERROR("Failed to call service from flight controller");
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
if (mav_client.call(cmd_srv)) {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else
ROS_ERROR("Failed to call service from flight controller");
} else if (tmp == buzzuav_closures::COMMAND_ARM) { /*FC call for arm*/
armstate=1;
Arm();
} else if (tmp == buzzuav_closures::COMMAND_DISARM){
armstate=0;
Arm();
} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
/*prepare the goto publish message */
mavros_msgs::PositionTarget pt;
pt.type_mask = mavros_msgs::PositionTarget::IGNORE_VX | mavros_msgs::PositionTarget::IGNORE_VY | mavros_msgs::PositionTarget::IGNORE_VZ | mavros_msgs::PositionTarget::IGNORE_AFX | mavros_msgs::PositionTarget::IGNORE_AFY | mavros_msgs::PositionTarget::IGNORE_AFZ | mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
pt.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_OFFSET_NED;
pt.position.x = goto_pos[0];
pt.position.y = goto_pos[1];
pt.position.z = goto_pos[2];
pt.yaw = 0;//goto_pos[3];
ROS_INFO("Sending local setpoint: %.2f, %.2f, %.2f",pt.position.x,pt.position.y,pt.position.z);
localsetpoint_pub.publish(pt);
}
if(!armstate){
SetMode("LOITER", 0);
armstate = 1;
Arm();
}
} else if (tmp == buzzuav_closures::COMMAND_DISARM) {
if(armstate){
armstate = 0;
SetMode("LOITER", 0);
Arm();
}
} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
roscontroller::SetLocalPosition(goto_pos[1], goto_pos[0], goto_pos[2], 0);
//roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], goto_pos[2], 0);
}
}
/*----------------------------------------------
/Refresh neighbours Position for every ten step
@ -453,7 +497,6 @@ namespace rosbzz_node{
cur_pos [2] =altitude;
}
/*-----------------------------------------------------------
/ Compute Range and Bearing of a neighbor in a local reference frame
/ from GPS coordinates
@ -468,15 +511,14 @@ namespace rosbzz_node{
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
out[1] = atan2(ned_y,ned_x);
out[2] = 0.0;
/* double lat1 = cur[0]*M_PI/180.0;
double lon1 = cur[1]*M_PI/180.0;
double lat2 = nei[0]*M_PI/180.0;
double lon2 = nei[1]*M_PI/180.0;
out[0] = acos(sin(lat1) * sin(lat2) +cos(lat2) * cos(lat1)*cos(lon2-lon1))*EARTH_RADIUS;
double y = sin(lon1-lon2)*cos(lat1);
double x = cos(lat2)*sin(lat1) - sin(lat2)*cos(lat1)*cos(lon1-lon2);
out[1] = atan2(y,x)+M_PI;
out[2] = 0.0;*/
}
void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
out[2] = cur[2];
}
/*------------------------------------------------------
@ -495,11 +537,11 @@ namespace rosbzz_node{
// TODO: Handle landing
std::cout << "Message: " << msg->mode << std::endl;
if(msg->mode == "GUIDED")
buzzuav_closures::flight_status_update(1);
buzzuav_closures::flight_status_update(2);
else if (msg->mode == "LAND")
buzzuav_closures::flight_status_update(4);
buzzuav_closures::flight_status_update(1);
else // ground standby = LOITER?
buzzuav_closures::flight_status_update(1);//?
buzzuav_closures::flight_status_update(7);//?
}
/*------------------------------------------------------------
@ -512,8 +554,23 @@ namespace rosbzz_node{
/ Update current position into BVM from subscriber
/-------------------------------------------------------------*/
void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
buzzuav_closures::set_currentpos(msg->latitude,msg->longitude,msg->altitude);
//ROS_INFO("Altitude out: %f", cur_rel_altitude);
fcu_timeout = TIMEOUT;
if(home[0]==0){
home[0]=msg->latitude;
home[1]=msg->longitude;
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);
}
/*-------------------------------------------------------------
/ Update altitude into BVM from subscriber
/-------------------------------------------------------------*/
void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr& msg){
//ROS_INFO("Altitude in: %f", msg->data);
cur_rel_altitude = (double)msg->data;
}
/*-------------------------------------------------------------
/Set obstacle Obstacle distance table into BVM from subscriber
@ -588,7 +645,7 @@ namespace rosbzz_node{
delete[] out;
buzz_utility::in_msg_process((message_obt+3));
}
}
/*-----------------------------------------------------------
@ -606,7 +663,7 @@ namespace rosbzz_node{
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_LAND:
ROS_INFO("RC_Call: LAND!!!!");
ROS_INFO("RC_Call: LAND!!!! sending land");
rc_cmd=mavros_msgs::CommandCode::NAV_LAND;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
@ -632,14 +689,17 @@ namespace rosbzz_node{
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! ");
ROS_INFO("RC_Call: GO TO!!!! --- Doing this! ");
double rc_goto[3];
// testing PositionTarget
rc_goto[0] = req.param5;
rc_goto[1] = req.param6;
rc_goto[2] = req.param7;
// for test
//SetLocalPosition(rc_goto[0], rc_goto[1], rc_goto[2], 0);
buzzuav_closures::rc_set_goto(rc_goto);
rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
rc_cmd= mavros_msgs::CommandCode::NAV_WAYPOINT;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
@ -669,9 +729,9 @@ namespace rosbzz_node{
old_val=neighbours_pos_map.size()+1;
}
else if(old_val==neighbours_pos_map.size()+1){
else if(no_cnt!=0 && old_val==neighbours_pos_map.size()+1){
no_cnt++;
if(no_cnt==3){
if(no_cnt>=4){
no_of_robots=neighbours_pos_map.size()+1;
no_cnt=0;
}
@ -680,8 +740,9 @@ namespace rosbzz_node{
no_cnt=0;
}
}
//if(count_robots.current !=0){
/*std::map< int, int> count_count;
/*
if(count_robots.current !=0){
std::map< int, int> count_count;
uint8_t index=0;
count_robots.history[count_robots.index]=neighbours_pos_map.size()+1;
//count_robots.current=neighbours_pos_map.size()+1;
@ -701,8 +762,8 @@ namespace rosbzz_node{
if(odd_count>current_count){
count_robots.current=odd_val;
}
//}
/*else{
}
else{
if(neighbours_pos_map.size()!=0){
count_robots.history[count_robots.index]=neighbours_pos_map.size()+1;
//count_robots.current=neighbours_pos_map.size()+1;
@ -712,8 +773,67 @@ namespace rosbzz_node{
count_robots.current=neighbours_pos_map.size()+1;
}
}
}*/
}
*/
}
}
/*
* SOLO SPECIFIC FUNCTIONS
*/
void roscontroller::SetLocalPosition(float x, float y, float z, float yaw){
// http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html
// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
geometry_msgs::PoseStamped moveMsg;
moveMsg.header.stamp = ros::Time::now();
moveMsg.header.seq = setpoint_counter++;
moveMsg.header.frame_id = 1;
double local_pos[3];
cvt_ned_coordinates(cur_pos,local_pos,home);
moveMsg.pose.position.x = local_pos[0]+x;
moveMsg.pose.position.y = local_pos[1]+y;
moveMsg.pose.position.z = z;
moveMsg.pose.orientation.x = 0;
moveMsg.pose.orientation.y = 0;
moveMsg.pose.orientation.z = 0;
moveMsg.pose.orientation.w = 1;
localsetpoint_pub.publish(moveMsg);
ROS_INFO("Sent local NON RAW position message!");
}
void roscontroller::SetMode(std::string mode, int delay_miliseconds){
// wait if necessary
if (delay_miliseconds != 0){
std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) );
}
// set mode
mavros_msgs::SetMode set_mode_message;
set_mode_message.request.base_mode = 0;
set_mode_message.request.custom_mode = mode;
current_mode = mode;
if(mode_client.call(set_mode_message)) {
ROS_INFO("Set Mode Service call successful!");
} else {
ROS_INFO("Set Mode Service call failed!");
}
}
void roscontroller::SetStreamRate(int id, int rate, int on_off){
mavros_msgs::StreamRate message;
message.request.stream_id = id;
message.request.message_rate = rate;
message.request.on_off = on_off;
while(!stream_client.call(message)){
ROS_INFO("Set stream rate call failed!, trying again...");
ros::Duration(0.1).sleep();
}
ROS_INFO("Set stream rate call successful");
}
}

22
src/testflockfev.bzz
View File

@ -11,7 +11,7 @@ function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 5.0
TARGET_ALTITUDE = 3.0
CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
@ -43,7 +43,15 @@ function hexagon() {
math.vec2.scale(accum, 1.0 / neighbors.count())
# Move according to vector
#print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
uav_moveto(accum.x, accum.y)
# uav_moveto(accum.x,accum.y)
if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
timeW =0
statef=land
} else {
timeW = timeW+1
uav_moveto(0.0,0.0)
}
}
########################################
@ -78,7 +86,7 @@ function barrier_ready() {
#
# Executes the barrier
#
WAIT_TIMEOUT = 100
WAIT_TIMEOUT = 200
timeW=0
function barrier_wait(threshold, transf) {
barrier.get(id)
@ -106,12 +114,14 @@ function takeoff() {
CURSTATE = "TAKEOFF"
statef=takeoff
log("TakeOff: ", flight.status)
log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
#statef=hexagon
}
else if( flight.status !=3){
else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
@ -126,6 +136,7 @@ function land() {
uav_land()
}
else {
timeW=0
barrier = nil
statef=idle
}
@ -133,6 +144,9 @@ function land() {
# Executed once at init time.
function init() {
s = swarm.create(1)
# s.select(1)
s.join()
statef=idle
CURSTATE = "IDLE"
}