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

extend xbeestatus

This commit is contained in:
David St-Onge 2017-04-02 14:19:45 -04:00
parent 05f51194cc
commit 29b234cd9d
3 changed files with 17 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/roscontroller.h
View File

@ -59,11 +59,11 @@ private:
int armstate;
int barrier;
int message_number=0;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name;
bool rcclient;
bool multi_msg;
ros::ServiceClient mav_client;
ros::ServiceClient robot_id_client;
ros::ServiceClient xbeestatus_srv;
ros::Publisher payload_pub;
ros::Publisher neigh_pos_pub;
ros::Publisher localsetpoint_pub;
@ -77,8 +77,6 @@ private:
/*Commands for flight controller*/
//mavros_msgs::CommandInt cmd_srv;
mavros_msgs::CommandLong cmd_srv;
mavros_msgs::ParamGet::Request robot_id_srv_request;
mavros_msgs::ParamGet::Response robot_id_srv_response;
std::vector<std::string> m_sMySubscriptions;
std::map<std::string, std::string> m_smTopic_infos;

18
src/buzzuav_closures.cpp
View File

@ -29,40 +29,42 @@ namespace buzzuav_closures{
int buzzros_print(buzzvm_t vm) {
int i;
char buffer [50] = "";
for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
buzzvm_lload(vm, i);
buzzobj_t o = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
switch(o->o.type) {
case BUZZTYPE_NIL:
ROS_INFO("BUZZ - [nil]");
sprintf(buffer,"%s BUZZ - [nil]", buffer);
break;
case BUZZTYPE_INT:
ROS_INFO("%d", o->i.value);
sprintf(buffer,"%s %d", buffer, o->i.value);
//fprintf(stdout, "%d", o->i.value);
break;
case BUZZTYPE_FLOAT:
ROS_INFO("%f", o->f.value);
sprintf(buffer,"%s %f", buffer, o->f.value);
break;
case BUZZTYPE_TABLE:
ROS_INFO("[table with %d elems]", (buzzdict_size(o->t.value)));
sprintf(buffer,"%s [table with %d elems]", buffer, (buzzdict_size(o->t.value)));
break;
case BUZZTYPE_CLOSURE:
if(o->c.value.isnative)
ROS_INFO("[n-closure @%d]", o->c.value.ref);
sprintf(buffer,"%s [n-closure @%d]", buffer, o->c.value.ref);
else
ROS_INFO("[c-closure @%d]", o->c.value.ref);
sprintf(buffer,"%s [c-closure @%d]", buffer, o->c.value.ref);
break;
case BUZZTYPE_STRING:
ROS_INFO("%s", o->s.value.str);
sprintf(buffer,"%s %s", buffer, o->s.value.str);
break;
case BUZZTYPE_USERDATA:
ROS_INFO("[userdata @%p]", o->u.value);
sprintf(buffer,"%s [userdata @%p]", buffer, o->u.value);
break;
default:
break;
}
}
ROS_INFO(buffer);
//fprintf(stdout, "\n");
return buzzvm_ret0(vm);
}

8
src/roscontroller.cpp
View File

@ -36,8 +36,9 @@ namespace rosbzz_node{
/rosbuzz_node loop method executed once every step
/--------------------------------------------------*/
void roscontroller::RosControllerRun(){
while(!robot_id_client.call(robot_id_srv_request,robot_id_srv_response)){
mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
mavros_msgs::ParamGet::Response robot_id_srv_response;
while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
/*run once*/
ros::spinOnce();
/*loop rate of ros*/
@ -118,6 +119,7 @@ namespace rosbzz_node{
n_c.getParam("No_of_Robots", barrier);
/*Obtain standby script to run during update*/
n_c.getParam("stand_by", stand_by);
n_c.getParam("xbee_status_srv", xbeesrv_name);
GetSubscriptionParameters(n_c);
// initialize topics to null?
@ -176,7 +178,7 @@ namespace rosbzz_node{
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
robot_id_client = n_c.serviceClient<mavros_msgs::ParamGet>("/Robot_ID_srv");
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
multi_msg=true;
}
/*---------------------------------------