changes to NED and REL_ALT
This commit is contained in:
isvogor
parent
96ebb3f470
commit
9f3a02e8ec
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@
|
|||
<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"/>
|
||||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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 |
|
||||
|
|
|
|||
|
|
@ -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() {
|
||||
|
|
|
|||
Loading…
Reference in New Issue