diff --git a/include/roscontroller.h b/include/roscontroller.h index 2892094..1a1c30a 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -1,6 +1,7 @@ #pragma once #include #include +#include #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); diff --git a/launch/rosbuzz_2_parallel.launch b/launch/rosbuzz_2_parallel.launch index 44841ea..de18b18 100644 --- a/launch/rosbuzz_2_parallel.launch +++ b/launch/rosbuzz_2_parallel.launch @@ -3,6 +3,7 @@ + diff --git a/log.txt b/log.txt new file mode 100644 index 0000000..e038009 --- /dev/null +++ b/log.txt @@ -0,0 +1,3 @@ + +2017-01-08-14-24-24 enter TranslateFunc +2017-01-08-14-24-24 start to read frames! \ No newline at end of file diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 374362b..82d785c 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -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; } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index edb71c8..741d24b 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -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); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 334359c..3c89b30 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -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. } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 9298ffa..1b19a30 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -68,36 +68,26 @@ namespace rosbzz_node{ //std::cout<<"long obt"<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(out_payload, 1000); neigh_pos_pub = n_c.advertise("/neighbours_pos",1000); /* Service Clients*/ - arm_client = n_c.serviceClient(armingclient); + arm_client = n_c.serviceClient(armclient); mode_client = n_c.serviceClient(modeclient); mav_client = n_c.serviceClient(fcclient_name); @@ -318,6 +309,13 @@ namespace rosbzz_node{ cout<<" [Debug:] sent message "<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; + + } + } diff --git a/src/test1.bzz b/src/test1.bzz index c3dc4c1..3e9d1fb 100644 --- a/src/test1.bzz +++ b/src/test1.bzz @@ -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. diff --git a/src/testalone.bzz b/src/testalone.bzz new file mode 100644 index 0000000..e99b3cf --- /dev/null +++ b/src/testalone.bzz @@ -0,0 +1,46 @@ + +# 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" +#################################################################################################### +# 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() { +}