Attempt to Merge branch 'solo-playground' of https://github.com/MISTLab/ROSBuzz
This commit is contained in:
vivek-shankar
commit
25fc91e432
|
|
@ -1 +1,3 @@
|
|||
src/test*
|
||||
.cproject
|
||||
.project
|
||||
|
|
|
|||
|
|
@ -3,8 +3,10 @@
|
|||
#include <sensor_msgs/NavSatFix.h>
|
||||
#include "mavros_msgs/GlobalPositionTarget.h"
|
||||
#include "mavros_msgs/CommandCode.h"
|
||||
#include "mavros_msgs/CommandInt.h"
|
||||
#include "mavros_msgs/CommandLong.h"
|
||||
#include "mavros_msgs/CommandBool.h"
|
||||
#include "mavros_msgs/ExtendedState.h"
|
||||
#include "mavros_msgs/SetMode.h"
|
||||
#include "mavros_msgs/State.h"
|
||||
#include "mavros_msgs/BatteryStatus.h"
|
||||
#include "mavros_msgs/Mavlink.h"
|
||||
|
|
@ -51,7 +53,7 @@ private:
|
|||
//int oldcmdID=0;
|
||||
int rc_cmd;
|
||||
int barrier;
|
||||
std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by; //, rcclient;
|
||||
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by;
|
||||
bool rcclient;
|
||||
bool multi_msg;
|
||||
ros::ServiceClient mav_client;
|
||||
|
|
@ -64,8 +66,17 @@ private:
|
|||
ros::Subscriber flight_status_sub;
|
||||
ros::Subscriber obstacle_sub;
|
||||
/*Commands for flight controller*/
|
||||
mavros_msgs::CommandInt cmd_srv;
|
||||
|
||||
//mavros_msgs::CommandInt cmd_srv;
|
||||
mavros_msgs::CommandLong cmd_srv;
|
||||
|
||||
std::vector<std::string> m_sMySubscriptions;
|
||||
std::map<std::string, std::string> m_smTopic_infos;
|
||||
|
||||
mavros_msgs::CommandBool m_cmdBool;
|
||||
ros::ServiceClient arm_client;
|
||||
|
||||
mavros_msgs::SetMode m_cmdSetMode;
|
||||
ros::ServiceClient mode_client;
|
||||
|
||||
void Initialize_pub_sub(ros::NodeHandle n_c);
|
||||
|
||||
|
|
@ -110,8 +121,11 @@ private:
|
|||
/*battery status callback*/
|
||||
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
|
||||
|
||||
/*flight extended status callback*/
|
||||
void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
|
||||
|
||||
/*flight status callback*/
|
||||
void flight_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
|
||||
void flight_status_update(const mavros_msgs::State::ConstPtr& msg);
|
||||
|
||||
/*current position callback*/
|
||||
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
|
||||
|
|
@ -120,11 +134,17 @@ private:
|
|||
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
|
||||
|
||||
/* RC commands service */
|
||||
bool rc_callback(mavros_msgs::CommandInt::Request &req,
|
||||
mavros_msgs::CommandInt::Response &res);
|
||||
bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res);
|
||||
|
||||
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
|
||||
|
||||
void GetSubscriptionParameters(ros::NodeHandle node_handle);
|
||||
|
||||
void Arm();
|
||||
|
||||
void SetMode();
|
||||
|
||||
void Subscribe(ros::NodeHandle n_c);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,11 @@
|
|||
topics:
|
||||
gps : /global_position
|
||||
battery : /power_status
|
||||
status : /flight_status
|
||||
fcclient : /dji_mavcmd
|
||||
armclient: /dji_mavarming
|
||||
modeclient: /dji_mavmode
|
||||
type:
|
||||
gps : sensor_msgs/NavSatFix
|
||||
battery : mavros_msgs/BatteryStatus
|
||||
status : mavros_msgs/ExtendedState
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
topics:
|
||||
gps : /mavros/global_position/global
|
||||
battery : /mavros/battery
|
||||
status : /mavros/state
|
||||
fcclient: /mavros/cmd/command
|
||||
armclient: /mavros/cmd/arming
|
||||
modeclient: /mavros/set_mode
|
||||
type:
|
||||
gps : sensor_msgs/NavSatFix
|
||||
# for SITL Solo
|
||||
battery : mavros_msgs/BatteryState
|
||||
# for solo
|
||||
#battery : mavros_msgs/BatteryStatus
|
||||
status : mavros_msgs/State
|
||||
environment :
|
||||
environment : solo-simulator
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
<!-- Launch file for ROSBuzz -->
|
||||
|
||||
<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" />
|
||||
<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="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
<launch>
|
||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
|
||||
<param name="bzzfile_name" value="../ROS_WS/src/ROSBuzz/src/test1.bzz" />
|
||||
<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/test1.bzz" />
|
||||
<param name="rcclient" value="true" />
|
||||
<param name="rcservice_name" value="/buzzcmd" />
|
||||
<param name="fcclient_name" value="/dji_mavcmd" />
|
||||
|
|
@ -10,7 +10,7 @@
|
|||
<param name="out_payload" value="/outMavlink"/>
|
||||
<param name="robot_id" value="3"/>
|
||||
<param name="No_of_Robots" value="3"/>
|
||||
<param name="stand_by" value="../ROS_WS/src/ROSBuzz/src/stand_by.bo"/>
|
||||
<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
|
||||
</node>
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,3 @@
|
|||
|
||||
2017-01-08-14-24-24 enter TranslateFunc
|
||||
2017-01-08-14-24-24 start to read frames!
|
||||
|
|
@ -233,6 +233,9 @@ namespace buzz_utility{
|
|||
buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto));
|
||||
buzzvm_gstore(VM);
|
||||
|
|
|
|||
|
|
@ -99,8 +99,8 @@ void gps_from_rb(double range, double bearing, double out[3]) {
|
|||
double lon = cur_pos[1]*M_PI/180.0;
|
||||
// bearing = bearing*M_PI/180.0;
|
||||
out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing));
|
||||
out[0] = out[0]*180.0/M_PI;
|
||||
out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0]));
|
||||
out[0] = out[0]*180.0/M_PI;
|
||||
out[1] = out[1]*180.0/M_PI;
|
||||
out[2] = height; //constant height.
|
||||
}
|
||||
|
|
@ -157,7 +157,7 @@ int buzzuav_moveto(buzzvm_t vm) {
|
|||
}
|
||||
}*/
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
|
||||
printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]);
|
||||
printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]);
|
||||
buzz_cmd=2;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
|
@ -171,40 +171,40 @@ int buzzuav_goto(buzzvm_t vm) {
|
|||
}
|
||||
/******************************/
|
||||
|
||||
double* getgoto(){
|
||||
return goto_pos;
|
||||
double* getgoto() {
|
||||
return goto_pos;
|
||||
}
|
||||
/******************************/
|
||||
int getcmd(){
|
||||
return cur_cmd;
|
||||
int getcmd() {
|
||||
return cur_cmd;
|
||||
}
|
||||
|
||||
void set_goto(double pos[]){
|
||||
goto_pos[0]=pos[0];
|
||||
goto_pos[1]=pos[1];
|
||||
goto_pos[2]=pos[2];
|
||||
|
||||
void set_goto(double pos[]) {
|
||||
goto_pos[0] = pos[0];
|
||||
goto_pos[1] = pos[1];
|
||||
goto_pos[2] = pos[2];
|
||||
|
||||
}
|
||||
|
||||
int bzz_cmd(){
|
||||
int cmd = buzz_cmd;
|
||||
buzz_cmd=0;
|
||||
return cmd;
|
||||
int bzz_cmd() {
|
||||
int cmd = buzz_cmd;
|
||||
buzz_cmd = 0;
|
||||
return cmd;
|
||||
}
|
||||
|
||||
void rc_set_goto(double pos[]){
|
||||
rc_goto_pos[0]=pos[0];
|
||||
rc_goto_pos[1]=pos[1];
|
||||
rc_goto_pos[2]=pos[2];
|
||||
|
||||
void rc_set_goto(double pos[]) {
|
||||
rc_goto_pos[0] = pos[0];
|
||||
rc_goto_pos[1] = pos[1];
|
||||
rc_goto_pos[2] = pos[2];
|
||||
|
||||
}
|
||||
void rc_call(int rc_cmd_in){
|
||||
rc_cmd=rc_cmd_in;
|
||||
void rc_call(int rc_cmd_in) {
|
||||
rc_cmd = rc_cmd_in;
|
||||
}
|
||||
|
||||
void set_obstacle_dist(float dist[]){
|
||||
for(int i=0; i<5;i++)
|
||||
obst[i]=dist[i];
|
||||
void set_obstacle_dist(float dist[]) {
|
||||
for (int i = 0; i < 5; i++)
|
||||
obst[i] = dist[i];
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -215,25 +215,25 @@ int buzzuav_takeoff(buzzvm_t vm) {
|
|||
buzzvm_lnum_assert(vm, 1);
|
||||
buzzvm_lload(vm, 1); /* Altitude */
|
||||
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
|
||||
goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value;
|
||||
goto_pos[2] = buzzvm_stack_at(vm, 1) -> f.value;
|
||||
height = goto_pos[2];
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
|
||||
printf(" Buzz requested Take off !!! \n");
|
||||
buzz_cmd=1;
|
||||
buzz_cmd = 1;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
int buzzuav_land(buzzvm_t vm) {
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
|
||||
printf(" Buzz requested Land !!! \n");
|
||||
buzz_cmd=1;
|
||||
buzz_cmd = 1;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
int buzzuav_gohome(buzzvm_t vm) {
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
|
||||
printf(" Buzz requested gohome !!! \n");
|
||||
buzz_cmd=1;
|
||||
buzz_cmd = 1;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -125,11 +125,8 @@ namespace rosbzz_node{
|
|||
else if(rcclient==false){ROS_INFO("RC service is disabled");}
|
||||
}
|
||||
else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
/*Obtain fc client name from parameter server*/
|
||||
if(n_c.getParam("fcclient_name", fcclient_name));
|
||||
else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
/*Obtain robot_id from parameter server*/
|
||||
n_c.getParam("robot_id", robot_id);
|
||||
n_c.getParam("robot_id", robot_id);
|
||||
/*Obtain out payload name*/
|
||||
n_c.getParam("out_payload", out_payload);
|
||||
/*Obtain in payload name*/
|
||||
|
|
@ -139,24 +136,81 @@ namespace rosbzz_node{
|
|||
/*Obtain standby script to run during update*/
|
||||
n_c.getParam("stand_by", stand_by);
|
||||
|
||||
GetSubscriptionParameters(n_c);
|
||||
// initialize topics to null?
|
||||
|
||||
}
|
||||
|
||||
void roscontroller::GetSubscriptionParameters(ros::NodeHandle node_handle){
|
||||
//todo: make it as an array in yaml?
|
||||
m_sMySubscriptions.clear();
|
||||
std::string gps_topic, gps_type;
|
||||
node_handle.getParam("topics/gps", gps_topic);
|
||||
node_handle.getParam("type/gps", gps_type);
|
||||
m_smTopic_infos.insert(pair <std::string, std::string>(gps_topic, gps_type));
|
||||
|
||||
std::string battery_topic, battery_type;
|
||||
node_handle.getParam("topics/battery", battery_topic);
|
||||
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));
|
||||
|
||||
/*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/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");}
|
||||
}
|
||||
|
||||
void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){
|
||||
/*subscribers*/
|
||||
current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this);
|
||||
battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this);
|
||||
|
||||
Subscribe(n_c);
|
||||
|
||||
//current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this);
|
||||
//battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this);
|
||||
payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this);
|
||||
flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_status_update,this);
|
||||
obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
|
||||
//flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this);
|
||||
|
||||
obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
|
||||
/*publishers*/
|
||||
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
|
||||
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
|
||||
/* Clients*/
|
||||
mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
|
||||
/* 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);
|
||||
|
||||
multi_msg=true;
|
||||
}
|
||||
|
||||
void roscontroller::Subscribe(ros::NodeHandle n_c){
|
||||
|
||||
for(std::map<std::string, std::string>::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it){
|
||||
if(it->second == "mavros_msgs/ExtendedState"){
|
||||
flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this);
|
||||
}
|
||||
else if(it->second == "mavros_msgs/State"){
|
||||
flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this);
|
||||
}
|
||||
else if(it->second == "mavros_msgs/BatteryStatus"){
|
||||
battery_sub = n_c.subscribe(it->first, 1000, &roscontroller::battery, this);
|
||||
}
|
||||
else if(it->second == "sensor_msgs/NavSatFix"){
|
||||
current_position_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this);
|
||||
}
|
||||
|
||||
std::cout << "Subscribed to: " << it->first << endl;
|
||||
}
|
||||
}
|
||||
|
||||
void roscontroller::Compile_bzz(){
|
||||
/*Compile the buzz code .bzz to .bo*/
|
||||
stringstream bzzfile_in_compile;
|
||||
|
|
@ -180,6 +234,29 @@ namespace rosbzz_node{
|
|||
dbgfname = bzzfile_in_compile.str();
|
||||
|
||||
}
|
||||
|
||||
void roscontroller::Arm(){
|
||||
mavros_msgs::CommandBool arming_message;
|
||||
arming_message.request.value = true;
|
||||
if(arm_client.call(arming_message)) {
|
||||
ROS_INFO("Service called!");
|
||||
} else {
|
||||
ROS_INFO("Service call failed!");
|
||||
}
|
||||
}
|
||||
|
||||
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("Service called!");
|
||||
} else {
|
||||
ROS_INFO("Service call failed!");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*Prepare messages for each step and publish*/
|
||||
/*******************************************************************************************************/
|
||||
/* Message format of payload (Each slot is uint64_t) */
|
||||
|
|
@ -195,15 +272,16 @@ namespace rosbzz_node{
|
|||
int tmp = buzzuav_closures::bzz_cmd();
|
||||
double* goto_pos = buzzuav_closures::getgoto();
|
||||
if (tmp == 1){
|
||||
cmd_srv.request.z = goto_pos[2];
|
||||
cmd_srv.request.param7 = goto_pos[2];
|
||||
//cmd_srv.request.z = goto_pos[2];
|
||||
cmd_srv.request.command = buzzuav_closures::getcmd();
|
||||
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
|
||||
else ROS_ERROR("Failed to call service from flight controller");
|
||||
} else if (tmp == 2) { /*FC call for goto*/
|
||||
/*prepare the goto publish message */
|
||||
cmd_srv.request.x = goto_pos[0]*pow(10,7);
|
||||
cmd_srv.request.y = goto_pos[1]*pow(10,7);
|
||||
cmd_srv.request.z = goto_pos[2];
|
||||
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");
|
||||
|
|
@ -485,8 +563,22 @@ namespace rosbzz_node{
|
|||
buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining);
|
||||
//ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, msg->current, msg ->remaining);
|
||||
}
|
||||
/*flight status update*/
|
||||
void roscontroller::flight_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg){
|
||||
|
||||
/*flight extended status update*/
|
||||
void roscontroller::flight_status_update(const mavros_msgs::State::ConstPtr& msg){
|
||||
// http://wiki.ros.org/mavros/CustomModes
|
||||
// TODO: Handle landing
|
||||
std::cout << "Message: " << msg->mode << std::endl;
|
||||
if(msg->mode == "GUIDED")
|
||||
buzzuav_closures::flight_status_update(1);
|
||||
else if (msg->mode == "LAND")
|
||||
buzzuav_closures::flight_status_update(4);
|
||||
else // ground standby = LOITER?
|
||||
buzzuav_closures::flight_status_update(1);//?
|
||||
}
|
||||
|
||||
/*flight extended status update*/
|
||||
void roscontroller::flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg){
|
||||
buzzuav_closures::flight_status_update(msg->landed_state);
|
||||
}
|
||||
/*current position callback*/
|
||||
|
|
@ -571,9 +663,10 @@ namespace rosbzz_node{
|
|||
neighbours_pos_payload[i]=cartesian_neighbours_pos[i]-cartesian_cur_pos[i];
|
||||
}
|
||||
double *cvt_neighbours_pos_payload = cvt_neighbours_pos_test;
|
||||
// cvt_spherical_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload);
|
||||
//double cvt_neighbours_pos_payload[3];
|
||||
//cvt_spherical_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload);
|
||||
/*Extract robot id of the neighbour*/
|
||||
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));
|
||||
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));
|
||||
cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl;
|
||||
// cout << "Rel Test Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_test[0] << ", "<< cvt_neighbours_pos_test[2] << ", "<< cvt_neighbours_pos_test[3] << endl;
|
||||
/*pass neighbour position to local maintaner*/
|
||||
|
|
@ -588,8 +681,8 @@ namespace rosbzz_node{
|
|||
}
|
||||
|
||||
/* RC command service */
|
||||
bool roscontroller::rc_callback(mavros_msgs::CommandInt::Request &req,
|
||||
mavros_msgs::CommandInt::Response &res){
|
||||
bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
|
||||
mavros_msgs::CommandLong::Response &res){
|
||||
int rc_cmd;
|
||||
/* if(req.command==oldcmdID)
|
||||
return true;
|
||||
|
|
@ -598,6 +691,9 @@ namespace rosbzz_node{
|
|||
case mavros_msgs::CommandCode::NAV_TAKEOFF:
|
||||
ROS_INFO("RC_call: TAKE OFF!!!!");
|
||||
rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
|
||||
/* arming */
|
||||
SetMode();
|
||||
Arm();
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
break;
|
||||
|
|
@ -616,9 +712,10 @@ namespace rosbzz_node{
|
|||
case mavros_msgs::CommandCode::NAV_WAYPOINT:
|
||||
ROS_INFO("RC_Call: GO TO!!!! ");
|
||||
double rc_goto[3];
|
||||
rc_goto[0]=req.x/pow(10,7);
|
||||
rc_goto[1]=req.y/pow(10,7);
|
||||
rc_goto[2]=req.z;
|
||||
rc_goto[0] = req.param5;
|
||||
rc_goto[1] = req.param6;
|
||||
rc_goto[2] = req.param7;
|
||||
|
||||
buzzuav_closures::rc_set_goto(rc_goto);
|
||||
rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
|
|
|
|||
|
|
@ -12,11 +12,11 @@ function updated_neigh(){
|
|||
neighbors.broadcast(updated, update_no)
|
||||
}
|
||||
|
||||
TARGET_ALTITUDE = 10.0
|
||||
TARGET_ALTITUDE = 2.0
|
||||
|
||||
# Lennard-Jones parameters
|
||||
TARGET = 50.0 #0.000001001
|
||||
EPSILON = 0.5 #0.001
|
||||
TARGET = 10.0 #0.000001001
|
||||
EPSILON = 0.05 #0.001
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
function lj_magnitude(dist, target, epsilon) {
|
||||
|
|
@ -41,8 +41,8 @@ function hexagon() {
|
|||
if(neighbors.count() > 0)
|
||||
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)
|
||||
print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
|
||||
uav_moveto(accum.x, accum.y)
|
||||
}
|
||||
|
||||
########################################
|
||||
|
|
@ -101,29 +101,28 @@ neighbors.listen("cmd",
|
|||
|
||||
|
||||
)
|
||||
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0)
|
||||
statef=hexagon
|
||||
}
|
||||
|
||||
function takeoff() {
|
||||
log("TakeOff: ", flight.status)
|
||||
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
||||
barrier_set(ROBOTS,hexagon)
|
||||
barrier_ready()
|
||||
}
|
||||
else if( flight.status !=3){
|
||||
if( flight.status !=3){
|
||||
log("Altitude: ", TARGET_ALTITUDE)
|
||||
neighbors.broadcast("cmd", 22)
|
||||
uav_takeoff(TARGET_ALTITUDE)
|
||||
}
|
||||
}
|
||||
function land() {
|
||||
if( flight.status == 1) {
|
||||
barrier_set(ROBOTS,idle)
|
||||
barrier_ready()
|
||||
}
|
||||
else if(flight.status!=0 and flight.status!=4){
|
||||
log("Land: ", flight.status)
|
||||
if(flight.status == 2 or flight.status == 3){
|
||||
neighbors.broadcast("cmd", 21)
|
||||
uav_land()
|
||||
}
|
||||
else
|
||||
statef=idle
|
||||
}
|
||||
|
||||
# Executed once at init time.
|
||||
|
|
@ -134,19 +133,22 @@ function init() {
|
|||
# Executed at each time step.
|
||||
function step() {
|
||||
|
||||
if(flight.rc_cmd==22 and flight.status==1) {
|
||||
if(flight.rc_cmd==22) {
|
||||
log("cmd 22")
|
||||
flight.rc_cmd=0
|
||||
statef=takeoff
|
||||
statef = takeoff
|
||||
neighbors.broadcast("cmd", 22)
|
||||
} else if(flight.rc_cmd==21 and flight.status==2) {
|
||||
} else if(flight.rc_cmd==21) {
|
||||
log("cmd 21")
|
||||
log("To land")
|
||||
flight.rc_cmd=0
|
||||
statef=land
|
||||
statef = land
|
||||
neighbors.broadcast("cmd", 21)
|
||||
} else if(flight.rc_cmd==16 and flight.status==2) {
|
||||
} else if(flight.rc_cmd==16) {
|
||||
flight.rc_cmd=0
|
||||
uav_goto()
|
||||
}
|
||||
statef()
|
||||
}
|
||||
statef()
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
|
|
|
|||
|
|
@ -0,0 +1,46 @@
|
|||
|
||||
# We need this for 2D vectors
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
||||
####################################################################################################
|
||||
# Updater related
|
||||
# This should be here for the updater to work, changing position of code will crash the updater
|
||||
####################################################################################################
|
||||
updated="update_ack"
|
||||
update_no=0
|
||||
function updated_neigh(){
|
||||
neighbors.broadcast(updated, update_no)
|
||||
}
|
||||
|
||||
TARGET_ALTITUDE = 2.0
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
|
||||
if(flight.rc_cmd==22) {
|
||||
flight.rc_cmd=0
|
||||
uav_takeoff(TARGET_ALTITUDE)
|
||||
} else if(flight.rc_cmd==21) {
|
||||
flight.rc_cmd=0
|
||||
uav_land()
|
||||
} else if(flight.rc_cmd==16) {
|
||||
flight.rc_cmd=0
|
||||
uav_goto()
|
||||
}
|
||||
|
||||
# test moveto cmd
|
||||
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0)
|
||||
uav_moveto(0.5, 0.5)
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
function reset() {
|
||||
}
|
||||
|
||||
# Executed once at the end of experiment.
|
||||
function destroy() {
|
||||
}
|
||||
107
src/vec2.bzz
107
src/vec2.bzz
|
|
@ -1,107 +0,0 @@
|
|||
#
|
||||
# Create a new namespace for vector2 functions
|
||||
#
|
||||
math.vec2 = {}
|
||||
|
||||
#
|
||||
# Creates a new vector2.
|
||||
# PARAM x: The x coordinate.
|
||||
# PARAM y: The y coordinate.
|
||||
# RETURN: A new vector2.
|
||||
#
|
||||
math.vec2.new = function(x, y) {
|
||||
return { .x = x, .y = y }
|
||||
}
|
||||
|
||||
#
|
||||
# Creates a new vector2 from polar coordinates.
|
||||
# PARAM l: The length of the vector2.
|
||||
# PARAM a: The angle of the vector2.
|
||||
# RETURN: A new vector2.
|
||||
#
|
||||
math.vec2.newp = function(l, a) {
|
||||
return {
|
||||
.x = l * math.cos(a),
|
||||
.y = l * math.sin(a)
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
# Calculates the length of the given vector2.
|
||||
# PARAM v: The vector2.
|
||||
# RETURN: The length of the vector.
|
||||
#
|
||||
math.vec2.length = function(v) {
|
||||
return math.sqrt(v.x * v.x + v.y * v.y)
|
||||
}
|
||||
|
||||
#
|
||||
# Calculates the angle of the given vector2.
|
||||
# PARAM v: The vector2.
|
||||
# RETURN: The angle of the vector.
|
||||
#
|
||||
math.vec2.angle = function(v) {
|
||||
return math.atan2(v.y, v.x)
|
||||
}
|
||||
|
||||
#
|
||||
# Returns the normalized form of a vector2.
|
||||
# PARAM v: The vector2.
|
||||
# RETURN: The normalized form.
|
||||
#
|
||||
math.vec2.norm = function(v) {
|
||||
var l = math.length(v)
|
||||
return {
|
||||
.x = v.x / l,
|
||||
.y = v.y / l
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
# Calculates v1 + v2.
|
||||
# PARAM v1: A vector2.
|
||||
# PARAM v2: A vector2.
|
||||
# RETURN: v1 + v2
|
||||
#
|
||||
math.vec2.add = function(v1, v2) {
|
||||
return {
|
||||
.x = v1.x + v2.x,
|
||||
.y = v1.y + v2.y
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
# Calculates v1 - v2.
|
||||
# PARAM v1: A vector2.
|
||||
# PARAM v2: A vector2.
|
||||
# RETURN: v1 + v2
|
||||
#
|
||||
math.vec2.sub = function(v1, v2) {
|
||||
return {
|
||||
.x = v1.x - v2.x,
|
||||
.y = v1.y - v2.y
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
# Scales a vector by a numeric constant.
|
||||
# PARAM v: A vector2.
|
||||
# PARAM s: A number (float or int).
|
||||
# RETURN: s * v
|
||||
#
|
||||
math.vec2.scale = function(v, s) {
|
||||
return {
|
||||
.x = v.x * s,
|
||||
.y = v.y * s
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
# Calculates v1 . v2 (the dot product)
|
||||
# PARAM v1: A vector2.
|
||||
# PARAM v2: A vector2.
|
||||
# RETURN: v1 . v2
|
||||
#
|
||||
math.vec2.dot = function(v1, v2) {
|
||||
return v1.x * v2.x + v1.y * v2.y
|
||||
}
|
||||
Loading…
Reference in New Issue