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

modif for sim

This commit is contained in:
dave 2017-04-20 16:07:36 -04:00
parent 73c81fb22b
commit 3e1653cdaa
4 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/buzz_utility.h
View File

@ -49,7 +49,7 @@ int buzz_script_done();
int update_step_test(); int update_step_test();
uint16_t get_robotid(); int get_robotid();
buzzvm_t get_vm(); buzzvm_t get_vm();

3
src/buzz_utility.cpp
View File

@ -51,6 +51,9 @@ namespace buzz_utility{
return out; return out;
} }
int get_robotid() {
return Robot_id;
}
/***************************************************/ /***************************************************/
/*Appends obtained messages to buzz in message Queue*/ /*Appends obtained messages to buzz in message Queue*/
/***************************************************/ /***************************************************/

1
src/buzzuav_closures.cpp
View File

@ -30,6 +30,7 @@ namespace buzzuav_closures{
int buzzros_print(buzzvm_t vm) { int buzzros_print(buzzvm_t vm) {
int i; int i;
char buffer [50] = ""; char buffer [50] = "";
sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid());
for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) { for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
buzzvm_lload(vm, i); buzzvm_lload(vm, i);
buzzobj_t o = buzzvm_stack_at(vm, 1); buzzobj_t o = buzzvm_stack_at(vm, 1);

10
src/roscontroller.cpp
View File

@ -29,6 +29,7 @@ namespace rosbzz_node{
robot_id=strtol(robot_name.c_str() + 5, NULL, 10);; robot_id=strtol(robot_name.c_str() + 5, NULL, 10);;
setpoint_counter = 0; setpoint_counter = 0;
fcu_timeout = TIMEOUT; fcu_timeout = TIMEOUT;
home[0]=0.0;home[1]=0.0;home[2]=0.0;
} }
/*--------------------- /*---------------------
@ -208,12 +209,12 @@ namespace rosbzz_node{
//current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this); //current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this);
//battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this); //battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this);
payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this); payload_sub = n_c.subscribe("/"+robot_name+in_payload, 1000, &roscontroller::payload_obt,this);
//flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this); //flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this);
//Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this); //Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this);
obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this); obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
/*publishers*/ /*publishers*/
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000); payload_pub = n_c.advertise<mavros_msgs::Mavlink>("/"+robot_name+out_payload, 1000);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000); neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
localsetpoint_pub = n_c.advertise<geometry_msgs::PoseStamped>("/"+robot_name+setpoint_name,1000); localsetpoint_pub = n_c.advertise<geometry_msgs::PoseStamped>("/"+robot_name+setpoint_name,1000);
/* Service Clients*/ /* Service Clients*/
@ -797,7 +798,7 @@ namespace rosbzz_node{
geometry_msgs::PoseStamped moveMsg; geometry_msgs::PoseStamped moveMsg;
moveMsg.header.stamp = ros::Time::now(); moveMsg.header.stamp = ros::Time::now();
moveMsg.header.seq = setpoint_counter++; moveMsg.header.seq = setpoint_counter++;
moveMsg.header.frame_id = 1; moveMsg.header.frame_id = "/world";
double local_pos[3]; double local_pos[3];
cvt_ned_coordinates(cur_pos,local_pos,home); cvt_ned_coordinates(cur_pos,local_pos,home);
moveMsg.pose.position.x = local_pos[0]+x; moveMsg.pose.position.x = local_pos[0]+x;
@ -811,6 +812,9 @@ namespace rosbzz_node{
localsetpoint_pub.publish(moveMsg); localsetpoint_pub.publish(moveMsg);
ROS_INFO("Home: %f %f %f", home[0],home[1],home[2]);
ROS_INFO("Current: %f %f %f", cur_pos[0],cur_pos[1],cur_pos[2]);
ROS_INFO("Offset NED: %f %f", local_pos[0], local_pos[1]);
ROS_INFO("Sent local NON RAW position message!"); ROS_INFO("Sent local NON RAW position message!");
} }