From 1814c0976bc1a414748640ccb1ee729acc8c9de8 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Tue, 5 Sep 2017 11:52:24 -0400 Subject: [PATCH 1/3] Logger change to adapt m_estate var removal in graphform.bzz --- src/roscontroller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 2711a73..924a249 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -267,12 +267,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(); - 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); - std::stringstream state_buff; - state_buff<< graph_state->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); From 32b0e44e9fff6b19adb9f7b7f1848e723508c5f8 Mon Sep 17 00:00:00 2001 From: pyhs Date: Tue, 5 Sep 2017 15:39:03 -0400 Subject: [PATCH 2/3] Added capture image service --- include/roscontroller.h | 19 +++++++++++++++++-- src/roscontroller.cpp | 25 +++++++++++++++++-------- 2 files changed, 34 insertions(+), 10 deletions(-) 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 2711a73..766b41b 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 ---------------*/ @@ -200,7 +202,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<(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); @@ -751,8 +759,9 @@ void roscontroller::flight_controller_service_call() } else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/ 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_INFO("TAKING A PICTURE HERE!! --------------"); + mavros_msgs::CommandBool capture_command; + capture_srv.call(capture_command); } } /*---------------------------------------------- From 0dfe6b1497a689696a12775ea9b4501325878715 Mon Sep 17 00:00:00 2001 From: pyhs Date: Tue, 5 Sep 2017 16:10:16 -0400 Subject: [PATCH 3/3] Fixing capture_image_srv name --- src/roscontroller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index bbbafdd..7927814 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -273,7 +273,7 @@ void roscontroller::RosControllerRun() //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); + // buzzvm_pop(VM); // state_buff<< graph_state->s.value.str<<","; buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1)); buzzvm_gload(VM); @@ -368,7 +368,7 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) } else n_c.getParam("xbee_status_srv", xbeesrv_name); - if(!n_c.getParam("xbee_status_srv", capture_srv_name)) + if(!n_c.getParam("capture_image_srv", capture_srv_name)) { capture_srv_name = CAPTURE_SRV_DEFAULT_NAME; }