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

fix ROS std msgs

Browse Source
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*/ /*Update neighbors position inside Buzz*/
buzz_utility::neighbour_pos_callback(neighbours_pos_map); buzz_utility::neighbour_pos_callback(neighbours_pos_map);
auto current_time = ros::Time::now();
map< int, buzz_utility::Pos_struct >::iterator it; map< int, buzz_utility::Pos_struct >::iterator it;
rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear(); rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear();
for (it=raw_neighbours_pos_map.begin(); it!=raw_neighbours_pos_map.end(); ++it){ for (it=raw_neighbours_pos_map.begin(); it!=raw_neighbours_pos_map.end(); ++it){
sensor_msgs::NavSatFix neigh_tmp; sensor_msgs::NavSatFix neigh_tmp;
//cout<<"iterator it val: "<< it-> first << " After convertion: " <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<<endl; //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.position_covariance_type=it->first; //custom robot id storage
neigh_tmp.longitude=(it->second).x; neigh_tmp.longitude=(it->second).x;
neigh_tmp.latitude=(it->second).y; neigh_tmp.latitude=(it->second).y;

12
src/test1.bzz
View File

@ -2,9 +2,11 @@
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..." # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz" include "/home/ubuntu/buzz/src/include/vec2.bzz"
TARGET_ALTITUDE = 10.0
# Lennard-Jones parameters # Lennard-Jones parameters
TARGET = 0.000000100001 TARGET = 0.000000100005
EPSILON = 0.01 EPSILON = 0.1
# Lennard-Jones interaction magnitude # Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) { function lj_magnitude(dist, target, epsilon) {
@ -29,7 +31,7 @@ function hexagon() {
if(neighbors.count() > 0) if(neighbors.count() > 0)
math.vec2.scale(accum, 1.0 / neighbors.count()) math.vec2.scale(accum, 1.0 / neighbors.count())
# Move according to vector # 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() { 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_set(ROBOTS,hexagon)
barrier_ready() barrier_ready()
} }
else if( flight.status !=3) else if( flight.status !=3)
uav_takeoff(10.1) uav_takeoff(TARGET_ALTITUDE)
} }
function land() { function land() {
if( flight.status == 1) { if( flight.status == 1) {