From dc014841054a5b71695527f5fde5f26ec027a412 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Wed, 6 Sep 2017 21:53:00 -0400 Subject: [PATCH 1/3] add camera service check and fix ROSBuzz gimbal control --- buzz_scripts/include/uavstates.bzz | 28 ++++++++++++++-------------- buzz_scripts/testaloneWP.bzz | 2 +- src/buzzuav_closures.cpp | 2 +- src/roscontroller.cpp | 16 +++++++++------- 4 files changed, 25 insertions(+), 23 deletions(-) diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 2ae665a..254d84d 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -35,7 +35,7 @@ function takeoff() { barrier_set(ROBOTS, action, land, -1) barrier_ready() } else { - log("Altitude: ", TARGET_ALTITUDE) + log("Altitude: ", position.altitude) neighbors.broadcast("cmd", 22) uav_takeoff(TARGET_ALTITUDE) } @@ -60,15 +60,15 @@ function set_goto(transf) { gotoWP(transf); } - #if(rc_goto.id==id){ - # if(s!=nil){ - # if(s.in()) - # s.leave() - #} - #} else { - # neighbors.broadcast("cmd", 16) - # neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) - #} + if(rc_goto.id==id){ + if(s!=nil){ + if(s.in()) + s.leave() + } + } else { + neighbors.broadcast("cmd", 16) + neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) + } } ptime=0 @@ -86,14 +86,14 @@ function picture() { } function gotoWP(transf) { - print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) + # print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) rb_from_gps(rc_goto.latitude, rc_goto.longitude) - print(" has to move ", goal.range) + print(" has to move ", goal.range, goal.bearing) m_navigation = math.vec2.newp(goal.range, goal.bearing) if(math.vec2.length(m_navigation)>GOTO_MAXDIST) - log("Sorry this is too far.") - else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity + log("Sorry this is too far.") + else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) } else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination diff --git a/buzz_scripts/testaloneWP.bzz b/buzz_scripts/testaloneWP.bzz index 892b5a6..97b92bf 100644 --- a/buzz_scripts/testaloneWP.bzz +++ b/buzz_scripts/testaloneWP.bzz @@ -23,7 +23,7 @@ function step() { statef() -# log("Current state: ", UAVSTATE) + log("Current state: ", UAVSTATE) } # Executed once when the robot (or the simulator) is reset. diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index ed9ef9b..23676d2 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -321,7 +321,7 @@ namespace buzzuav_closures{ / Buzz closure to take a picture here. /----------------------------------------*/ int buzzuav_takepicture(buzzvm_t vm) { - cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST; + //cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST; buzz_cmd = COMMAND_PICTURE; return buzzvm_ret0(vm); } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 3383569..1041ec7 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -761,17 +761,20 @@ 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!! --------------"); - cmd_srv.request.param1 = 90; - cmd_srv.request.param2 = 0; - cmd_srv.request.param3 = 0; - cmd_srv.request.param4 = 0; + cmd_srv.request.param1 = 0.0; + cmd_srv.request.param2 = 0.0; + cmd_srv.request.param3 = -90.0; + cmd_srv.request.param4 = 0.0; cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; if (mav_client.call(cmd_srv)) { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); } else ROS_ERROR("Failed to call service from flight controller"); mavros_msgs::CommandBool capture_command; - capture_srv.call(capture_command); + if (capture_srv.call(capture_command)) { + ROS_INFO("Reply: %ld", (long int)capture_command.response.success); + } else + ROS_ERROR("Failed to call service from camera streamer"); } } /*---------------------------------------------- @@ -993,8 +996,7 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) { // gps_ned_home(ned_x, ned_y); // ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], // home[1]); - // ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, - // local_pos[0], local_pos[1]); +// ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos_new[0], local_pos_new[1]); /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD * (then converted to NED)*/ From 7830235c78ead1d6e79df67ccaf6c2708935ca48 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 7 Sep 2017 18:17:03 -0400 Subject: [PATCH 2/3] 8 Hz improved loop rate --- src/roscontroller.cpp | 74 ++++++++++++++++++++++--------------------- 1 file changed, 38 insertions(+), 36 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 1041ec7..6ecd042 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -195,7 +195,9 @@ void roscontroller::RosControllerRun() robot_id)) { ROS_INFO("[%i] Bytecode file found and set", robot_id); std::string standby_bo = Compile_bzz(stand_by) + ".bo"; - init_update_monitor(bcfname.c_str(), standby_bo.c_str()); + //init_update_monitor(bcfname.c_str(), standby_bo.c_str()); + /*loop rate of ros*/ + ros::Rate loop_rate(15); /////////////////////////////////////////////////////// // MAIN LOOP ////////////////////////////////////////////////////// @@ -220,15 +222,15 @@ void roscontroller::RosControllerRun() } const uint8_t shrt_id= 0xFF; float result; - if ( GetAPIRssi(shrt_id, result) ) - log<::iterator it = neighbours_pos_map.begin(); log << "," << neighbours_pos_map.size(); @@ -262,7 +264,7 @@ void roscontroller::RosControllerRun() << "," << (double)it->second.z; } log << std::endl;*/ - } + //} /*Set ROBOTS variable for barrier in .bzz from neighbours count*/ // no_of_robots=get_number_of_robots(); get_number_of_robots(); @@ -270,12 +272,12 @@ void roscontroller::RosControllerRun() /*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF SCRIPT IS NOT graphform.bzz*/ static buzzvm_t VM = buzz_utility::get_vm(); - 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<<","; + std::stringstream state_buff; + // 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); @@ -287,7 +289,7 @@ void roscontroller::RosControllerRun() // buzz_utility::set_robot_var(no_of_robots); /*Set no of robots for updates TODO only when not updating*/ // if(multi_msg) - updates_set_robots(no_of_robots); + //updates_set_robots(no_of_robots); // ROS_INFO("ROBOTS: %i , acutal : // %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1); get_xbee_status(); @@ -633,7 +635,7 @@ void roscontroller::prepare_msg_and_publish() delete[] out; delete[] payload_out_ptr; /*Check for updater message if present send*/ - if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 && + /* if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 && multi_msg) { uint8_t *buff_send = 0; @@ -675,7 +677,7 @@ void roscontroller::prepare_msg_and_publish() payload_pub.publish(update_packets); multi_msg = false; delete[] payload_64; - } + }*/ } /*--------------------------------------------------------------------------------- /Flight controller service call every step if there is a command set from bzz @@ -1260,22 +1262,22 @@ void roscontroller::get_xbee_status() bool result_bool; float result_float; const uint8_t all_ids = 0xFF; - if(GetDequeFull(result_bool)) - { - buzzuav_closures::set_deque_full(result_bool); - } - if(GetRssi(result_float)) - { - buzzuav_closures::set_rssi(result_float); - } - if(GetRawPacketLoss(all_ids, result_float)) - { - buzzuav_closures::set_raw_packet_loss(result_float); - } - if(GetFilteredPacketLoss(all_ids, result_float)) - { - buzzuav_closures::set_filtered_packet_loss(result_float); - } + //if(GetDequeFull(result_bool)) + //{ + // buzzuav_closures::set_deque_full(result_bool); + //} + //if(GetRssi(result_float)) + //{ + // buzzuav_closures::set_rssi(result_float); + //} + //if(GetRawPacketLoss(all_ids, result_float)) + //{ + // buzzuav_closures::set_raw_packet_loss(result_float); + //} + //if(GetFilteredPacketLoss(all_ids, result_float)) + //{ + // buzzuav_closures::set_filtered_packet_loss(result_float); + //} // This part needs testing since it can overload the xbee module /* * if(GetAPIRssi(all_ids, result_float)) From b41a086ede24c213a8264588f36a4403a87b3efa Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 7 Sep 2017 18:42:26 -0400 Subject: [PATCH 3/3] Looprate setting bug fix. --- src/roscontroller.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 6ecd042..342b0ed 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -197,7 +197,7 @@ void roscontroller::RosControllerRun() std::string standby_bo = Compile_bzz(stand_by) + ".bo"; //init_update_monitor(bcfname.c_str(), standby_bo.c_str()); /*loop rate of ros*/ - ros::Rate loop_rate(15); + ros::Rate loop_rate(BUZZRATE); /////////////////////////////////////////////////////// // MAIN LOOP ////////////////////////////////////////////////////// @@ -295,8 +295,6 @@ void roscontroller::RosControllerRun() get_xbee_status(); /*run once*/ ros::spinOnce(); - /*loop rate of ros*/ - ros::Rate loop_rate(BUZZRATE); loop_rate.sleep(); if (fcu_timeout <= 0) buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND);