new m100 launch

This commit is contained in:
David St-Onge 2016-12-05 17:59:27 -05:00
commit 1a910ff80e
7 changed files with 76 additions and 25 deletions

View File

@ -9,7 +9,7 @@ endif()
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
mavros_msgs
#mavros_msgs
sensor_msgs
)

View File

@ -22,8 +22,10 @@ int buzzros_print(buzzvm_t vm);
int buzzuav_goto(buzzvm_t vm);
/* Returns the current command from local variable*/
int getcmd();
/*Sets goto position could be used for bypassing*/
/*Sets goto position */
void set_goto(double pos[]);
/*Sets goto position from rc client*/
void rc_set_goto(double pos[]);
/*sets rc requested command */
void rc_call(int rc_cmd);
/* sets the battery state */
@ -57,7 +59,9 @@ int buzzuav_update_battery(buzzvm_t vm);
*/
int buzzuav_update_currentpos(buzzvm_t vm);
/*
* Updates flight status in Buzz
* Updates flight status and rc command in Buzz, put it in a tabel to acess it
* use flight.status for flight status
* use flight.rc_cmd for current rc cmd
*/
int buzzuav_update_flight_status(buzzvm_t vm);

View File

@ -42,6 +42,7 @@ private:
int robot_id=0;
int oldcmdID=0;
int rc_cmd;
int bzz_old_cmd=0;
std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname,out_payload,in_payload; //, rcclient;
bool rcclient;
ros::ServiceClient mav_client;

View File

@ -2,13 +2,21 @@
<launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<<<<<<< HEAD
<param name="bzzfile_name" value="../ROS_WS/src/ROSBuzz/src/test1.bzz" />
=======
<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz" />
>>>>>>> 9e9eb964b10f7fa49342b6eb629fc7a30e47c355
<param name="rcclient" value="true" />
<param name="rcservice_name" value="rc_cmd" />
<param name="fcclient_name" value="/djicmd" />
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<<<<<<< HEAD
<param name="robot_id" value="1"/>
=======
<param name="robot_id" value="2"/>
>>>>>>> 9e9eb964b10f7fa49342b6eb629fc7a30e47c355
</node>

View File

@ -9,10 +9,11 @@
#include "buzzuav_closures.h"
namespace buzzuav_closures{
static double goto_pos[3];
static double rc_goto_pos[3];
static float batt[3];
static double cur_pos[3];
static uint8_t status;
static int cur_cmd;
static int cur_cmd = 0;
static int rc_cmd=0;
/****************************************/
/****************************************/
@ -83,12 +84,7 @@ return goto_pos;
}
/******************************/
int getcmd(){
if(rc_cmd==0) return cur_cmd;
else {
cur_cmd=rc_cmd;
rc_cmd=0;
return cur_cmd;
}
return cur_cmd;
}
void set_goto(double pos[]){
@ -98,6 +94,12 @@ goto_pos[2]=pos[2];
}
void rc_set_goto(double pos[]){
rc_goto_pos[0]=pos[0];
rc_goto_pos[1]=pos[1];
rc_goto_pos[2]=pos[2];
}
void rc_call(int rc_cmd_in){
rc_cmd=rc_cmd_in;
}
@ -188,14 +190,35 @@ int buzzuav_update_flight_status(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_cmd", 1));
buzzvm_pushi(vm, rc_cmd);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1));
buzzvm_pushf(vm, status);
buzzvm_pushi(vm, status);
buzzvm_tput(vm);
buzzvm_gstore(vm);
//also set rc_controllers goto
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
buzzvm_pushf(vm, rc_goto_pos[0]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
buzzvm_pushf(vm, rc_goto_pos[1]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
buzzvm_pushf(vm, rc_goto_pos[2]);
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
}
/****************************************/
/*To do !!!!!*/
/****************************************/

View File

@ -92,6 +92,7 @@ namespace rosbzz_node{
flight_status_sub =n_c.subscribe("/mav/flight_status",100, &roscontroller::flight_status_update,this);
/*publishers*/
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
cout<<out_payload<<endl;
/* Clients*/
mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
@ -122,15 +123,19 @@ namespace rosbzz_node{
}
void roscontroller::prepare_msg_and_publish(){
/*prepare the goto publish message */
double* goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.param1 = goto_pos[0];
cmd_srv.request.param2 = goto_pos[1];
cmd_srv.request.param3 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
/* flight controller client call*/
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");
if (bzz_old_cmd != buzzuav_closures::getcmd()) {
/*prepare the goto publish message */
double* goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.param1 = goto_pos[0];
cmd_srv.request.param2 = goto_pos[1];
cmd_srv.request.param3 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
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");
bzz_old_cmd = cmd_srv.request.command;
}
/*obtain Pay load to be sent*/
uint64_t* payload_out_ptr= buzz_utility::out_msg_process();
uint64_t position[3];
@ -276,12 +281,12 @@ namespace rosbzz_node{
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! x = %f , y = %f , Z = %f",req.param1,req.param2,req.param3);
ROS_INFO("RC_Call: GO TO!!!! ");
double rc_goto[3];
rc_goto[0]=req.param1;
rc_goto[1]=req.param2;
rc_goto[2]=req.param3;
buzzuav_closures::set_goto(rc_goto);
rc_goto[0]=req.x;
rc_goto[1]=req.y;
rc_goto[2]=req.z;
buzzuav_closures::rc_set_goto(rc_goto);
rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;

View File

@ -1,6 +1,8 @@
# Executed once at init time.
function init() {
i = 1
a = 0
val = 0
}
# Executed at each time step.
@ -15,11 +17,19 @@ neighbors.listen("Take",
neighbors.listen("key",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
val = value
}
)
print(val)
if ((val == 23) and (a == 0)) {
uav_takeoff()
a=1
}
if (a == 10) uav_land()
if (a != 0) a = a+1
}
else{
neighbors.broadcast("key", "yes")
neighbors.broadcast("key", 23)
neighbors.broadcast("Take", "no")
}