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

Merge branch 'solo-playground' of https://github.com/MISTLab/ROSBuzz

This commit is contained in:
David St-Onge 2017-02-19 15:22:45 -05:00
parent f2c4977fe4 ea95bb1b48
commit b196fc7ffa
10 changed files with 241 additions and 185 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -1 +1,3 @@
src/test*
.cproject
.project

34
include/roscontroller.h
View File

@ -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);
};
}

11
launch/launch_config/m100.yaml Normal file
View File

@ -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

16
launch/launch_config/solo.yaml Normal file
View File

@ -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

16
launch/rosbuzz.launch Normal file
View File

@ -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>

4
launch/rosbuzzm100.launch
View File

@ -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>

56
src/buzzuav_closures.cpp
View File

@ -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);
}

138
src/roscontroller.cpp
View File

@ -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>(armingclient);
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*/
@ -588,8 +680,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 +690,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 +711,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);

42
src/test1.bzz
View File

@ -106,24 +106,23 @@ if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/
}
function takeoff() {
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 == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
# barrier_set(ROBOTS,hexagon)
# barrier_ready()
#}
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){
neighbors.broadcast("cmd", 21)
uav_land()
}
#log("Land: ", flight.status)
#if(flight.status != 0 and flight.status != 4){
# neighbors.broadcast("cmd", 21)
# uav_land()
#}
uav_land()
}
# 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.

107
src/vec2.bzz
View File

@ -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
}