Service and client tested Implementation
This commit is contained in:
parent
622fd9ac77
commit
8d6efae326
|
@ -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;
|
||||||
|
}
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
|
50
src/out.basm
50
src/out.basm
|
@ -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
|
||||||
|
|
BIN
src/out.bdbg
BIN
src/out.bdbg
Binary file not shown.
BIN
src/out.bo
BIN
src/out.bo
Binary file not shown.
115
src/rosbuzz.cpp
115
src/rosbuzz.cpp
|
@ -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,47 +232,43 @@ 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*/
|
||||||
ros::spinOnce();
|
ros::spinOnce();
|
||||||
/*sleep for the mentioned loop rate*/
|
/*sleep for the mentioned loop rate*/
|
||||||
loop_rate.sleep();
|
loop_rate.sleep();
|
||||||
++count;
|
++count;
|
||||||
}
|
}
|
||||||
|
/* Cleanup */
|
||||||
|
buzz_script_destroy();
|
||||||
|
|
||||||
/* Stop the robot */
|
/* Stop the robot */
|
||||||
uav_done();
|
uav_done();
|
||||||
|
|
|
@ -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")
|
||||||
|
# }
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue