Spiri
/
ROSBuzz_MISTLab
12
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( add_message_files(

53
include/roscontroller.h
View File

@ -13,8 +13,13 @@
#include "mavros_msgs/Mavlink.h" #include "mavros_msgs/Mavlink.h"
#include "mavros_msgs/PositionTarget.h" #include "mavros_msgs/PositionTarget.h"
#include "sensor_msgs/NavSatStatus.h" #include "sensor_msgs/NavSatStatus.h"
#include <mavros_msgs/ParamGet.h> #include "mavros_msgs/WaypointPush.h"
#include <mavros_msgs/ParamValue.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 <sensor_msgs/LaserScan.h>
#include <rosbuzz/neigh_pos.h> #include <rosbuzz/neigh_pos.h>
#include <sstream> #include <sstream>
@ -32,6 +37,7 @@
#define UPDATER_MESSAGE_CONSTANT 987654321 #define UPDATER_MESSAGE_CONSTANT 987654321
#define XBEE_MESSAGE_CONSTANT 586782343 #define XBEE_MESSAGE_CONSTANT 586782343
#define XBEE_STOP_TRANSMISSION 4355356352 #define XBEE_STOP_TRANSMISSION 4355356352
#define TIMEOUT 60
using namespace std; using namespace std;
@ -56,6 +62,8 @@ private:
}; typedef struct num_robot_count Num_robot_count ; }; typedef struct num_robot_count Num_robot_count ;
double cur_pos[3]; double cur_pos[3];
double home[3];
double cur_rel_altitude;
uint64_t payload; uint64_t payload;
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map; std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map; std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
@ -64,14 +72,18 @@ private:
int robot_id=0; int robot_id=0;
//int oldcmdID=0; //int oldcmdID=0;
int rc_cmd; int rc_cmd;
float fcu_timeout;
int armstate; int armstate;
int barrier; int barrier;
int message_number=0; int message_number=0;
int no_of_robots=0; uint8_t no_of_robots=0;
/*tmp to be corrected*/ /*tmp to be corrected*/
int no_cnt=0; uint8_t no_cnt=0;
int old_val=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; 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 rcclient;
bool xbeeplugged; bool xbeeplugged;
bool multi_msg; bool multi_msg;
@ -87,7 +99,13 @@ private:
ros::Subscriber payload_sub; ros::Subscriber payload_sub;
ros::Subscriber flight_status_sub; ros::Subscriber flight_status_sub;
ros::Subscriber obstacle_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*/ /*Commands for flight controller*/
//mavros_msgs::CommandInt cmd_srv; //mavros_msgs::CommandInt cmd_srv;
mavros_msgs::CommandLong cmd_srv; mavros_msgs::CommandLong cmd_srv;
@ -103,6 +121,8 @@ private:
/*Initialize publisher and subscriber, done in the constructor*/ /*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*/ /*Obtain data from ros parameter server*/
void Rosparameters_get(ros::NodeHandle n_c); void Rosparameters_get(ros::NodeHandle n_c);
@ -110,7 +130,7 @@ private:
void Compile_bzz(); void Compile_bzz();
/*Flight controller service call*/ /*Flight controller service call*/
void flight_controler_service_call(); void flight_controller_service_call();
/*Neighbours pos publisher*/ /*Neighbours pos publisher*/
void neighbours_pos_publisher(); void neighbours_pos_publisher();
@ -134,6 +154,7 @@ private:
double altitude); double altitude);
/*convert from spherical to cartesian coordinate system callback */ /*convert from spherical to cartesian coordinate system callback */
void cvt_rangebearing_coordinates(double neighbours_pos_payload [], double out[], double pos[]); 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*/ /*battery status callback*/
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
@ -147,6 +168,9 @@ private:
/*current position callback*/ /*current position callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); 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*/ /*payload callback callback*/
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg); void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
@ -166,12 +190,23 @@ private:
void Arm(); void Arm();
/*set mode like guided for solo*/ /*set mode like guided for solo*/
void SetMode(); void SetMode(std::string mode, int delay_miliseconds);
/*Robot independent subscribers*/ /*Robot independent subscribers*/
void Subscribe(ros::NodeHandle n_c); 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 get_number_of_robots();
void GetRobotId();
}; };
} }

5
launch/launch_config/m100.yaml
View File

@ -3,9 +3,14 @@ topics:
battery : /power_status battery : /power_status
status : /flight_status status : /flight_status
fcclient : /dji_mavcmd fcclient : /dji_mavcmd
setpoint : /setpoint_position/local
armclient: /dji_mavarm armclient: /dji_mavarm
modeclient: /dji_mavmode modeclient: /dji_mavmode
altitude: /rel_alt
stream: /set_stream_rate
type: type:
gps : sensor_msgs/NavSatFix gps : sensor_msgs/NavSatFix
battery : mavros_msgs/BatteryStatus battery : mavros_msgs/BatteryStatus
status : mavros_msgs/ExtendedState status : mavros_msgs/ExtendedState
altitude: std_msgs/Float64

4
launch/launch_config/solo.yaml
View File

@ -3,8 +3,11 @@ topics:
battery : /mavros/battery battery : /mavros/battery
status : /mavros/state status : /mavros/state
fcclient: /mavros/cmd/command fcclient: /mavros/cmd/command
setpoint: /mavros/setpoint_position/local
armclient: /mavros/cmd/arming armclient: /mavros/cmd/arming
modeclient: /mavros/set_mode modeclient: /mavros/set_mode
stream: /mavros/set_stream_rate
altitude: /mavros/global_position/rel_alt
type: type:
gps : sensor_msgs/NavSatFix gps : sensor_msgs/NavSatFix
# for SITL Solo # for SITL Solo
@ -12,5 +15,6 @@ type:
# for solo # for solo
#battery : mavros_msgs/BatteryStatus #battery : mavros_msgs/BatteryStatus
status : mavros_msgs/State status : mavros_msgs/State
altitude: std_msgs/Float64
environment : environment :
environment : solo-simulator 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>

5
launch/rosbuzz.launch
View File

@ -2,8 +2,8 @@
<launch> <launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" > <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"/> <rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/test1.bzz" /> <param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testmoveto.bzz" />
<param name="rcclient" value="true" /> <param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" /> <param name="rcservice_name" value="/buzzcmd" />
<param name="in_payload" value="/inMavlink"/> <param name="in_payload" value="/inMavlink"/>
@ -11,5 +11,4 @@
<param name="xbee_status_srv" value="/xbee_status"/> <param name="xbee_status_srv" value="/xbee_status"/>
<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/> <param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
</node> </node>
</launch> </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; 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; time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0;
//int bytecodesize=(int); //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; //FILE *Fileptr;
//Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a"); //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); //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);

123
src/buzz_utility.cpp
View File

@ -425,6 +425,7 @@ namespace buzz_utility{
void check_swarm_members(const void* key, void* data, void* params) { void check_swarm_members(const void* key, void* data, void* params) {
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data; buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
int* status = (int*)params; int* status = (int*)params;
fprintf(stderr, "CHECKING SWARM MEMBERS\n");
if(*status == 3) return; if(*status == 3) return;
if(buzzdarray_size(e->swarms) != 1) { if(buzzdarray_size(e->swarms) != 1) {
fprintf(stderr, "Swarm list size is not 1\n"); fprintf(stderr, "Swarm list size is not 1\n");
@ -470,83 +471,84 @@ namespace buzz_utility{
buzz_error_info()); buzz_error_info());
buzzvm_dump(VM); buzzvm_dump(VM);
} }
/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/ // usleep(10000);
/* look into this currently we don't have swarm feature tested */
/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
/*Print swarm*/ /*Print swarm*/
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
/* Check swarm state */ int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
/* int status = 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); buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
if(status == 1 && /* if(status == 1 &&
buzzdict_size(VM->swarmmembers) < 9) buzzdict_size(VM->swarmmembers) < 9)
status = 2; status = 2;*/
buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));
buzzvm_pushi(VM, status); 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() { void buzz_script_destroy() {
if(VM) { if(VM) {
if(VM->state != BUZZVM_STATE_READY) { if(VM->state != BUZZVM_STATE_READY) {
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n", fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
BO_FNAME, BO_FNAME,
buzz_error_info()); buzz_error_info());
buzzvm_dump(VM); buzzvm_dump(VM);
} }
buzzvm_function_call(VM, "destroy", 0); buzzvm_function_call(VM, "destroy", 0);
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
free(BO_FNAME); free(BO_FNAME);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
} }
fprintf(stdout, "Script execution stopped.\n"); fprintf(stdout, "Script execution stopped.\n");
} }
/****************************************/ /****************************************/
/****************************************/ /****************************************/
/****************************************/ /****************************************/
/*Execution completed*/ /*Execution completed*/
/****************************************/ /****************************************/
int buzz_script_done() { int buzz_script_done() {
return VM->state != BUZZVM_STATE_READY; return VM->state != BUZZVM_STATE_READY;
} }
int update_step_test(){ int update_step_test() {
buzzuav_closures::buzzuav_update_battery(VM); buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM); buzzuav_closures::buzzuav_update_flight_status(VM);
int a = buzzvm_function_call(VM, "step", 0); int a = buzzvm_function_call(VM, "step", 0);
if(a != BUZZVM_STATE_READY){ if(a != BUZZVM_STATE_READY) {
fprintf(stdout, "step test VM state %i\n",a); fprintf(stdout, "step test VM state %i\n",a);
fprintf(stdout, " execution terminated abnormally\n\n"); fprintf(stdout, " execution terminated abnormally\n\n");
} }
return a == BUZZVM_STATE_READY; return a == BUZZVM_STATE_READY;
} }
uint16_t get_robotid(){ /* uint16_t get_robotid() {
/* Get hostname */ // Get hostname
char hstnm[30]; char hstnm[30];
gethostname(hstnm, 30); gethostname(hstnm, 30);
/* Make numeric id from hostname */ // Make numeric id from hostname
/* NOTE: here we assume that the hostname is in the format Knn */ // NOTE: here we assume that the hostname is in the format Knn
int id = strtol(hstnm + 4, NULL, 10); int id = strtol(hstnm + 4, NULL, 10);
//fprintf(stdout, "Robot id from get rid buzz util: %i\n",id); //fprintf(stdout, "Robot id from get rid buzz util: %i\n",id);
return (uint16_t)id; return (uint16_t)id;
} }*/
buzzvm_t get_vm(){ buzzvm_t get_vm() {
return VM; return VM;
} }
void set_robot_var(int ROBOTS){ void set_robot_var(int ROBOTS){
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); 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 double d = sqrt(dx*dx+dy*dy); //range
goto_pos[0]=dx; goto_pos[0]=dx;
goto_pos[1]=dy; goto_pos[1]=dy;
goto_pos[2]=0; goto_pos[2]=height;
/*double b = atan2(dy,dx); //bearing /*double b = atan2(dy,dx); //bearing
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
gps_from_rb(d, b, goto_pos); gps_from_rb(d, b, goto_pos);

292
src/roscontroller.cpp
View File

@ -1,5 +1,5 @@
#include "roscontroller.h" #include "roscontroller.h"
#include <thread>
namespace rosbzz_node{ namespace rosbzz_node{
/*--------------- /*---------------
@ -16,8 +16,16 @@ namespace rosbzz_node{
Compile_bzz(); Compile_bzz();
set_bzz_file(bzzfile_name.c_str()); set_bzz_file(bzzfile_name.c_str());
/*Initialize variables*/ /*Initialize variables*/
// Solo things
SetMode("LOITER", 0);
armstate = 0; armstate = 0;
multi_msg = true; 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,22 +40,25 @@ namespace rosbzz_node{
uav_done(); 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 /rosbuzz_node loop method executed once every step
/--------------------------------------------------*/ /--------------------------------------------------*/
void roscontroller::RosControllerRun(){ 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 */ /* Set the Buzz bytecode */
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) { if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
@ -66,7 +77,7 @@ namespace rosbzz_node{
/*Prepare messages and publish them in respective topic*/ /*Prepare messages and publish them in respective topic*/
prepare_msg_and_publish(); prepare_msg_and_publish();
/*call flight controler service to set command long*/ /*call flight controler service to set command long*/
flight_controler_service_call(); flight_controller_service_call();
/*Set multi message available after update*/ /*Set multi message available after update*/
if(get_update_status()){ if(get_update_status()){
set_read_update_status(); set_read_update_status();
@ -84,6 +95,10 @@ namespace rosbzz_node{
/*loop rate of ros*/ /*loop rate of ros*/
ros::Rate loop_rate(10); ros::Rate loop_rate(10);
loop_rate.sleep(); 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*/ /*sleep for the mentioned loop rate*/
timer_step+=1; timer_step+=1;
maintain_pos(timer_step); maintain_pos(timer_step);
@ -150,19 +165,27 @@ namespace rosbzz_node{
node_handle.getParam("type/battery", battery_type); node_handle.getParam("type/battery", battery_type);
m_smTopic_infos.insert(pair <std::string, std::string>(battery_topic, battery_type)); m_smTopic_infos.insert(pair <std::string, std::string>(battery_topic, battery_type));
std::string status_topic, status_type; std::string status_topic, status_type;
node_handle.getParam("topics/status", status_topic); node_handle.getParam("topics/status", status_topic);
node_handle.getParam("type/status", status_type); node_handle.getParam("type/status", status_type);
m_smTopic_infos.insert(pair <std::string, std::string>(status_topic, 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*/ /*Obtain fc client name from parameter server*/
if(node_handle.getParam("topics/fcclient", fcclient_name)); if(node_handle.getParam("topics/fcclient", fcclient_name));
else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");} 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)); if(node_handle.getParam("topics/armclient", armclient));
else {ROS_ERROR("Provide an arm client name in Launch file"); system("rosnode kill rosbuzz_node");} else {ROS_ERROR("Provide an arm client name in Launch file"); system("rosnode kill rosbuzz_node");}
if(node_handle.getParam("topics/modeclient", modeclient)); if(node_handle.getParam("topics/modeclient", modeclient));
else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");} 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*/ /*publishers*/
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000); payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",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*/ /* Service Clients*/
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient); arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient); mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name); mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_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; multi_msg=true;
} }
/*--------------------------------------- /*---------------------------------------
@ -208,6 +233,9 @@ namespace rosbzz_node{
else if(it->second == "sensor_msgs/NavSatFix"){ else if(it->second == "sensor_msgs/NavSatFix"){
current_position_sub = n_c.subscribe(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(it->first, 1000, &roscontroller::current_rel_alt, this);
}
std::cout << "Subscribed to: " << it->first << endl; std::cout << "Subscribed to: " << it->first << endl;
} }
@ -269,6 +297,7 @@ namespace rosbzz_node{
void roscontroller::Arm(){ void roscontroller::Arm(){
mavros_msgs::CommandBool arming_message; mavros_msgs::CommandBool arming_message;
arming_message.request.value = armstate; arming_message.request.value = armstate;
ROS_INFO("FC Arm Service called!------------------------------------------------------");
if(arm_client.call(arming_message)) { if(arm_client.call(arming_message)) {
if(arming_message.response.success==1) if(arming_message.response.success==1)
ROS_INFO("FC Arm Service called!"); ROS_INFO("FC Arm Service called!");
@ -278,20 +307,6 @@ namespace rosbzz_node{
ROS_INFO("FC Arm Service call failed!"); 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 /Prepare Buzz messages payload for each step and publish
@ -371,46 +386,75 @@ namespace rosbzz_node{
/*--------------------------------------------------------------------------------- /*---------------------------------------------------------------------------------
/Flight controller service call every step if there is a command set from bzz script /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*/ /* flight controller client call if requested from Buzz*/
/*FC call for takeoff,land, gohome and arm/disarm*/ /*FC call for takeoff,land, gohome and arm/disarm*/
int tmp = buzzuav_closures::bzz_cmd(); int tmp = buzzuav_closures::bzz_cmd();
double* goto_pos = buzzuav_closures::getgoto(); double* goto_pos = buzzuav_closures::getgoto();
if (tmp == buzzuav_closures::COMMAND_TAKEOFF || tmp== buzzuav_closures::COMMAND_LAND || tmp==buzzuav_closures::COMMAND_GOHOME){ 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.param7 = goto_pos[2];
//cmd_srv.request.z = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd(); cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); } //std::cout << " CMD: " << buzzuav_closures::getcmd() << std::endl;
else ROS_ERROR("Failed to call service from flight controller"); // 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*/ } else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*FC call for goto*/
/*prepare the goto publish message */ /*prepare the goto publish message */
cmd_srv.request.param5 = goto_pos[0]; cmd_srv.request.param5 = goto_pos[0];
cmd_srv.request.param6 = goto_pos[1]; cmd_srv.request.param6 = goto_pos[1];
cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd(); cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); } if (mav_client.call(cmd_srv)) {
else ROS_ERROR("Failed to call service from flight controller"); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; } else
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); } ROS_ERROR("Failed to call service from flight controller");
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*/ } else if (tmp == buzzuav_closures::COMMAND_ARM) { /*FC call for arm*/
armstate=1; if(!armstate){
Arm(); SetMode("LOITER", 0);
} else if (tmp == buzzuav_closures::COMMAND_DISARM){ armstate = 1;
armstate=0; Arm();
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*/ } else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
/*prepare the goto publish message */ /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
mavros_msgs::PositionTarget pt; roscontroller::SetLocalPosition(goto_pos[1], goto_pos[0], goto_pos[2], 0);
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; //roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], goto_pos[2], 0);
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);
}
} }
/*---------------------------------------------- /*----------------------------------------------
/Refresh neighbours Position for every ten step /Refresh neighbours Position for every ten step
@ -453,7 +497,6 @@ namespace rosbzz_node{
cur_pos [2] =altitude; cur_pos [2] =altitude;
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------
/ Compute Range and Bearing of a neighbor in a local reference frame / Compute Range and Bearing of a neighbor in a local reference frame
/ from GPS coordinates / from GPS coordinates
@ -468,15 +511,14 @@ namespace rosbzz_node{
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y); out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
out[1] = atan2(ned_y,ned_x); out[1] = atan2(ned_y,ned_x);
out[2] = 0.0; 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; void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){
double lon2 = nei[1]*M_PI/180.0; double d_lon = nei[1] - cur[1];
out[0] = acos(sin(lat1) * sin(lat2) +cos(lat2) * cos(lat1)*cos(lon2-lon1))*EARTH_RADIUS; double d_lat = nei[0] - cur[0];
double y = sin(lon1-lon2)*cos(lat1); out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
double x = cos(lat2)*sin(lat1) - sin(lat2)*cos(lat1)*cos(lon1-lon2); out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
out[1] = atan2(y,x)+M_PI; out[2] = cur[2];
out[2] = 0.0;*/
} }
/*------------------------------------------------------ /*------------------------------------------------------
@ -495,11 +537,11 @@ namespace rosbzz_node{
// TODO: Handle landing // TODO: Handle landing
std::cout << "Message: " << msg->mode << std::endl; std::cout << "Message: " << msg->mode << std::endl;
if(msg->mode == "GUIDED") if(msg->mode == "GUIDED")
buzzuav_closures::flight_status_update(1); buzzuav_closures::flight_status_update(2);
else if (msg->mode == "LAND") else if (msg->mode == "LAND")
buzzuav_closures::flight_status_update(4); buzzuav_closures::flight_status_update(1);
else // ground standby = LOITER? 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 / Update current position into BVM from subscriber
/-------------------------------------------------------------*/ /-------------------------------------------------------------*/
void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){ void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
set_cur_pos(msg->latitude,msg->longitude,msg->altitude); //ROS_INFO("Altitude out: %f", cur_rel_altitude);
buzzuav_closures::set_currentpos(msg->latitude,msg->longitude,msg->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 /Set obstacle Obstacle distance table into BVM from subscriber
@ -606,7 +663,7 @@ namespace rosbzz_node{
res.success = true; res.success = true;
break; break;
case mavros_msgs::CommandCode::NAV_LAND: 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; rc_cmd=mavros_msgs::CommandCode::NAV_LAND;
buzzuav_closures::rc_call(rc_cmd); buzzuav_closures::rc_call(rc_cmd);
res.success = true; res.success = true;
@ -632,14 +689,17 @@ namespace rosbzz_node{
res.success = true; res.success = true;
break; break;
case mavros_msgs::CommandCode::NAV_WAYPOINT: case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! "); ROS_INFO("RC_Call: GO TO!!!! --- Doing this! ");
double rc_goto[3]; double rc_goto[3];
// testing PositionTarget
rc_goto[0] = req.param5; rc_goto[0] = req.param5;
rc_goto[1] = req.param6; rc_goto[1] = req.param6;
rc_goto[2] = req.param7; 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); 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); buzzuav_closures::rc_call(rc_cmd);
res.success = true; res.success = true;
break; break;
@ -669,9 +729,9 @@ namespace rosbzz_node{
old_val=neighbours_pos_map.size()+1; 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++; no_cnt++;
if(no_cnt==3){ if(no_cnt>=4){
no_of_robots=neighbours_pos_map.size()+1; no_of_robots=neighbours_pos_map.size()+1;
no_cnt=0; no_cnt=0;
} }
@ -680,8 +740,9 @@ namespace rosbzz_node{
no_cnt=0; 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; uint8_t index=0;
count_robots.history[count_robots.index]=neighbours_pos_map.size()+1; count_robots.history[count_robots.index]=neighbours_pos_map.size()+1;
//count_robots.current=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){ if(odd_count>current_count){
count_robots.current=odd_val; count_robots.current=odd_val;
} }
//} }
/*else{ else{
if(neighbours_pos_map.size()!=0){ if(neighbours_pos_map.size()!=0){
count_robots.history[count_robots.index]=neighbours_pos_map.size()+1; count_robots.history[count_robots.index]=neighbours_pos_map.size()+1;
//count_robots.current=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; 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) neighbors.broadcast(updated, update_no)
} }
TARGET_ALTITUDE = 5.0 TARGET_ALTITUDE = 3.0
CURSTATE = "TURNEDOFF" CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters # Lennard-Jones parameters
@ -43,7 +43,15 @@ function hexagon() {
math.vec2.scale(accum, 1.0 / neighbors.count()) math.vec2.scale(accum, 1.0 / neighbors.count())
# Move according to vector # Move according to vector
#print("Robot ", id, "must push ",accum.length, "; ", accum.angle) #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 # Executes the barrier
# #
WAIT_TIMEOUT = 100 WAIT_TIMEOUT = 200
timeW=0 timeW=0
function barrier_wait(threshold, transf) { function barrier_wait(threshold, transf) {
barrier.get(id) barrier.get(id)
@ -106,12 +114,14 @@ function takeoff() {
CURSTATE = "TAKEOFF" CURSTATE = "TAKEOFF"
statef=takeoff statef=takeoff
log("TakeOff: ", flight.status) log("TakeOff: ", flight.status)
log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon) barrier_set(ROBOTS,hexagon)
barrier_ready() barrier_ready()
#statef=hexagon #statef=hexagon
} }
else if( flight.status !=3){ else {
log("Altitude: ", TARGET_ALTITUDE) log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22) neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE) uav_takeoff(TARGET_ALTITUDE)
@ -126,6 +136,7 @@ function land() {
uav_land() uav_land()
} }
else { else {
timeW=0
barrier = nil barrier = nil
statef=idle statef=idle
} }
@ -133,6 +144,9 @@ function land() {
# Executed once at init time. # Executed once at init time.
function init() { function init() {
s = swarm.create(1)
# s.select(1)
s.join()
statef=idle statef=idle
CURSTATE = "IDLE" CURSTATE = "IDLE"
} }