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

bringup debug mode to launch file, fine tuned LJ params and rename hooks (generic hardware)

This commit is contained in:
dave 2018-06-01 20:12:06 -04:00
parent db452bd063
commit 42d990676c
8 changed files with 37 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
buzz_scripts/include/act/states.bzz
View File

@ -45,8 +45,13 @@ function launch() {
}
}
gohomeT=100
function stop() {
BVMSTATE = "STOP"
if(gohomeT > 0) { # TODO: Make a real check if home is reached
gohome()
gohomeT = gohomeT - 1
}
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
neighbors.broadcast("cmd", 21)
uav_land()
@ -64,7 +69,7 @@ function take_picture() {
BVMSTATE="PICTURE"
uav_setgimbal(0.0, 0.0, -90.0, 20.0)
if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize
uav_takepicture()
takepicture()
} else if(pic_time>=PICTURE_WAIT) { # wait for the picture
BVMSTATE="IDLE"
pic_time=0
@ -137,9 +142,10 @@ function pursuit() {
# Lennard-Jones interaction magnitude
TARGET = 8.0
EPSILON = 0.1
EPSILON = 30.0
function lj_magnitude(dist, target, epsilon) {
lj = -(epsilon / dist) * ((target / dist)^4 - 1.02 * (target / dist)^2)
lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
log(target, dist, epsilon, lj)
return lj
}
@ -154,17 +160,17 @@ function lj_sum(rid, data, accum) {
}
# Calculates and actuates the flocking interaction
old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes
function formation() {
BVMSTATE="FORMATION"
# Calculate accumulator
accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count())
old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/2.0)
goto_abs(old_lj.x, old_lj.y, 0.0, 0.0)
accum_lj = LimitSpeed(accum_lj, 1.0/3.0)
goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
}
# listens to commands from the remote control (web, commandline, etc)
function rc_cmd_listen() {
if(flight.rc_cmd==22) {
log("cmd 22")
@ -216,6 +222,7 @@ function rc_cmd_listen() {
}
}
# listens to other fleet members broadcasting commands
function nei_cmd_listen() {
neighbors.listen("cmd",
function(vid, value, rid) {

2
buzz_scripts/main.bzz
View File

@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz"
include "vstigenv.bzz"
#State launched after takeoff
AUTO_LAUNCH_STATE = "IDLE"
AUTO_LAUNCH_STATE = "FORMATION"
#####
# Vehicule type:

1
include/roscontroller.h
View File

@ -103,6 +103,7 @@ private:
bool multi_msg;
uint8_t no_cnt = 0;
uint8_t old_val = 0;
bool debug = false;
std::string bzzfile_name;
std::string fcclient_name;
std::string armclient;

1
launch/rosbuzz.launch
View File

@ -10,6 +10,7 @@
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="$(find rosbuzz)/launch/launch_config/topics.yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
<param name="debug" value="true" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/>

1
launch/rosbuzzd.launch
View File

@ -10,6 +10,7 @@
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" launch-prefix="gdb -ex run --args">
<rosparam file="$(find rosbuzz)/launch/launch_config/topics.yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
<param name="debug" value="true" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/>

12
src/buzz_utility.cpp
View File

@ -204,25 +204,25 @@ static int buzz_register_hooks()
buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "storegoal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "setgimbal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_setgimbal));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "takepicture", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "disarm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_disarm));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "gohome", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_gohome));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));

4
src/buzzuav_closures.cpp
View File

@ -104,10 +104,10 @@ float constrainAngle(float x)
/ Wrap the angle between -pi, pi
----------------------------------------------------------- */
{
x = fmod(x, 2 * M_PI);
x = fmod(x + M_PI, 2 * M_PI);
if (x < 0.0)
x += 2 * M_PI;
return x;
return x - M_PI;
}
void rb_from_gps(double nei[], double out[], double cur[])

19
src/roscontroller.cpp
View File

@ -11,7 +11,6 @@
namespace rosbzz_node
{
const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
const bool debug = false;
roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
/*
@ -188,6 +187,14 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
ROS_ERROR("Provide a .bzz file to run in Launch file");
system("rosnode kill rosbuzz_node");
}
// Obtain debug mode from launch file parameter
if (n_c.getParam("debug", debug))
;
else
{
ROS_ERROR("Provide a debug mode in Launch file");
system("rosnode kill rosbuzz_node");
}
// Obtain rc service option from parameter server
if (n_c.getParam("rcclient", rcclient))
{
@ -670,7 +677,7 @@ script
ROS_ERROR("Failed to call service from flight controller");
break;
case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!!
case buzzuav_closures::COMMAND_GOTO: // TODO: NOT FULLY IMPLEMENTED/TESTED !!!
goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.param5 = goto_pos[0];
cmd_srv.request.param6 = goto_pos[1];
@ -794,10 +801,10 @@ float roscontroller::constrainAngle(float x)
/ Wrap the angle between -pi, pi
----------------------------------------------------------- */
{
x = fmod(x, 2 * M_PI);
x = fmod(x + M_PI, 2 * M_PI);
if (x < 0.0)
x += 2 * M_PI;
return x;
return x - M_PI;
}
void roscontroller::gps_rb(POSE nei_pos, double out[])
@ -809,10 +816,7 @@ void roscontroller::gps_rb(POSE nei_pos, double out[])
float ned_x = 0.0, ned_y = 0.0;
gps_ned_cur(ned_x, ned_y, nei_pos);
out[0] = sqrt(ned_x * ned_x + ned_y * ned_y);
// out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = atan2(ned_y, ned_x);
out[1] = constrainAngle(atan2(ned_y, ned_x));
// out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0;
}
@ -1055,6 +1059,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
nei_pos.longitude = neighbours_pos_payload[1];
nei_pos.altitude = neighbours_pos_payload[2];
double cvt_neighbours_pos_payload[3];
// Compute RB in robot body ref. frame
gps_rb(nei_pos, cvt_neighbours_pos_payload);
// Extract robot id of the neighbour
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3));