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

merge with master

This commit is contained in:
isvogor 2017-02-24 14:58:30 -05:00
parent f5899ecac0 6593215a57
commit 100d0c0599
6 changed files with 44 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/buzzuav_closures.h
View File

@ -48,6 +48,7 @@ void set_obstacle_dist(float dist[]);
int buzzuav_takeoff(buzzvm_t vm);
int buzzuav_arm(buzzvm_t vm);
int buzzuav_disarm(buzzvm_t vm) ;
/* Commands the UAV to land
*/
int buzzuav_land(buzzvm_t vm);
@ -83,6 +84,8 @@ int buzzuav_update_prox(buzzvm_t vm);
int bzz_cmd();
int dummy_closure(buzzvm_t vm);
//#endif
}

2
launch/rosbuzz_2_parallel.launch
View File

@ -2,7 +2,7 @@
<launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/testflockfev_barrier.bzz" />
<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/testflockfev.bzz" />
<rosparam file="/home/vivek/catkin_ws/src/rosbuzz/launch/launch_config/m100.yaml"/>
<param name="rcclient" value="true" />
<param name="rcservice_name" value="rc_cmd" />

6
src/buzz_utility.cpp
View File

@ -242,6 +242,9 @@ namespace buzz_utility{
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_disarm));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff));
buzzvm_gstore(VM);
@ -274,6 +277,9 @@ namespace buzz_utility{
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);

9
src/buzzuav_closures.cpp
View File

@ -173,10 +173,16 @@ int buzzuav_goto(buzzvm_t vm) {
int buzzuav_arm(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
printf(" Buzz requested Arm/Disarm \n");
printf(" Buzz requested Arm \n");
buzz_cmd=3;
return buzzvm_ret0(vm);
}
int buzzuav_disarm(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
printf(" Buzz requested Disarm \n");
buzz_cmd=4;
return buzzvm_ret0(vm);
}
/******************************/
double* getgoto() {
@ -414,4 +420,3 @@ int buzzuav_update_prox(buzzvm_t vm) {
int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
}

15
src/roscontroller.cpp
View File

@ -290,6 +290,10 @@ namespace rosbzz_node{
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
else ROS_ERROR("Failed to call service from flight controller");
} else if (tmp == 3) { /*FC call for arm*/
armstate=1;
Arm();
} else if (tmp == 4){
armstate=0;
Arm();
}
}
@ -804,12 +808,16 @@ namespace rosbzz_node{
case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM:
rc_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
armstate = req.param1;
if(armstate)
if(armstate){
ROS_INFO("RC_Call: ARM!!!!");
else
ROS_INFO("RC_Call: DISARM!!!!");
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
}
else{
ROS_INFO("RC_Call: DISARM!!!!");
buzzuav_closures::rc_call(rc_cmd+1);
res.success = true;
}
break;
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
ROS_INFO("RC_Call: GO HOME!!!!");
@ -818,6 +826,7 @@ namespace rosbzz_node{
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! ");
double rc_goto[3];
rc_goto[0] = req.param5;
rc_goto[1] = req.param6;

9
src/testflockfev.bzz
View File

@ -1,4 +1,3 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz"
@ -100,6 +99,10 @@ neighbors.listen("cmd",
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400) {
uav_arm()
} else if(value==401){
uav_disarm()
}
}
@ -160,6 +163,10 @@ function step() {
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
uav_disarm()
neighbors.broadcast("cmd", 401)
}
statef()
log("Current state: ", CURSTATE)