open loop

include/roscontroller.h

@@ -70,6 +70,7 @@ private:
         double DEFAULT_REFERENCE_LONGITUDE = -73.636075;
 
  	double cur_pos[3];
+ 	double target[3];
  	double home[3];
  	double cur_rel_altitude;
 	uint64_t payload;

launch/rosbuzz-testing-sitl.launch

@@ -27,7 +27,7 @@
 	<!-- run rosbuzz -->
 	<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
 		<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
-		<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/script/testalone.bzz" />
+		<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz" />
 		<param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="buzzcmd" />
 		<param name="in_payload" value="inMavlink"/>

script/testsolo.bzz

@@ -51,10 +51,9 @@ function hexagon() {
   	statef=land
   	} else {
   		if(timeW >= (WAIT_TIMEOUT / 2)){
-  			#uav_moveto(4.0,0.0)
+  			uav_moveto(0.03,0.0)
   		} else {
-  			#uav_moveto(4.0,0.0)
-  			#uav_moveto(0.0,4.0)
+  			uav_moveto(0.0,0.03)
   		}
   		timeW = timeW+1
   	}

src/roscontroller.cpp

@@ -7,6 +7,11 @@ namespace rosbzz_node{
 	---------------*/
 	roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)	
 	{
+
+		home[0]=0.0;home[1]=0.0;home[2]=0.0;
+		target[0]=0.0;target[1]=0.0;target[2]=0.0;
+		cur_pos[0]=0.0;cur_pos[1]=0.0;cur_pos[2]=0.0;
+
 		ROS_INFO("Buzz_node");
 		/*Obtain parameters from ros parameter server*/
 	  	Rosparameters_get(n_c_priv);
@@ -32,7 +37,10 @@ namespace rosbzz_node{
 		setpoint_counter = 0;
 		fcu_timeout = TIMEOUT;
         
-		home[0]=0.0;home[1]=0.0;home[2]=0.0;
+
+		//ros::Duration(0.1).sleep();
+
+
 	}
 
 	/*---------------------
@@ -115,7 +123,7 @@ namespace rosbzz_node{
     			timer_step+=1;
    				maintain_pos(timer_step);
 
-   				std::cout<< "HOME: " << home[0] << ", " << home[1] <<std::endl;
+   				std::cout<< "HOME: " << home[0] << ", " << home[1];
 			}
 			/* Destroy updater and Cleanup */
     			//update_routine(bcfname.c_str(), dbgfname.c_str(),1);
@@ -764,8 +772,10 @@ namespace rosbzz_node{
                 ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]);
                     
                 /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
-		moveMsg.pose.position.x = local_pos[1]+y;
-		moveMsg.pose.position.y = local_pos[0]+x;
+        target[0]+=y;
+		target[1]+=x;
+        moveMsg.pose.position.x = target[0];//local_pos[1]+y;
+		moveMsg.pose.position.y = target[1];
 		moveMsg.pose.position.z = z;
 
 		moveMsg.pose.orientation.x = 0;