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

Merge branch 'master' of https://github.com/MISTLab/ROSBuzz

Browse Source
This commit is contained in:
David St-Onge 2017-02-20 16:12:52 -05:00
commit 1f10d531ca
9 changed files with 137 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/roscontroller.h
View File

@ -1,6 +1,7 @@
#pragma once
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/UInt8.h>
#include "mavros_msgs/GlobalPositionTarget.h"
#include "mavros_msgs/CommandCode.h"
#include "mavros_msgs/CommandLong.h"
@ -53,6 +54,7 @@ private:
//int oldcmdID=0;
int rc_cmd;
int barrier;
int message_number=0;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by;
bool rcclient;
bool multi_msg;
@ -65,6 +67,7 @@ private:
ros::Subscriber payload_sub;
ros::Subscriber flight_status_sub;
ros::Subscriber obstacle_sub;
ros::Subscriber Robot_id_sub;
/*Commands for flight controller*/
//mavros_msgs::CommandInt cmd_srv;
mavros_msgs::CommandLong cmd_srv;
@ -136,6 +139,9 @@ private:
/* RC commands service */
bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res);
/*robot id sub callback*/
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
void GetSubscriptionParameters(ros::NodeHandle node_handle);

1
launch/rosbuzz_2_parallel.launch
View File

@ -3,6 +3,7 @@
<launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test1.bzz" />
<rosparam file="/home/vivek/catkin_ws/src/rosbuzz/launch/launch_config/m100.yaml"/>
<param name="rcclient" value="true" />
<param name="rcservice_name" value="rc_cmd" />
<param name="fcclient_name" value="/djicmd" />

3
log.txt Normal file
View File

@ -0,0 +1,3 @@
2017-01-08-14-24-24 enter TranslateFunc
2017-01-08-14-24-24 start to read frames!

8
src/buzz_update.cpp
View File

@ -294,7 +294,7 @@ buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
}
else{
gettimeofday(&t1, NULL);
//gettimeofday(&t1, NULL);
if(neigh==0 && (!is_msg_present())){
fprintf(stdout,"Sending code... \n");
code_message_outqueue_append();
@ -309,10 +309,10 @@ buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
if(tObj->i.value==no_of_robot) {
*(int*)(updater->mode) = CODE_RUNNING;
gettimeofday(&t2, NULL);
collect_data();
//collect_data();
timer_steps=0;
//neigh=-1;
//buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgfname, *(size_t*)(updater->bcode_size));
neigh=0;
buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgfname, *(size_t*)(updater->bcode_size));
//buzzvm_function_call(m_tBuzzVM, "updated", 0);
updated=1;
}

3
src/buzz_utility.cpp
View File

@ -233,6 +233,9 @@ namespace buzz_utility{
buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto));
buzzvm_gstore(VM);

2
src/buzzuav_closures.cpp
View File

@ -99,8 +99,8 @@ void gps_from_rb(double range, double bearing, double out[3]) {
double lon = cur_pos[1]*M_PI/180.0;
// bearing = bearing*M_PI/180.0;
out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing));
out[0] = out[0]*180.0/M_PI;
out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0]));
out[0] = out[0]*180.0/M_PI;
out[1] = out[1]*180.0/M_PI;
out[2] = height; //constant height.
}

102
src/roscontroller.cpp
View File

@ -68,36 +68,26 @@ namespace rosbzz_node{
//std::cout<<"long obt"<<neigh_tmp.longitude<<endl;
}
neigh_pos_pub.publish(neigh_pos_array);
//fprintf(stdout, "before update routine\n");
/*Check updater state and step code*/
update_routine(bcfname.c_str(), dbgfname.c_str());
/*Step buzz script */
//fprintf(stdout, "before code step\n");
if(get_update_status()){
buzz_utility::buzz_update_set((get_updater())->bcode, dbgfname.c_str(), *(size_t*)((get_updater())->bcode_size));
set_read_update_status();
mavros_msgs::Mavlink stop_req_packet;
stop_req_packet.payload64.push_back((uint64_t) XBEE_STOP_TRANSMISSION);
payload_pub.publish(stop_req_packet);
std::cout << "request xbee to stop multi-packet transmission" << std::endl;
}
/*Step buzz script */
buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/
//fprintf(stdout, "before publish msg\n");
prepare_msg_and_publish();
/*Set multi message available after update*/
if(get_update_status()){
set_read_update_status();
multi_msg=true;
}
/*run once*/
ros::spinOnce();
/*loop rate of ros*/
/*if( get_update_mode() == CODE_STANDBY){
ros::Rate loop_rate(10);
loop_rate.sleep();
}
else{*/
ros::Rate loop_rate(10);
loop_rate.sleep();
//}
/*sleep for the mentioned loop rate*/
timer_step+=1;
maintain_pos(timer_step);
@ -126,7 +116,8 @@ namespace rosbzz_node{
}
else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");}
/*Obtain robot_id from parameter server*/
n_c.getParam("robot_id", robot_id);
//n_c.getParam("robot_id", robot_id);
//robot_id=(int)buzz_utility::get_robotid();
/*Obtain out payload name*/
n_c.getParam("out_payload", out_payload);
/*Obtain in payload name*/
@ -178,13 +169,13 @@ namespace rosbzz_node{
//battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this);
payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this);
//flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this);
Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this);
obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
/*publishers*/
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
/* Service Clients*/
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armingclient);
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
@ -318,6 +309,13 @@ namespace rosbzz_node{
cout<<" [Debug:] sent message "<<out[k]<<endl;
}
}*/
/*Add Robot id and message number to the published message*/
if (message_number < 0) message_number=0;
else message_number++;
payload_out.sysid=(uint8_t)robot_id;
payload_out.msgid=(uint32_t)message_number;
/*publish prepared messages in respective topic*/
payload_pub.publish(payload_out);
delete[] out;
@ -334,27 +332,32 @@ namespace rosbzz_node{
/*allocate mem and clear it*/
buff_send =(uint8_t*)malloc(sizeof(uint16_t)+updater_msgSize);
memset(buff_send, 0,sizeof(uint16_t)+updater_msgSize);
/*Append updater msg size*/
*(uint16_t*)(buff_send + tot)=updater_msgSize;
//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
tot += sizeof(uint16_t);
/*Append updater msgs*/
memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
tot += updater_msgSize;
/*Destroy the updater out msg queue*/
destroy_out_msg_queue();
uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t)));
uint64_t* payload_64 = new uint64_t[total_size];
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
delete[] buff_send;
/*Send a constant number to differenciate updater msgs*/
update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT);
for(int i=0;i<total_size;i++){
update_packets.payload64.push_back(payload_64[i]);
}
payload_pub.publish(update_packets);
multi_msg=false;
delete[] payload_64;
/*Append updater msg size*/
*(uint16_t*)(buff_send + tot)=updater_msgSize;
//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
tot += sizeof(uint16_t);
/*Append updater msgs*/
memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
tot += updater_msgSize;
/*Destroy the updater out msg queue*/
destroy_out_msg_queue();
uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t)));
uint64_t* payload_64 = new uint64_t[total_size];
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
delete[] buff_send;
/*Send a constant number to differenciate updater msgs*/
update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT);
for(int i=0;i<total_size;i++){
update_packets.payload64.push_back(payload_64[i]);
}
/*Add Robot id and message number to the published message*/
if (message_number < 0) message_number=0;
else message_number++;
update_packets.sysid=(uint8_t)robot_id;
update_packets.msgid=(uint32_t)message_number;
payload_pub.publish(update_packets);
multi_msg=false;
delete[] payload_64;
}
/*Request xbee to stop any multi transmission updated not needed any more*/
//if(get_update_status()){
@ -639,7 +642,7 @@ namespace rosbzz_node{
}
else{
else if(msg->payload64.size() > 3){
uint64_t message_obt[msg->payload64.size()];
/* Go throught the obtained payload*/
for(int i=0;i < (int)msg->payload64.size();i++){
@ -663,9 +666,10 @@ namespace rosbzz_node{
neighbours_pos_payload[i]=cartesian_neighbours_pos[i]-cartesian_cur_pos[i];
}
double *cvt_neighbours_pos_payload = cvt_neighbours_pos_test;
// cvt_spherical_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload);
//double cvt_neighbours_pos_payload[3];
//cvt_spherical_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload);
/*Extract robot id of the neighbour*/
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));
cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl;
// cout << "Rel Test Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_test[0] << ", "<< cvt_neighbours_pos_test[2] << ", "<< cvt_neighbours_pos_test[3] << endl;
/*pass neighbour position to local maintaner*/
@ -726,7 +730,11 @@ namespace rosbzz_node{
}
return true;
}
void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){
robot_id=(int)msg->data;
}
}

36
src/test1.bzz
View File

@ -12,11 +12,11 @@ function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 10.0
TARGET_ALTITUDE = 2.0
# Lennard-Jones parameters
TARGET = 50.0 #0.000001001
EPSILON = 0.5 #0.001
TARGET = 10.0 #0.000001001
EPSILON = 10.0 #0.001
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
@ -41,8 +41,8 @@ function hexagon() {
if(neighbors.count() > 0)
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)
print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
uav_moveto(accum.x, accum.y)
}
########################################
@ -101,28 +101,28 @@ neighbors.listen("cmd",
)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0)
statef=hexagon
}
function takeoff() {
#if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
# barrier_set(ROBOTS,hexagon)
# barrier_ready()
#}
if( flight.status !=3){
log("TakeOff: ", flight.status)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
}
else if( flight.status !=3){
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
#log("Land: ", flight.status)
#if(flight.status != 0 and flight.status != 4){
# neighbors.broadcast("cmd", 21)
# uav_land()
#}
uav_land()
log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else
statef=idle
}
# Executed once at init time.

46
src/testalone.bzz Normal file
View File

@ -0,0 +1,46 @@
# 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"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 2.0
# Executed once at init time.
function init() {
}
# Executed at each time step.
function step() {
if(flight.rc_cmd==22) {
flight.rc_cmd=0
uav_takeoff(TARGET_ALTITUDE)
} else if(flight.rc_cmd==21) {
flight.rc_cmd=0
uav_land()
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
uav_goto()
}
# test moveto cmd
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0)
uav_moveto(0.5, 0.5)
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}