launch file commit

This commit is contained in:
Vivek Shankar Varadharajan 2016-10-09 19:58:50 -04:00
parent d664adc06b
commit a128625619
11 changed files with 59 additions and 185 deletions

0
.gitignore vendored Normal file
View File

View File

@ -37,4 +37,5 @@ install(TARGETS rosbuzz_node
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
find_package(catkin REQUIRED COMPONENTS roslaunch)
roslaunch_add_file_check(launch)

11
launch/rosbuzz.launch Normal file
View File

@ -0,0 +1,11 @@
<!-- Launch file for ROSBuzz -->
<launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<param name="bzzfile_name" value="../catkin_ws/src/rosbuzz/src/test.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="rc_cmd" />
<param name="fcclient_name" value="fc_cmd" />
</node>
<param name="robot_id" value="1"/>
</launch>

View File

@ -40,8 +40,8 @@
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>
<!-- <build_depend>message_generation</build_depend> -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- The export tag contains other, unspecified, tags -->
<export>

View File

@ -199,13 +199,13 @@ static int buzz_register_hooks() {
/****************************************/
int buzz_script_set(const char* bo_filename,
const char* bdbg_filename) {
const char* bdbg_filename, int robot_id) {
/* Get hostname */
char hstnm[30];
gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Knn */
int id = strtol(hstnm + 1, NULL, 10);
int id = robot_id; //strtol(hstnm + 1, NULL, 10);
cout << " Robot ID" << id<< endl;
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM);

View File

@ -22,7 +22,7 @@ void in_msg_process(uint64_t* payload);
uint64_t* out_msg_process();
int buzz_script_set(const char* bo_filename,
const char* bdbg_filename);
const char* bdbg_filename, int robot_id);
void buzz_script_step();
void buzz_script_destroy();

View File

@ -1,150 +0,0 @@
!24
'init
'i
's
'swarm
'create
'join
'step
'neighbors
'foreach
'print
'robot
':
'distance =
'distance
',
'azimuth =
'azimuth
'elevation =
'elevation
'exec
'uav_takeoff
'uav_goto
'reset
'destroy
pushs 0
pushcn @__label_1
gstore
pushs 6
pushcn @__label_2
gstore
pushs 22
pushcn @__label_8
gstore
pushs 23
pushcn @__label_9
gstore
nop
@__label_0
@__exitpoint
done
@__label_1
pushs 1 |3,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushi 0 |3,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gstore |3,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 2 |4,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 3 |4,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gload |4,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 4 |4,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
tget |4,16,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushi 10 |4,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushi 1 |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
callc |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gstore |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 2 |5,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gload |5,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 5 |5,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
tget |5,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushi 0 |5,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
callc |5,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
ret0 |7,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@__label_2
pushs 7 |12,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gload |12,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 8 |12,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
tget |12,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushl @__label_3 |13,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushi 1 |17,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
callc |17,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 1 |18,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gload |18,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushi 1 |18,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
eq |18,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
jumpz @__label_4 |18,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 2 |19,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gload |19,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 19 |19,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
tget |19,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushl @__label_6 |19,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushi 1 |19,34,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
callc |19,34,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 1 |20,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushi 0 |20,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gstore |20,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
jump @__label_5 |22,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@__label_4 |22,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 2 |23,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gload |23,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 19 |23,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
tget |23,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushl @__label_7 |23,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushi 1 |23,51,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
callc |23,51,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 1 |24,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushi 1 |24,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gstore |24,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@__label_5 |25,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
ret0 |35,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@__label_3
pushs 9 |14,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gload |14,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 10 |14,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
lload 1 |14,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 11 |14,25,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 12 |15,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
lload 2 |15,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 13 |15,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
tget |15,37,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 14 |15,39,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 15 |16,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
lload 2 |16,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 16 |16,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
tget |16,36,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 14 |16,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 17 |17,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
lload 2 |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 18 |17,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
tget |17,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushi 11 |17,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
callc |17,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
ret0 |17,41,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@__label_6
pushs 20 |19,30,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gload |19,30,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushi 0 |19,32,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
callc |19,32,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
ret0 |19,33,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@__label_7
pushs 21 |23,27,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gload |23,27,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushf 1.1234 |23,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushf 2.2345 |23,35,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushf 3.3456 |23,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushi 3 |23,49,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
callc |23,49,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
ret0 |23,50,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@__label_8
ret0 |39,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@__label_9
ret0 |43,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz

Binary file not shown.

Binary file not shown.

View File

@ -30,16 +30,15 @@ using namespace std;
/**
* This program implements Buzz node in ros using mavros_msgs, Developed for Dji M100.
* This program implements Buzz node in ros using mavros_msgs.
*/
static int done = 0;
static double cur_pos[3];
static uint64_t payload;
static std::map< int, Pos_struct> neighbours_pos_map;
static int timer_step=0;
static int robot_id=0;
/*Refresh neighbours Position for every ten step*/
void maintain_pos(int tim_step){
@ -188,18 +187,37 @@ static void ctrlc_handler(int sig) {
int main(int argc, char **argv)
{
/*Compile the buzz code .bzz to .bo*/
system("rm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../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");
ros::ServiceServer service;
/*initiate rosBuzz*/
ros::init(argc, argv, "rosBuzz");
ROS_INFO("Buzz_node");
/*Create node Handler*/
ros::NodeHandle n_c;
std::string bzzfile_name, fcclient_name, rcservice_name; //, rcclient;
bool rcclient;
/*Obtain .bzz file name from parameter server*/
if(ros::param::get("/rosbuzz_node/bzzfile_name", bzzfile_name));
else {ROS_ERROR("Provide a .bzz file to run in Launch file"); system("rosnode kill rosbuzz_node");}
/*Obtain rc service option from parameter server*/
if(ros::param::get("/rosbuzz_node/rcclient", rcclient)){
if(rcclient==true){
/*Service*/
if(ros::param::get("/rosbuzz_node/rcservice_name", rcservice_name)){
service = n_c.advertiseService(rcservice_name, rc_callback);
ROS_INFO("Ready to receive Mav Commands from RC client");
}
else{ROS_ERROR("Provide a name topic name for rc service in Launch file"); system("rosnode kill rosbuzz_node");}
}
else if(rcclient==false){ROS_INFO("RC service is disabled");}
}
else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");}
/*Obtain fc client name from parameter server*/
if(ros::param::get("/rosbuzz_node/fcclient_name", fcclient_name));
else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}
/*Obtain robot_id from parameter server*/
ros::param::get("/rosbuzz_node/robot_id", robot_id);
/*subscribers*/
ros::Subscriber current_position_sub = n_c.subscribe("current_pos", 1000, current_pos);
@ -213,15 +231,19 @@ int main(int argc, char **argv)
ros::Publisher payload_pub = n_c.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
/* Clients*/
ros::ServiceClient mav_client = n_c.serviceClient<mavros_msgs::CommandInt>("djicmd");
ros::ServiceClient mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
cout<< " rc client name"<<rcservice_name;
/*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*/
ros::Rate loop_rate(1);
/*Compile the buzz code .bzz to .bo*/
stringstream bzzfile_in_compile;
bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" ../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(bzzfile_in_compile.str().c_str());
system("bzzasm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
/* The bytecode filename */
char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
@ -233,7 +255,7 @@ int main(int argc, char **argv)
/* Set the Buzz bytecode */
if(buzz_script_set(bcfname, dbgfname)) {
if(buzz_script_set(bcfname, dbgfname,robot_id)) {
fprintf(stdout, "Bytecode file found and set\n");
/*Commands for dji flight controller*/
@ -255,7 +277,6 @@ int main(int argc, char **argv)
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 = getcmd();
/* diji client call*/
if (mav_client.call(cmd_srv)){ ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }

View File

@ -1,6 +1,6 @@
# Executed once at init time.
function init() {
i=0
i=1
s = swarm.create(10)
s.join()
@ -8,6 +8,7 @@ s.join()
# Executed at each time step.
function step() {
# print all the neighbours position
neighbors.foreach(
function(rid, data) {
@ -17,20 +18,10 @@ neighbors.foreach(
"elevation = ", data.elevation) })
if(i==1){
s.exec(function() {uav_takeoff()})
i=0
}
else{
s.exec(function() {uav_goto(1.1234,2.2345,3.3456)})
i=1
}
#if(i==5){
#s.leave()
#i=0
#}
#else{
#i=1
#}
#i = i + 1
}