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

fix ROS std msgs

This commit is contained in:
David St-Onge 2017-01-25 21:10:07 -05:00
parent 7e1d8c5244
commit 149abb9d53
2 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/roscontroller.cpp
View File

@ -37,12 +37,14 @@ namespace rosbzz_node{
{
/*Update neighbors position inside Buzz*/
buzz_utility::neighbour_pos_callback(neighbours_pos_map);
auto current_time = ros::Time::now();
map< int, buzz_utility::Pos_struct >::iterator it;
rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear();
for (it=raw_neighbours_pos_map.begin(); it!=raw_neighbours_pos_map.end(); ++it){
sensor_msgs::NavSatFix neigh_tmp;
//cout<<"iterator it val: "<< it-> first << " After convertion: " <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<<endl;
neigh_tmp.header.stamp = current_time;
neigh_tmp.header.frame_id = "/world";
neigh_tmp.position_covariance_type=it->first; //custom robot id storage
neigh_tmp.longitude=(it->second).x;
neigh_tmp.latitude=(it->second).y;

12
src/test1.bzz
View File

@ -1,10 +1,12 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz"
TARGET_ALTITUDE = 10.0
# Lennard-Jones parameters
TARGET = 0.000000100001
EPSILON = 0.01
TARGET = 0.000000100005
EPSILON = 0.1
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
@ -29,7 +31,7 @@ function hexagon() {
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)
uav_goto(accum.x+position.latitude, accum.y+position.longitude, TARGET_ALTITUDE)
}
########################################
@ -89,12 +91,12 @@ neighbors.listen("cmd",
}
function takeoff() {
if( flight.status == 2 and position.altitude >= 10.0) {
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/50.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
}
else if( flight.status !=3)
uav_takeoff(10.1)
uav_takeoff(TARGET_ALTITUDE)
}
function land() {
if( flight.status == 1) {