forked from cesar.alejandro/oscillation_ctrl
Can now reliably be installed on 18.04 with ROS Melodic
This commit is contained in:
parent
a99ba1059c
commit
9284b4b7ba
|
|
@ -1,4 +1,4 @@
|
|||
config/mocap_*
|
||||
config/mocap*
|
||||
launch/cortex_bridge.launch
|
||||
launch/debug.launch
|
||||
launch/klausen_dampen.launch
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
|
@ -4,28 +4,29 @@
|
|||
{#--------------------------------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 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 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 friction = 0.0 -%} {#- Friction of the model relative to the world -#}
|
||||
{%- set tether_stiffness = 0.05 -%} {#- Tether stiffness -#}
|
||||
{%- set joint_stiffness = 0.05 -%} {#- 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 -#}
|
||||
{%- set vehicle_spawn_point_y = 0.0 -%} {#- Y coordinate of vehicle spawn point -#}
|
||||
{%- set debug = 0 -%} {#- Used for debugging tether joints -#}
|
||||
{%- set damping = 0.0001 -%} {#- Model damping -#}
|
||||
{%- set friction = 0.0 -%} {#- Friction of the model relative to the world -#}
|
||||
{%- set tether_stiffness = 0.0005 -%} {#- Tether stiffness -#}
|
||||
{%- 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 -#}
|
||||
{%- set vehicle_spawn_point_y = 0.0 -%} {#- Y coordinate of vehicle spawn point -#}
|
||||
{%- set debug = 0 -%} {#- Used for debugging tether joints -#}
|
||||
|
||||
{#----------------------------------------------------------------------------------------------------------#}
|
||||
{#---------------------------------------------:Payload parameters:-----------------------------------------#}
|
||||
{#----------------------------------------------------------------------------------------------------------#}
|
||||
{%- set payload_m = 0.5 -%} {#- Payload mass (kg) -#}
|
||||
{%- set pr = 0.025 -%} {#- Payload radius (meters). Has to be less than FULL tether length -#}
|
||||
{%- 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>
|
||||
|
||||
|
|
|
|||
|
|
@ -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!"
|
||||
|
|
@ -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(current_state.mode == "OFFBOARD" && current_state.armed){
|
||||
ROS_INFO_ONCE("Spiri is taking off");
|
||||
if (waypoint_cl.call(wpoint_srv))
|
||||
{
|
||||
// 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 {
|
||||
|
|
|
|||
|
|
@ -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'):
|
||||
self.wpoints = rospy.get_param('sim/waypoints') # get waypoints
|
||||
if rospy.has_param('status/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
|
||||
|
|
|
|||
Loading…
Reference in New Issue