diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz index 3ba5262..ddcf448 100644 --- a/buzz_scripts/graphformGPS.bzz +++ b/buzz_scripts/graphformGPS.bzz @@ -543,7 +543,6 @@ function DoJoining(){ set_rc_goto() else gotoWP(TransitionToJoined) - #pack the communication package m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel diff --git a/buzz_scripts/include/barrier.bzz b/buzz_scripts/include/barrier.bzz index 87b83d3..88fe411 100644 --- a/buzz_scripts/include/barrier.bzz +++ b/buzz_scripts/include/barrier.bzz @@ -30,7 +30,7 @@ function barrier_create() { function barrier_set(threshold, transf, resumef, bdt) { statef = function() { - barrier_wait(threshold, transf, resumef, bdt); + barrier_wait(threshold, transf, resumef); } UAVSTATE = "BARRIERWAIT" barrier_create() @@ -40,6 +40,7 @@ function barrier_set(threshold, transf, resumef, bdt) { # Make yourself ready # function barrier_ready() { + log("BARRIER READY -------") barrier.put(id, 1) barrier.put("d", 0) } diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index cdf9e3b..b2d4d03 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -27,9 +27,15 @@ function idle() { UAVSTATE = "IDLE" } +firstpassT = 1 function takeoff() { UAVSTATE = "TAKEOFF" statef=takeoff + + if(firstpassT) { + barrier_create(TAKEOFF_VSTIG) + firstpassT = 0 + } if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { barrier_set(ROBOTS, action, land, -1) @@ -41,6 +47,7 @@ function takeoff() { } } +firstpassL = 1 function land() { UAVSTATE = "LAND" statef=land diff --git a/include/roscontroller.h b/include/roscontroller.h index bc5e117..cebee9d 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -55,6 +55,8 @@ public: //void RosControllerInit(); void RosControllerRun(); + static const string CAPTURE_SRV_DEFAULT_NAME; + private: struct num_robot_count { @@ -70,7 +72,7 @@ private: double latitude=0.0; float altitude=0.0; }; typedef struct gps GPS ; // not useful in cpp - + GPS target, home, cur_pos; double cur_rel_altitude; @@ -92,7 +94,19 @@ private: /*tmp to be corrected*/ uint8_t no_cnt=0; uint8_t old_val=0 ; - std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,obstacles_topic,stand_by,xbeesrv_name, setpoint_name; + std::string bzzfile_name; + std::string fcclient_name; + std::string armclient; + std::string modeclient; + std::string rcservice_name; + std::string bcfname,dbgfname; + std::string out_payload; + std::string in_payload; + std::string obstacles_topic; + std::string stand_by; + std::string xbeesrv_name; + std::string capture_srv_name; + std::string setpoint_name; std::string stream_client_name; std::string relative_altitude_sub_name; std::string setpoint_nonraw; @@ -102,6 +116,7 @@ private: Num_robot_count count_robots; ros::ServiceClient mav_client; ros::ServiceClient xbeestatus_srv; + ros::ServiceClient capture_srv; ros::Publisher payload_pub; ros::Publisher MPpayload_pub; ros::Publisher neigh_pos_pub; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 6354584..f4e0162 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -2,6 +2,8 @@ #include namespace rosbzz_node { +const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; + /*--------------- /Constructor ---------------*/ @@ -201,7 +203,7 @@ void roscontroller::RosControllerRun() while (ros::ok() && !buzz_utility::buzz_script_done()) { /*Update neighbors position inside Buzz*/ // buzz_closure::neighbour_pos_callback(neighbours_pos_map); - + /*log ROS Time stamp, pos (lat,long,alt), Swarm size, no. of neigh, neigh pos, RSSI val, Packet loss, filtered packet loss*/ log<second.x << "," << (double)it->second.y << "," << (double)it->second.z << ","; } - const uint8_t shrt_id= 0xFF; + const uint8_t shrt_id= 0xFF; float result; - if ( GetAPIRssi(shrt_id, result) ) + if ( GetAPIRssi(shrt_id, result) ) log<s.value.str<<","; + std::stringstream state_buff; + //buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1)); + // buzzvm_gload(VM); + // buzzobj_t graph_state = buzzvm_stack_at(VM, 1); + // buzzvm_pop(VM); + // state_buff<< graph_state->s.value.str<<","; buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1)); buzzvm_gload(VM); buzzobj_t uav_state = buzzvm_stack_at(VM, 1); @@ -367,6 +369,11 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) } else n_c.getParam("xbee_status_srv", xbeesrv_name); + if(!n_c.getParam("capture_image_srv", capture_srv_name)) + { + capture_srv_name = CAPTURE_SRV_DEFAULT_NAME; + } + GetSubscriptionParameters(n_c); // initialize topics to null? } @@ -475,6 +482,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c) service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this); ROS_INFO("Ready to receive Mav Commands from RC client"); xbeestatus_srv = n_c.serviceClient(xbeesrv_name); + capture_srv = n_c.serviceClient(capture_srv_name); stream_client = n_c.serviceClient(stream_client_name); users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this); @@ -753,7 +761,8 @@ void roscontroller::flight_controller_service_call() roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); } else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/ ROS_INFO("TAKING A PICTURE HERE!! --------------"); - ros::Duration(0.2).sleep(); + mavros_msgs::CommandBool capture_command; + capture_srv.call(capture_command); } } /*----------------------------------------------