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

Added capture image service

Browse Source
This commit is contained in:
pyhs 2017-09-05 15:39:03 -04:00
parent ccdb80a4cf
commit 32b0e44e9f
2 changed files with 34 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
include/roscontroller.h
View File

@ -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;

25
src/roscontroller.cpp
View File

@ -2,6 +2,8 @@
#include <thread>
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<<ros::Time::now()<<",";
@ -215,17 +217,17 @@ void roscontroller::RosControllerRun()
log << (double)it->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<<result<<",";
else
log<<"0,";
if ( GetRawPacketLoss(shrt_id, result) )
if ( GetRawPacketLoss(shrt_id, result) )
log<<result<<",";
else
log<<"0,";
if ( GetFilteredPacketLoss(shrt_id, result) )
if ( GetFilteredPacketLoss(shrt_id, result) )
log<<result<<",";
else
log<<"0,";
@ -264,7 +266,7 @@ void roscontroller::RosControllerRun()
// no_of_robots=get_number_of_robots();
get_number_of_robots();
buzz_utility::set_robot_var(no_of_robots);
/*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF
/*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));
@ -366,6 +368,11 @@ 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))
{
capture_srv_name = CAPTURE_SRV_DEFAULT_NAME;
}
GetSubscriptionParameters(n_c);
// initialize topics to null?
}
@ -474,6 +481,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<mavros_msgs::ParamGet>(xbeesrv_name);
capture_srv = n_c.serviceClient<mavros_msgs::CommandBool>(capture_srv_name);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(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);
}
}
/*----------------------------------------------