scorpio1
/
oscillation_ctrl
forked from cesar.alejandro/oscillation_ctrl
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Weekly Improvements

This commit is contained in:
cesar 2022-04-01 11:03:22 -03:00
parent 527f5cb9e6
commit 792b791ef8
6 changed files with 44 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.gitignore vendored
View File

@ -1,3 +1,6 @@
launch/debug.launch
src/MoCap_*.py
launch/cortex_bridge.launch
src/MoCap_Localization_*.py
src/MocapBridge.py
src/*_client.py
*.rviz

2
config/px4_pluginlists.yaml
View File

@ -2,9 +2,7 @@ plugin_blacklist:
# common
- safety_area
# extras
- tdr_radio
- image_pub
- vibration
- distance_sensor
- rangefinder
- wheel_odometry

11
launch/cortex_bridge.launch
View File

@ -9,8 +9,8 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
<arg name="fcu_protocol" default="v2.0" />
<arg name="respawn_mavros" default="false" />
<arg name="gazebo_gui" default="false" />
<!--arg name="fcu_url" default="udp://:14551@192.168.1.91:14556"/>
<arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" /-->
<arg name="fcu_url" default="udp://:14549@192.168.1.91:14554"/>
<arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" />
<node
pkg="oscillation_ctrl"
@ -26,7 +26,10 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
<include file="$(find mavros)/launch/px4.launch">
<arg name="pluginlists_yaml" value="$(find oscillation_ctrl)/config/px4_pluginlists.yaml" />
<arg name="config_yaml" value="$(find oscillation_ctrl)/config/px4_config.yaml" />
<arg name="fcu_url" value="udp://:14549@192.168.1.91:14554" />
<arg name="gcs_url" value="udp-b://127.0.0.1:14555@14550" />
<arg name="fcu_protocol" value="$(arg fcu_protocol)" />
<!--arg name="fcu_url" value="udp://:14549@192.168.1.91:14554" />
<arg name="gcs_url" value="udp-b://127.0.0.1:14555@14550" /-->
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
</include>
</launch>

6
launch/px4.launch
View File

@ -1,7 +1,7 @@
<launch>
<!-- vim: set ft=xml noet : -->
<!-- example launch script for PX4 based FCU's -->
<arg name="fcu_url" default="udp://:14551@192.168.1.91:14556"/>
<arg name="fcu_url" default="udp://:14549@192.168.1.91:14554"/>
<arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
@ -10,8 +10,8 @@
<arg name="respawn_mavros" default="false" />
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />
<arg name="pluginlists_yaml" value="$(find oscillation_ctrl)/config/px4_pluginlists.yaml" />
<arg name="config_yaml" value="$(find oscillation_ctrl)/config/px4_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />

19
src/MoCap_Localization_noTether.py
View File

@ -17,7 +17,7 @@ class Main:
def __init__(self):
# rate(s)
rate = 100 # rate for the publisher method, specified in Hz -- 20 Hz
rate = 120 # rate for the publisher method, specified in Hz -- 20 Hz
# Variables needed for testing start
self.tstart = rospy.get_time() # Keep track of the start time
@ -99,12 +99,25 @@ class Main:
self.drone_pose.pose.orientation.z,
self.drone_pose.pose.orientation.w])
self.q = quaternion_from_euler(self.eul[0],self.eul[1],self.eul[2])
offset_yaw = math.pi/2
q_offset = quaternion_from_euler(0,0,-offset_yaw)
self.q = quaternion_multiply(self.q,q_offset)
self.eul = euler_from_quaternion([self.q[0],self.q[1],self.q[2],self.q[3]])
self.drone_pose.pose.orientation.x = self.q[0]
self.drone_pose.pose.orientation.y = self.q[1]
self.drone_pose.pose.orientation.z = self.q[2]
self.drone_pose.pose.orientation.w = self.q[3]
def FRD_Transform(self):
'''
Transforms mocap reading to proper coordinate frame
'''
self.drone_pose = self.buff_pose
# self.drone_pose = self.buff_pose
self.drone_pose.header.frame_id = "/map"
# self.drone_pose.pose.position.x = 0
@ -139,7 +152,7 @@ class Main:
def bodyPose_cb(self,msg):
try:
self.buff_pose = msg
self.drone_pose = msg
except ValueError:
pass

15
src/MocapBridge.py
View File

@ -19,7 +19,7 @@ class Main:
def __init__(self):
# rate(s)
rate = 50 # rate for the publisher method, specified in Hz -- 20 Hz
rate = 30 # rate for the publisher method, specified in Hz -- 20 Hz
# Variables needed for testing start
self.tstart = rospy.get_time() # Keep track of the start time
@ -135,6 +135,18 @@ class Main:
pose.orientation.z,
pose.orientation.w])
# self.q = quaternion_from_euler(self.eul[0],self.eul[1],self.eul[2])
# offset_yaw = math.pi/2
# q_offset = quaternion_from_euler(0,0,offset_yaw)
# self.q = quaternion_multiply(self.q,q_offset)
# self.eul = euler_from_quaternion([self.q[0],self.q[1],self.q[2],self.q[3]])
# self.LabDrone_pose.pose.orientation.x = self.q[0]
# self.LabDrone_pose.pose.orientation.y = self.q[1]
# self.LabDrone_pose.pose.orientation.z = self.q[2]
def FRD_Transform(self):
'''
Transforms mocap reading to proper coordinate frame
@ -172,6 +184,7 @@ class Main:
def fake_mocap(self):
self.LabDrone_pose.header.stamp = rospy.Time.now()
#self.LabDrone_pose.header.stamp.secs = self.LabDrone_pose.header.stamp.secs - self.tstart
self.LabDrone_pose.pose.position.x = 0.0
self.LabDrone_pose.pose.position.y = 0.0
self.LabDrone_pose.pose.position.z = 0.5