From 9493c2e37c2568f7aab78bfc799e275f20c9abb2 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Wed, 25 Jan 2017 14:08:15 -0500 Subject: [PATCH] fix waypoint bug --- src/roscontroller.cpp | 18 +++---- src/test1.bzz | 39 +++++++++++++-- src/vec2.bzz | 107 ++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 153 insertions(+), 11 deletions(-) create mode 100644 src/vec2.bzz diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 115ed65..39415e1 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -155,22 +155,24 @@ namespace rosbzz_node{ /* flight controller client call if requested from Buzz*/ /*FC call for takeoff,land and gohome*/ - if (buzzuav_closures::bzz_cmd()==1){ + int tmp = buzzuav_closures::bzz_cmd(); + if (tmp == 1){ 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"); - } - /*FC call for goto*/ - else if (buzzuav_closures::bzz_cmd() == 2) { + } else if (tmp == 2) { /*FC call for goto*/ /*prepare the goto publish message */ double* goto_pos = buzzuav_closures::getgoto(); - cmd_srv.request.param1 = goto_pos[0]; - cmd_srv.request.param2 = goto_pos[1]; - cmd_srv.request.param3 = goto_pos[2]; + 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.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"); - } + cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; + 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"); + } /*obtain Pay load to be sent*/ uint64_t* payload_out_ptr= buzz_utility::out_msg_process(); uint64_t position[3]; diff --git a/src/test1.bzz b/src/test1.bzz index 6c9a0dc..53d71db 100644 --- a/src/test1.bzz +++ b/src/test1.bzz @@ -1,4 +1,37 @@ - +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" + +# Lennard-Jones parameters +TARGET = 28.3 +EPSILON = 15.0 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + math.vec2.scale(accum, 1.0 / neighbors.count()) + # Move according to vector + uav_goto(accum.x+position.latitude, accum.y+position.longitude, position.altitude) +} + ######################################## # # BARRIER-RELATED FUNCTIONS @@ -9,7 +42,7 @@ # Constants # BARRIER_VSTIG = 1 -ROBOTS = 2 # number of robots in the swarm +ROBOTS = 3 # number of robots in the swarm # # Sets a barrier @@ -57,7 +90,7 @@ neighbors.listen("cmd", function takeoff() { if( flight.status == 2) { - barrier_set(ROBOTS,idle) + barrier_set(ROBOTS,hexagon) barrier_ready() } else if(flight.status!=3) diff --git a/src/vec2.bzz b/src/vec2.bzz new file mode 100644 index 0000000..e2fb9b0 --- /dev/null +++ b/src/vec2.bzz @@ -0,0 +1,107 @@ +# +# 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 +}