merge solo-playground
This commit is contained in:
dave
commit
2cafc1cbe1
|
|
@ -15,7 +15,6 @@ find_package(catkin REQUIRED COMPONENTS
|
|||
)
|
||||
|
||||
##############################
|
||||
#Generate messages#
|
||||
##############################
|
||||
|
||||
add_message_files(
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
@ -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>
|
||||
|
|
@ -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>
|
||||
|
|
@ -2,8 +2,8 @@
|
|||
|
||||
<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"/>
|
||||
|
|
@ -11,5 +11,4 @@
|
|||
<param name="xbee_status_srv" value="/xbee_status"/>
|
||||
<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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){
|
|||
}
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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,22 +40,25 @@ 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)) {
|
||||
|
|
@ -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);
|
||||
|
|
@ -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
|
||||
|
|
@ -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");
|
||||
//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();
|
||||
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 */
|
||||
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);
|
||||
}
|
||||
/*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
|
||||
|
|
@ -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");
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue