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

changes to NED and REL_ALT

This commit is contained in:
isvogor 2017-04-03 19:50:09 -04:00
parent 96ebb3f470
commit 9f3a02e8ec
5 changed files with 76 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
include/roscontroller.h
View File

@ -18,6 +18,7 @@
#include "mavros_msgs/PositionTarget.h"
#include "mavros_msgs/StreamRate.h"
#include "mavros_msgs/ParamGet.h"
#include "std_msgs/Float64.h"
#include <sensor_msgs/LaserScan.h>
#include <rosbuzz/neigh_pos.h>
#include <sstream>
@ -59,6 +60,7 @@ private:
}; typedef struct num_robot_count Num_robot_count ;
double cur_pos[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;
@ -76,6 +78,7 @@ private:
int 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 stream_client_name;
std::string relative_altitude_sub_name;
bool rcclient;
bool multi_msg;
Num_robot_count count_robots;
@ -91,6 +94,7 @@ private:
ros::Subscriber flight_status_sub;
ros::Subscriber obstacle_sub;
ros::Subscriber Robot_id_sub;
ros::Subscriber relative_altitude_sub;
ros::ServiceClient stream_client;
/*Commands for flight controller*/
@ -154,6 +158,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);

2
launch/launch_config/solo.yaml
View File

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

5
launch/rosbuzz.launch
View File

@ -3,13 +3,12 @@
<launch>
<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/testalone.bzz" />
<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testmoveto.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="xbee_status_srv" value="/xbee_status"/>
<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
</node>
</node>
</launch>

71
src/roscontroller.cpp
View File

@ -43,7 +43,7 @@ namespace rosbzz_node{
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::spinOnce();
ros::Rate loop_rate(10);
@ -52,6 +52,8 @@ namespace rosbzz_node{
}
robot_id=robot_id_srv_response.value.integer;
*/
robot_id = 32;
//robot_id = 84;
/* Set the Buzz bytecode */
@ -159,6 +161,11 @@ namespace rosbzz_node{
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");}
@ -215,6 +222,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;
}
@ -370,6 +380,7 @@ namespace rosbzz_node{
/*FC call for takeoff,land, gohome and arm/disarm*/
int tmp = buzzuav_closures::bzz_cmd();
double* goto_pos = buzzuav_closures::getgoto();
ros::Rate loop_rate(0.25);
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.command = buzzuav_closures::getcmd();
@ -378,19 +389,29 @@ namespace rosbzz_node{
switch(buzzuav_closures::getcmd()){
case mavros_msgs::CommandCode::NAV_LAND:
if(current_mode != "LAND")
SetMode("LAND", 0);
{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();
loop_rate.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;
}
if (mav_client.call(cmd_srv))
{ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
else
ROS_ERROR("Failed to call service from flight controller");
} else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*FC call for goto*/
/*prepare the goto publish message */
cmd_srv.request.param5 = goto_pos[0];
@ -407,16 +428,20 @@ namespace rosbzz_node{
} else
ROS_ERROR("Failed to call service from flight controller");
} else if (tmp == buzzuav_closures::COMMAND_ARM) { /*FC call for arm*/
SetMode("LOITER", 0);
armstate = 1;
Arm();
if(!armstate){
SetMode("LOITER", 0);
armstate = 1;
Arm();
}
} else if (tmp == buzzuav_closures::COMMAND_DISARM) {
armstate = 0;
SetMode("LOITER", 0);
Arm();
if(armstate){
armstate = 0;
SetMode("LOITER", 0);
Arm();
}
} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
/*prepare the goto publish message */
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
/*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);
}
}
/*----------------------------------------------
@ -460,7 +485,6 @@ namespace rosbzz_node{
cur_pos [2] =altitude;
}
/*-----------------------------------------------------------
/ Compute Range and Bearing of a neighbor in a local reference frame
/ from GPS coordinates
@ -519,8 +543,17 @@ 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);
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
@ -735,7 +768,7 @@ namespace rosbzz_node{
// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
mavros_msgs::PositionTarget moveMsg;
moveMsg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_OFFSET_NED;
moveMsg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_OFFSET_NED;
moveMsg.type_mask = mavros_msgs::PositionTarget::IGNORE_AFX |
mavros_msgs::PositionTarget::IGNORE_AFY |
mavros_msgs::PositionTarget::IGNORE_AFZ |

18
src/testflockfev.bzz
View File

@ -11,7 +11,7 @@ function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 5.0
TARGET_ALTITUDE = 3.0
CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
@ -43,7 +43,14 @@ 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)
if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
timeW =0
statef=land
} else {
timeW = timeW+1
uav_moveto(0.5,0.0)
}
}
########################################
@ -106,16 +113,17 @@ function takeoff() {
CURSTATE = "TAKEOFF"
statef=takeoff
log("TakeOff: ", flight.status)
uav_takeoff(TARGET_ALTITUDE)
log("Relative position: ", position.altitude)
if( flight.status == 1) {
if( flight.status == 1 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
#statef=hexagon
}
else if( flight.status == 7 ){
else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {