Can now reliably be installed on 18.04 with ROS Melodic

.gitignore

@@ -1,4 +1,4 @@
-config/mocap_*
+config/mocap*
 launch/cortex_bridge.launch
 launch/debug.launch
 launch/klausen_dampen.launch

airframes/18.04/4000_spiri

@@ -20,6 +20,7 @@ param set-default MPC_Z_VEL_MAX_UP 1.0
 param set EKF2_AID_MASK 1
 param set EKF2_HGT_MODE 0
 param set SYS_FAILURE_EN 0
+param set COM_RCL_EXCEPT 4
 param set NAV_RCL_ACT 0
 param set NAV_DLL_ACT 0
 

airframes/18.04/4001_spiri_with_tether

@@ -21,6 +21,9 @@ param set MPC_TKO_SPEED 1.0
 param set EKF2_AID_MASK 1
 param set EKF2_HGT_MODE 0
 param set SYS_FAILURE_EN 0
+param set COM_RCL_EXCEPT 4
+param set NAV_RCL_ACT 0
+param set NAV_DLL_ACT 0
 
 set MIXER quad_x
 set PWM_OUT 1234

models/spiri_with_tether/spiri_with_tether.sdf.jinja

@@ -4,17 +4,17 @@
 {#--------------------------------The parameters bellow are the ones to be tweaked---------------------------#}
 {#--------------------------------------------:Tweakable parameters:-----------------------------------------#}
 {%- set number_elements = 1 -%}
-{%- set tl = 1 -%}	  			                {#- tl: length of the tether element (meters) -#}
+{%- set tl = 0.8 -%}                        {#- tl: length of the tether element (meters) -#}
 {%- set cr = 0.005 -%}                      {#- cr: radius of the tether element (meters) -#}
 {%- set cr_v = 0.005 -%}                    {#- cr_v: radius of the tether element visual (meters) -#}
 {%- set sr = 0.01 -%}                       {#- sr: element joint radius (sphere) (meters) -#}
 {%- set m = 0.005 * tl -%}                  {#- m: mass of the element (kg), given the length -#}
 {%- set cda = 1.253582 -%}                  {#- cda: the ratio of the drag coefficient before stall. -#}
 {%- set cda_stall = 1.4326647564469914 -%}  {#- cda_stall: the ratio of the drag coefficient after stall. -#}
-{%- set damping = 0.05 -%}		              {#- Model damping -#}
+{%- set damping = 0.0001 -%}                  {#- Model damping -#}
 {%- set friction = 0.0 -%}                  {#- Friction of the model relative to the world -#}
-{%- set tether_stiffness = 0.05 -%}		      {#- Tether stiffness  -#}
+{%- set tether_stiffness = 0.0005 -%}         {#- Tether stiffness  -#}
-{%- set joint_stiffness = 0.05 -%}		      {#- Joint stiffness, i.e where tether attaches to drone -#}
+{%- set joint_stiffness = 0.0005 -%}          {#- Joint stiffness, i.e where tether attaches to drone -#}
 {%- set spring_reference = 0.0 -%}          {#- Reference where the spring forces are applied -#}
 {%- set element_color = 'White' -%}         {#- Color of the tether elements -#}
 {%- set joint_color = 'Red' -%}             {#- Color of the tether element joints -#}
@@ -24,8 +24,9 @@
 {#----------------------------------------------------------------------------------------------------------#}
 {#---------------------------------------------:Payload parameters:-----------------------------------------#}
 {#----------------------------------------------------------------------------------------------------------#}
-{%- set payload_m = 0.5 -%}                 {#- Payload mass (kg) -#}
+{%- set payload_m = 0.25 -%}  {#- Payload mass (kg). Used to be 0.5 -#}
 {%- set pr = 0.025 -%}       {#- Payload radius (meters). Has to be less than FULL tether length -#}
+{%- set add_imu = 1 -%}      {#- Add IMU to payload for localization-#}
 {#-----------------------------------------------------------------------------------------------------------#}
 {#----------------------------------------------:Spiri parameters:-------------------------------------------#}
 {#-----------------------------------------------------------------------------------------------------------#}
@@ -62,7 +63,7 @@
 {%- set pload_joint_z = tl/2 -%}
 {%- endif -%}
 
-{%- set tether_pos =  vehicle_spawn_point_z - tl/2  -%}  {#- Spawns tether vertically -#}
+{%- set tether_pos =  vehicle_spawn_point_z - full_tether_len/2  -%}  {#- Spawns tether vertically -#}
 {#----------------------------------------------------------------------------------------------------------#}
 {#-----------------------------------------------:MACROS:---------------------------------------------------#}
 {#----------------------------------------------------------------------------------------------------------#}
@@ -114,7 +115,7 @@
       <jointName>tether_mass_connection</jointName>
     </plugin>
 {%- endif %}
-  </model>
+  <!--/model-->
 {%- endmacro -%}
 
 {%- macro cylinder(tl, cr_v) -%}
@@ -126,6 +127,51 @@
         </geometry>
 {%- endmacro -%}
 
+{%- macro imu_link(x, y, z) -%}
+	    <link name='pload_imu'>
+	      <pose>{{ x }} {{ y }} {{ z }} 0 0 0</pose>
+	      <inertial>
+		<mass>0.015</mass>
+		<inertia>
+		  <ixx>1e-05</ixx>
+		  <ixy>0</ixy>
+		  <ixz>0</ixz>
+		  <iyy>1e-05</iyy>
+		  <iyz>0</iyz>
+		  <izz>1e-05</izz>
+		</inertia>
+	      </inertial>
+	    </link>
+	    <joint name='pload_imu_joint' type='revolute'>
+	      <child>pload_imu</child>
+	      <parent>payload</parent>
+	      <axis>
+		<xyz>1 0 0</xyz>
+		<limit>
+		  <lower>0</lower>
+		  <upper>0</upper>
+		  <effort>0</effort>
+		  <velocity>0</velocity>
+		</limit>
+		<dynamics>
+		  <spring_reference>0</spring_reference>
+		  <spring_stiffness>0</spring_stiffness>
+		</dynamics>
+		<use_parent_model_frame>0</use_parent_model_frame>
+	      </axis>
+	    </joint>
+      <plugin name='imu_plugin' filename='libgazebo_ros_imu.so'>
+        <robotNamespace/>
+        <alwaysOn>true</alwaysOn>
+        <bodyName>pload_imu</bodyName>
+        <topicName>/pload_imu</topicName>
+        <serviceName>imu_service</serviceName>
+        <gaussianNoise>0.0</gaussianNoise>
+        <updateRate>20.0</updateRate>
+      </plugin>
+    </model>
+{%- endmacro -%}
+
 {%- macro sphere(sr) -%}
       <geometry>
           <sphere>
@@ -357,7 +403,7 @@
  <model name="spiri_with_tether">
   <pose> 0 0 0 0.0 0.0 0.0</pose> {# only have to change this to change spawn location #}
   <include>
-    <uri>model://spiri</uri>
+    <uri>model://spiri_mocap</uri>
     <pose>0.0 0.0 {{ vehicle_spawn_point_z }} 0.0 0.0 {{ yaw }}</pose>
     <static>{{ debug }}</static>
   </include>
@@ -385,6 +431,11 @@
   {%- else -%}
   {%- set n = number_elements - 1 -%}
   {% endif %} {{ payload(payload_m,n,pr,pload_spawn_x,pload_spawn_y,pload_spawn_z,pload_joint_x,pload_joint_y,pload_joint_z) }}
+
+{#------------- ADD IMU -----------#}
+  {%- if add_imu == 1 -%}
+  {{ imu_link(pload_spawn_x,pload_spawn_y,pload_spawn_z) }}
+  {% endif %}
  </model>
 </sdf>
 

px4_setup/px4_bash.sh

@@ -0,0 +1,9 @@
+echo "Copying model files to px4 directory..."
+cp -R ~/catkin_ws/src/oscillation_ctrl/models/* ~/PX4-Autopilot/Tools/sitl_gazebo/models
+echo "Copying world files to px4 directory..."
+cp -R ~/catkin_ws/src/oscillation_ctrl/worlds/* ~/PX4-Autopilot/Tools/sitl_gazebo/worlds
+echo "Copying airframes files to px4 directory..."
+cp -R ~/catkin_ws/src/oscillation_ctrl/airframes/18.04/* ~/PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes
+echo "Adding airframe names to px4_sitl make targets..."
+cp -R ~/catkin_ws/src/oscillation_ctrl/px4_setup/CMakeLists.txt ~/PX4-Autopilot/Tools/sitl_gazebo/
+echo "Done!"

src/path_follow.cpp

@@ -143,7 +143,7 @@ int main(int argc, char **argv)
 	if (waypoint_cl.call(wpoint_srv))
 	{
 		ROS_INFO("Waypoints received");
-		// populate desired waypoints - this is only for original hover as 
+		// populate desired waypoints
 		buff_pose.pose.position.x = wpoint_srv.response.xd.x;
 		buff_pose.pose.position.y = wpoint_srv.response.xd.y;
 		buff_pose.pose.position.z = wpoint_srv.response.xd.z;
@@ -167,11 +167,14 @@ int main(int argc, char **argv)
 				last_request = ros::Time::now();
 			}
 		}
-		
+		if (waypoint_cl.call(wpoint_srv))
-		if(current_state.mode == "OFFBOARD" && current_state.armed){
+		{
-			ROS_INFO_ONCE("Spiri is taking off");
+			// check if waypoints have changed
+			buff_pose.pose.position.x = wpoint_srv.response.xd.x;
+			buff_pose.pose.position.y = wpoint_srv.response.xd.y;
+			buff_pose.pose.position.z = wpoint_srv.response.xd.z;
+			alt_des = wpoint_srv.response.xd.z;
 		}
-
 		// Check if we want to use oscillation controller
 		if (ros::param::has("/status/use_ctrl")){
 			ros::param::get("/status/use_ctrl", oscillation_damp);
@@ -186,7 +189,7 @@ int main(int argc, char **argv)
 				// Publish position setpoints
 				local_pos_pub.publish(buff_pose);
 			}
-			ROS_INFO("Des Altitude: %.2f", wpoint_srv.response.xd.z);
+			ROS_INFO("Des Altitude: %.2f", alt_des);
 			ROS_INFO("Cur Altitude: %.2f", current_altitude);	
 			ROS_INFO("---------------------------");	
 		} else {

src/wpoint_tracker.py

@@ -25,19 +25,12 @@ class Main:
 		self.param_exists = False # check in case param does not exist
 
 		while self.param_exists == False:
-			if rospy.has_param('sim/waypoints'):
+			if rospy.has_param('status/waypoints'):
-				self.wpoints	= rospy.get_param('sim/waypoints') # get waypoints
+				self.wpoints	= rospy.get_param('status/waypoints') # get waypoints
 				self.xd.x = self.wpoints['x']
 				self.xd.y = self.wpoints['y']
 				self.xd.z = self.wpoints['z']
 				self.param_exists = True
-			elif rospy.has_param('mocap/waypoints'):
-				self.wpoints	= rospy.get_param('mocap/waypoints') # get waypoints
-				self.xd.x = self.wpoints['x']
-				self.xd.y = self.wpoints['y']
-				self.xd.z = self.wpoints['z']
-				self.param_exists = True
-
 			elif rospy.get_time() - self.tstart >= 5.0:
 				self.xd.x = 0.0
 				self.xd.y = 0.0