Merge branch 'master' of https://github.com/MISTLab/ROSBuzz

include/roscontroller.h

@@ -1,6 +1,7 @@
 #pragma once
 #include <ros/ros.h>
 #include <sensor_msgs/NavSatFix.h>
+#include <std_msgs/UInt8.h>
 #include "mavros_msgs/GlobalPositionTarget.h"
 #include "mavros_msgs/CommandCode.h"
 #include "mavros_msgs/CommandLong.h"
@@ -53,6 +54,7 @@ private:
         //int oldcmdID=0;
 	int rc_cmd;
 	int barrier;
+	int message_number=0;
 	std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by;
 	bool rcclient;
 	bool multi_msg;
@@ -65,6 +67,7 @@ private:
 	ros::Subscriber payload_sub;
 	ros::Subscriber flight_status_sub;
 	ros::Subscriber obstacle_sub;
+	ros::Subscriber Robot_id_sub;
 	/*Commands for flight controller*/
   	//mavros_msgs::CommandInt cmd_srv;
   	mavros_msgs::CommandLong cmd_srv;
@@ -136,6 +139,9 @@ private:
 	/* RC commands service */
 	bool rc_callback(mavros_msgs::CommandLong::Request  &req, mavros_msgs::CommandLong::Response &res);
 
+	/*robot id sub callback*/
+	void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
+
 	void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
 
 	void GetSubscriptionParameters(ros::NodeHandle node_handle);

launch/rosbuzz_2_parallel.launch

@@ -3,6 +3,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/test1.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" />
 		<param name="fcclient_name" value="/djicmd" />

log.txt

@@ -0,0 +1,3 @@
+
+2017-01-08-14-24-24    enter TranslateFunc
+2017-01-08-14-24-24    start to read frames!

src/buzz_update.cpp

@@ -294,7 +294,7 @@ buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
 	}
 
 	else{
-		gettimeofday(&t1, NULL);
+		//gettimeofday(&t1, NULL);
 		if(neigh==0 && (!is_msg_present())){ 
 			fprintf(stdout,"Sending code... \n");		
 			code_message_outqueue_append();
@@ -309,10 +309,10 @@ buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
 		if(tObj->i.value==no_of_robot) { 
 			*(int*)(updater->mode) = CODE_RUNNING;
 			gettimeofday(&t2, NULL);
-			collect_data();
+			//collect_data();
 			timer_steps=0;
-			//neigh=-1;
+			neigh=0;
-			//buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgfname, *(size_t*)(updater->bcode_size));
+			buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgfname, *(size_t*)(updater->bcode_size));
 			//buzzvm_function_call(m_tBuzzVM, "updated", 0);
 			updated=1;	
 			}

src/buzz_utility.cpp

@@ -233,6 +233,9 @@ namespace buzz_utility{
    		buzzvm_pushs(VM,  buzzvm_string_register(VM, "log", 1));
                 buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
                 buzzvm_gstore(VM);
+                buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_moveto", 1));
+   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
+   		buzzvm_gstore(VM);
                 buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_goto", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto));
    		buzzvm_gstore(VM);

src/buzzuav_closures.cpp

@@ -99,8 +99,8 @@ void gps_from_rb(double  range, double bearing, double out[3]) {
 	double lon = cur_pos[1]*M_PI/180.0;
 //	bearing = bearing*M_PI/180.0;
  	out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing));
-	out[0] = out[0]*180.0/M_PI;
 	out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0]));
+	out[0] = out[0]*180.0/M_PI;
 	out[1] = out[1]*180.0/M_PI;
 	out[2] = height; //constant height.
 }

src/roscontroller.cpp

@@ -68,36 +68,26 @@ namespace rosbzz_node{
 					//std::cout<<"long obt"<<neigh_tmp.longitude<<endl;  					
 					}
 				neigh_pos_pub.publish(neigh_pos_array); 
-				//fprintf(stdout, "before update routine\n");
+			
 				/*Check updater state and step code*/
   				update_routine(bcfname.c_str(), dbgfname.c_str());
-      				/*Step buzz script */
+      				
-				//fprintf(stdout, "before code step\n");
+				/*Step buzz script */
-				if(get_update_status()){
-				buzz_utility::buzz_update_set((get_updater())->bcode, dbgfname.c_str(), *(size_t*)((get_updater())->bcode_size));
-				set_read_update_status();
-				mavros_msgs::Mavlink stop_req_packet;
-				stop_req_packet.payload64.push_back((uint64_t) XBEE_STOP_TRANSMISSION);
-				payload_pub.publish(stop_req_packet);
-				std::cout << "request xbee to stop multi-packet transmission" << std::endl;
-				}
       				buzz_utility::buzz_script_step();
 				/*Prepare messages and publish them in respective topic*/
-				//fprintf(stdout, "before publish msg\n");
+	
 		  		prepare_msg_and_publish();
+				/*Set multi message available after update*/
+				if(get_update_status()){
+					set_read_update_status();
+					multi_msg=true;
+				}
     				/*run once*/
     				ros::spinOnce();
 				/*loop rate of ros*/
-				/*if( get_update_mode() == CODE_STANDBY){
 				 ros::Rate loop_rate(10);
 				 loop_rate.sleep();
-				}	
-				else{*/
-				 ros::Rate loop_rate(10);
-				 loop_rate.sleep();
-				//} 
  				/*sleep for the mentioned loop rate*/
-    				
     				timer_step+=1;
    				maintain_pos(timer_step);
 				
@@ -126,7 +116,8 @@ namespace rosbzz_node{
   		}
   		else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");} 
   		/*Obtain robot_id from parameter server*/
-  		n_c.getParam("robot_id", robot_id);
+  		//n_c.getParam("robot_id", robot_id);
+		//robot_id=(int)buzz_utility::get_robotid();
 		/*Obtain out payload name*/
   		n_c.getParam("out_payload", out_payload);
 		/*Obtain in payload name*/
@@ -178,13 +169,13 @@ namespace rosbzz_node{
   		//battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this);
   		payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this);
 		//flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this);
-
+		Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this);
   		obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
   		/*publishers*/
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
 		neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);	
 		/* Service Clients*/
-		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armingclient);
+		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
 		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
 		mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
 
@@ -318,6 +309,13 @@ namespace rosbzz_node{
 				cout<<" [Debug:] sent message "<<out[k]<<endl;
 			}
 		}*/
+
+		/*Add Robot id and message number to the published message*/
+		if (message_number < 0) message_number=0;
+		else message_number++;
+		payload_out.sysid=(uint8_t)robot_id;
+		payload_out.msgid=(uint32_t)message_number;
+		
      		/*publish prepared messages in respective topic*/
     		payload_pub.publish(payload_out);
     		delete[] out;
@@ -334,27 +332,32 @@ namespace rosbzz_node{
 			/*allocate mem and clear it*/
 			buff_send =(uint8_t*)malloc(sizeof(uint16_t)+updater_msgSize);
 			memset(buff_send, 0,sizeof(uint16_t)+updater_msgSize);
-			   /*Append updater msg size*/
+		   	/*Append updater msg size*/
-			   *(uint16_t*)(buff_send + tot)=updater_msgSize;
+		  	 *(uint16_t*)(buff_send + tot)=updater_msgSize;
-			   //fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
+		   	//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
-	      		   tot += sizeof(uint16_t);
+      		   	tot += sizeof(uint16_t);
-			   /*Append updater msgs*/   	
+		   	/*Append updater msgs*/   	
-	    		   memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
+    		   	memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
-			   tot += updater_msgSize;
+		   	tot += updater_msgSize;
-			   /*Destroy the updater out msg queue*/
+		   	/*Destroy the updater out msg queue*/
-			    destroy_out_msg_queue();
+		    	destroy_out_msg_queue();
-			    uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t))); 
+		    	uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t))); 
-   			    uint64_t* payload_64 = new uint64_t[total_size];
+		    	uint64_t* payload_64 = new uint64_t[total_size];
-		  	    memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
+	  	    	memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
-			    delete[] buff_send;
+		    	delete[] buff_send;
-			    /*Send a constant number to differenciate updater msgs*/
+		    	/*Send a constant number to differenciate updater msgs*/
-			    update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT);
+		    	update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT);
-			    for(int i=0;i<total_size;i++){
+		    	for(int i=0;i<total_size;i++){
-    				update_packets.payload64.push_back(payload_64[i]);
+				update_packets.payload64.push_back(payload_64[i]);
-    			    }
+		    	}
-			    payload_pub.publish(update_packets);
+		    	/*Add Robot id and message number to the published message*/
-			    multi_msg=false;
+		    	if (message_number < 0) message_number=0;
-			    delete[] payload_64;
+		    	else message_number++;
+		    	update_packets.sysid=(uint8_t)robot_id;
+		    	update_packets.msgid=(uint32_t)message_number;
+		    	payload_pub.publish(update_packets);
+		    	multi_msg=false;
+		    	delete[] payload_64;
 		}
 		/*Request xbee to stop any multi transmission updated not needed any more*/
 		//if(get_update_status()){
@@ -639,7 +642,7 @@ namespace rosbzz_node{
 
 		}
 
-		else{
+		else if(msg->payload64.size() > 3){
 			uint64_t message_obt[msg->payload64.size()];
 			/* Go throught the obtained payload*/
 			for(int i=0;i < (int)msg->payload64.size();i++){
@@ -663,9 +666,10 @@ namespace rosbzz_node{
 				neighbours_pos_payload[i]=cartesian_neighbours_pos[i]-cartesian_cur_pos[i];
 			}
 			double *cvt_neighbours_pos_payload = cvt_neighbours_pos_test;
-	//		cvt_spherical_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload);
+			//double cvt_neighbours_pos_payload[3];
+			//cvt_spherical_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload);
 			/*Extract robot id of the neighbour*/
-	 		uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));  
+	 		uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));
 			cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl;
 	//		cout << "Rel Test Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_test[0] << ", "<< cvt_neighbours_pos_test[2] << ", "<< cvt_neighbours_pos_test[3] << endl;
 			/*pass neighbour position to local maintaner*/
@@ -726,7 +730,11 @@ namespace rosbzz_node{
    		}
    		return true;
 	}
-
+	void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){
+	robot_id=(int)msg->data;
+	
+	}
+	
 }
 
 

src/test1.bzz

@@ -12,11 +12,11 @@ function updated_neigh(){
 neighbors.broadcast(updated, update_no)
 }
 
-TARGET_ALTITUDE = 10.0
+TARGET_ALTITUDE = 2.0
  
 # Lennard-Jones parameters
-TARGET     = 50.0	#0.000001001
+TARGET     = 10.0	#0.000001001
-EPSILON    = 0.5	#0.001
+EPSILON    = 10.0	#0.001
  
 # Lennard-Jones interaction magnitude
 function lj_magnitude(dist, target, epsilon) {
@@ -41,8 +41,8 @@ function hexagon() {
   if(neighbors.count() > 0)
     math.vec2.scale(accum, 1.0 / neighbors.count())
   # Move according to vector
-#  print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
+  print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
-#  uav_moveto(accum.x, accum.y)
+  uav_moveto(accum.x, accum.y)
 }
  
 ########################################
@@ -101,28 +101,28 @@ neighbors.listen("cmd",
    
 
 )
-if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0)
-	statef=hexagon
 }
 
 function takeoff() {
-	#if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+	log("TakeOff: ", flight.status)
-	#	barrier_set(ROBOTS,hexagon)
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
-	#	barrier_ready()
+		barrier_set(ROBOTS,hexagon)
-	#}
+		barrier_ready()
-	if( flight.status !=3){
+	}
+	else if( flight.status !=3){
 		log("Altitude: ", TARGET_ALTITUDE)
 		neighbors.broadcast("cmd", 22)
 		uav_takeoff(TARGET_ALTITUDE)
 	}
 }
 function land() {
-	#log("Land: ", flight.status)
+	log("Land: ", flight.status)
-	#if(flight.status != 0 and flight.status != 4){
+	if(flight.status == 2 or flight.status == 3){
-	#	neighbors.broadcast("cmd", 21)
+		neighbors.broadcast("cmd", 21)
-	#	uav_land()
+		uav_land()
-	#}
+	}
-	uav_land()
+	else
+		statef=idle
 }
 
 # Executed once at init time.

src/testalone.bzz

@@ -0,0 +1,46 @@
+
+# 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"
+####################################################################################################
+# Updater related
+# This should be here for the updater to work, changing position of code will crash the updater
+####################################################################################################
+updated="update_ack"
+update_no=0
+function updated_neigh(){
+neighbors.broadcast(updated, update_no)
+}
+
+TARGET_ALTITUDE = 2.0
+ 
+# Executed once at init time.
+function init() {
+}
+
+# Executed at each time step.
+function step() {
+
+	if(flight.rc_cmd==22) {
+		flight.rc_cmd=0
+		uav_takeoff(TARGET_ALTITUDE)		
+	} else if(flight.rc_cmd==21) {
+		flight.rc_cmd=0
+		uav_land()
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		uav_goto()
+	}
+
+# test moveto cmd
+if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0)
+	uav_moveto(0.5, 0.5)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}