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

Added capture image service

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

17
include/roscontroller.h
View File

@ -55,6 +55,8 @@ public:
//void RosControllerInit(); //void RosControllerInit();
void RosControllerRun(); void RosControllerRun();
static const string CAPTURE_SRV_DEFAULT_NAME;
private: private:
struct num_robot_count struct num_robot_count
{ {
@ -92,7 +94,19 @@ private:
/*tmp to be corrected*/ /*tmp to be corrected*/
uint8_t no_cnt=0; uint8_t no_cnt=0;
uint8_t old_val=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 stream_client_name;
std::string relative_altitude_sub_name; std::string relative_altitude_sub_name;
std::string setpoint_nonraw; std::string setpoint_nonraw;
@ -102,6 +116,7 @@ private:
Num_robot_count count_robots; Num_robot_count count_robots;
ros::ServiceClient mav_client; ros::ServiceClient mav_client;
ros::ServiceClient xbeestatus_srv; ros::ServiceClient xbeestatus_srv;
ros::ServiceClient capture_srv;
ros::Publisher payload_pub; ros::Publisher payload_pub;
ros::Publisher MPpayload_pub; ros::Publisher MPpayload_pub;
ros::Publisher neigh_pos_pub; ros::Publisher neigh_pos_pub;

13
src/roscontroller.cpp
View File

@ -2,6 +2,8 @@
#include <thread> #include <thread>
namespace rosbzz_node { namespace rosbzz_node {
const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
/*--------------- /*---------------
/Constructor /Constructor
---------------*/ ---------------*/
@ -366,6 +368,11 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
} else } else
n_c.getParam("xbee_status_srv", xbeesrv_name); 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); GetSubscriptionParameters(n_c);
// initialize topics to null? // 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); service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
ROS_INFO("Ready to receive Mav Commands from RC client"); ROS_INFO("Ready to receive Mav Commands from RC client");
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name); 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); stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this); 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*/ } else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); 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*/ } 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);
} }
} }
/*---------------------------------------------- /*----------------------------------------------