Service and client tested Implementation

This commit is contained in:
Vivek Shankar Varadharajan 2016-09-30 12:48:18 -04:00
parent 622fd9ac77
commit 8d6efae326
7 changed files with 137 additions and 54 deletions

View File

@ -65,6 +65,7 @@ int buzzuav_goto(buzzvm_t vm) {
goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value * 10.0f; goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value * 10.0f;
goto_pos[1]=buzzvm_stack_at(vm, 2)->f.value * 10.0f; goto_pos[1]=buzzvm_stack_at(vm, 2)->f.value * 10.0f;
goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value * 10.0f; goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value * 10.0f;
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
@ -78,6 +79,17 @@ int getcmd(){
return cur_cmd; return cur_cmd;
} }
void set_goto(double pos[]){
goto_pos[0]=pos[0];
goto_pos[1]=pos[1];
goto_pos[2]=pos[2];
}
void rc_call(int rc_cmd){
cur_cmd=rc_cmd;
}
/****************************************/ /****************************************/
/****************************************/ /****************************************/

View File

@ -17,6 +17,11 @@ int buzzros_print(buzzvm_t vm);
int buzzuav_goto(buzzvm_t vm); int buzzuav_goto(buzzvm_t vm);
/* Returns the current command from local variable*/ /* Returns the current command from local variable*/
int getcmd(); int getcmd();
void set_goto(double pos[]);
void rc_call(int rc_cmd);
/* sets the battery state to the local variable /* sets the battery state to the local variable
*/ */
void set_battery(float voltage,float current,float remaining); void set_battery(float voltage,float current,float remaining);

View File

@ -1,4 +1,4 @@
!15 !16
'init 'init
'id 'id
'mydist 'mydist
@ -9,6 +9,7 @@
'min 'min
'get 'get
'distance 'distance
'a
'step 'step
'broadcast 'broadcast
'print 'print
@ -18,13 +19,13 @@
pushs 0 pushs 0
pushcn @__label_1 pushcn @__label_1
gstore gstore
pushs 10 pushs 11
pushcn @__label_5 pushcn @__label_5
gstore gstore
pushs 13 pushs 14
pushcn @__label_6 pushcn @__label_6
gstore gstore
pushs 14 pushs 15
pushcn @__label_7 pushcn @__label_7
gstore gstore
nop nop
@ -57,7 +58,10 @@
pushi 2 |16,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz pushi 2 |16,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
callc |16,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz callc |16,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@__label_3 |17,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz @__label_3 |17,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
ret0 |18,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz pushs 10 |18,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushi 1 |18,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gstore |18,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
ret0 |19,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@__label_4 @__label_4
pushs 2 |13,15,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz pushs 2 |13,15,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@ -84,25 +88,25 @@
ret0 |16,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz ret0 |16,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@__label_5 @__label_5
pushs 3 |22,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz pushs 3 |23,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gload |22,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz gload |23,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 11 |22,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz pushs 12 |23,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
tget |22,19,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz tget |23,19,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 5 |22,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz pushs 5 |23,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 2 |22,44,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz pushs 2 |23,44,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gload |22,44,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz gload |23,44,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushi 2 |22,45,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz pushi 2 |23,45,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
callc |22,45,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz callc |23,45,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 12 |23,5,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz pushs 13 |24,5,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gload |23,5,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz gload |24,5,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 2 |23,12,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz pushs 2 |24,12,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gload |23,12,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz gload |24,12,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushi 1 |23,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz pushi 1 |24,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
callc |23,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz callc |24,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
ret0 |26,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz ret0 |35,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@__label_6 @__label_6
ret0 |30,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz ret0 |39,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@__label_7 @__label_7
ret0 |34,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz ret0 |43,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz

Binary file not shown.

Binary file not shown.

View File

@ -1,13 +1,15 @@
/* /** @file rosbuzz.cpp
* @version 1
* Header * @date 27.09.2016
* @brief Buzz Implementation as a node in ROS.
* @copyright 2016 MistLab. All rights reserved.
*/ */
#include "ros/ros.h" #include "ros/ros.h"
#include "sensor_msgs/NavSatFix.h" #include "sensor_msgs/NavSatFix.h"
#include "mavros_msgs/GlobalPositionTarget.h" #include "mavros_msgs/GlobalPositionTarget.h"
#include "mavros_msgs/CommandCode.h" #include "mavros_msgs/CommandCode.h"
#include "mavros_msgs/CommandInt.h"
#include "mavros_msgs/State.h" #include "mavros_msgs/State.h"
#include "mavros_msgs/BatteryStatus.h" #include "mavros_msgs/BatteryStatus.h"
#include "mavros_msgs/Mavlink.h" #include "mavros_msgs/Mavlink.h"
@ -117,6 +119,54 @@ cvt_spherical_coordinates(latitude,longitude,altitude);
//neighbour_location_handler( distance, azimuth, elevation, 01); //neighbour_location_handler( distance, azimuth, elevation, 01);
} }
int oldcmdID=0;
int rc_cmd;
bool rc_callback(mavros_msgs::CommandInt::Request &req,
mavros_msgs::CommandInt::Response &res){
if(req.command==oldcmdID)
return true;
else
oldcmdID=req.command;
switch(req.command)
{
case mavros_msgs::CommandCode::NAV_TAKEOFF:
ROS_INFO("RC_call: TAKE OFF!!!!");
rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_LAND:
ROS_INFO("RC_Call: LAND!!!!");
rc_cmd=mavros_msgs::CommandCode::NAV_LAND;
rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
ROS_INFO("GO HOME!!!!");
rc_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("GO TO!!!!");
double rc_goto[3];
rc_goto[0]=req.param1;
rc_goto[1]=req.param2;
rc_goto[2]=req.param3;
set_goto(rc_goto);
rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
rc_call(rc_cmd);
res.success = true;
break;
default:
res.success = false;
break;
}
return true;
}
/*controlc handler callback*/ /*controlc handler callback*/
static void ctrlc_handler(int sig) { static void ctrlc_handler(int sig) {
@ -129,12 +179,13 @@ int main(int argc, char **argv)
{ {
/*Compile the buzz code .bzz to .bo*/ /*Compile the buzz code .bzz to .bo*/
system("bzzparse /home/vivek/catkin_ws/src/rosbuzz/src/test.bzz /home/vivek/catkin_ws/src/rosbuzz/src/out.basm"); system("rm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
system("bzzasm /home/vivek/catkin_ws/src/rosbuzz/src/out.basm /home/vivek/catkin_ws/src/rosbuzz/src/out.bo /home/vivek/catkin_ws/src/rosbuzz/src/out.bdbg"); system("bzzparse ../catkin_ws/src/rosbuzz/src/test.bzz ../catkin_ws/src/rosbuzz/src/out.basm");
system("bzzasm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
/*initiate rosBuzz*/ /*initiate rosBuzz*/
ros::init(argc, argv, "rosBuzz"); ros::init(argc, argv, "rosBuzz");
ROS_INFO("Buzz_node");
/*Create node Handler*/ /*Create node Handler*/
ros::NodeHandle n_c; ros::NodeHandle n_c;
@ -151,20 +202,26 @@ int main(int argc, char **argv)
/*publishers*/ /*publishers*/
ros::Publisher goto_pub = n_c.advertise<mavros_msgs::GlobalPositionTarget>("go_to", 1000); //ros::Publisher goto_pub = n_c.advertise<mavros_msgs::GlobalPositionTarget>("go_to", 1000);
ros::Publisher cmds_pub = n_c.advertise<mavros_msgs::State>("newstate", 1000); //ros::Publisher cmds_pub = n_c.advertise<mavros_msgs::State>("newstate", 1000);
ros::Publisher payload_pub = n_c.advertise<mavros_msgs::Mavlink>("pay_load_out", 1000); ros::Publisher payload_pub = n_c.advertise<mavros_msgs::Mavlink>("pay_load_out", 1000);
/* Clients*/
ros::ServiceClient mav_client = n_c.serviceClient<mavros_msgs::CommandInt>("djicmd");
/*Services*/
ros::ServiceServer service = n_c.advertiseService("djicmd_rc", rc_callback);
ROS_INFO("Ready to receive Mav Commands from dji RC client");
/*loop rate of ros*/ /*loop rate of ros*/
ros::Rate loop_rate(1); ros::Rate loop_rate(1);
/* The bytecode filename */ /* The bytecode filename */
char* bcfname = "/home/vivek/catkin_ws/src/rosbuzz/src/out.bo"; //argv[1]; char* bcfname = "../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
/* The debugging information file name */ /* The debugging information file name */
char* dbgfname = "/home/vivek/catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2]; char* dbgfname = "../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
/* Set CTRL-C handler */ /* Set CTRL-C handler */
signal(SIGTERM, ctrlc_handler); signal(SIGTERM, ctrlc_handler);
signal(SIGINT, ctrlc_handler); signal(SIGINT, ctrlc_handler);
@ -175,39 +232,33 @@ int main(int argc, char **argv)
fprintf(stdout, "Bytecode file found and set\n"); fprintf(stdout, "Bytecode file found and set\n");
// buzz setting // buzz setting
mavros_msgs::CommandInt cmd_srv;
int count = 0; int count = 0;
while (ros::ok() && !done && !buzz_script_done()) while (ros::ok() && !done && !buzz_script_done())
{ {
/* Main loop */ /* Main loop */
buzz_script_step(); buzz_script_step();
/* Cleanup */
// buzz_script_destroy();
/*prepare the goto publish message */ /*prepare the goto publish message */
mavros_msgs::GlobalPositionTarget goto_set;
double* goto_pos = getgoto();
goto_set.latitude = goto_pos[0]; double* goto_pos = getgoto();
goto_set.longitude=goto_pos[1]; cmd_srv.request.param1 = goto_pos[0];
goto_set.altitude=goto_pos[2]; cmd_srv.request.param2 = goto_pos[1];
cmd_srv.request.param3 = goto_pos[2];
ROS_INFO("set value X = %f, Y =%f, Z= %f ",cmd_srv.request.param1,cmd_srv.request.param2,cmd_srv.request.param3);
/*prepare commands for takeoff,land and gohome*/ /*prepare commands for takeoff,land and gohome*/
mavros_msgs::State cmds_set; //char tmp[20];
char tmp[20]; //sprintf(tmp,"%i",getcmd());
sprintf(tmp,"%i",getcmd()); cmd_srv.request.command = getcmd();
cmds_set.mode = tmp; /* diji 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 'djicmd'"); }
/*Prepare Pay load to be sent*/ /*Prepare Pay load to be sent*/
unsigned long int* payload_out_ptr= out_msg_process(); unsigned long int* payload_out_ptr= out_msg_process();
mavros_msgs::Mavlink payload_out; mavros_msgs::Mavlink payload_out;
payload_out.payload64.push_back(*payload_out_ptr); payload_out.payload64.push_back(*payload_out_ptr);
/*publish prepared messages in respective topic*/ /*publish prepared messages in respective topic*/
goto_pub.publish(goto_set);
cmds_pub.publish(cmds_set);
payload_pub.publish(payload_out); payload_pub.publish(payload_out);
/*run once*/ /*run once*/
@ -216,6 +267,8 @@ int main(int argc, char **argv)
loop_rate.sleep(); loop_rate.sleep();
++count; ++count;
} }
/* Cleanup */
buzz_script_destroy();
/* Stop the robot */ /* Stop the robot */
uav_done(); uav_done();

View File

@ -15,12 +15,21 @@ if(id == 0) {
neighbors.get(robot_id).distance + value) neighbors.get(robot_id).distance + value)
}) })
} }
a=1
} }
# Executed at each time step. # Executed at each time step.
function step() { function step() {
neighbors.broadcast("dist_to_source", mydist) neighbors.broadcast("dist_to_source", mydist)
print(mydist) print(mydist)
#if(a==1){ uav_takeoff()
# a=0
# print("Buzz: take off")
# }
#else {uav_land()
# a=1
# print("Buzz: land")
# }
} }