diff --git a/ros/src/f16/launch/rocket.launch b/ros/src/f16/launch/rocket.launch
index a04f37fc..0763169c 100644
--- a/ros/src/f16/launch/rocket.launch
+++ b/ros/src/f16/launch/rocket.launch
@@ -26,4 +26,6 @@
+
+
diff --git a/ros/src/f16/models/rocket/meshes/rocket.scad b/ros/src/f16/models/rocket/meshes/rocket.scad
index 74160783..d5f3c06b 100644
--- a/ros/src/f16/models/rocket/meshes/rocket.scad
+++ b/ros/src/f16/models/rocket/meshes/rocket.scad
@@ -15,9 +15,9 @@ module body() {
module fin() {
color([0.3, 0.3, 0.3]) rotate([90, 0, 0]) linear_extrude(fh, center=true) polygon([
- [0, fl],
- [fl, 0],
- [0, 0]
+ [r+0.01, fl],
+ [r+0.01 + fl, 0],
+ [r+0.01, 0]
]);
}
diff --git a/ros/src/f16/models/rocket/meshes/stl/fin.stl b/ros/src/f16/models/rocket/meshes/stl/fin.stl
index 09f3ffb0..702ac99b 100644
Binary files a/ros/src/f16/models/rocket/meshes/stl/fin.stl and b/ros/src/f16/models/rocket/meshes/stl/fin.stl differ
diff --git a/ros/src/f16/models/rocket/rocket.sdf b/ros/src/f16/models/rocket/rocket.sdf
index 5e08be28..d85cf956 100644
--- a/ros/src/f16/models/rocket/rocket.sdf
+++ b/ros/src/f16/models/rocket/rocket.sdf
@@ -106,6 +106,13 @@
+
+
+
+ model://rocket/meshes/stl/fin.stl
+
+
+
@@ -115,30 +122,30 @@
1 0 0
- 0
- 0
+ -1
+ 1
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+ 0.0
+ 4.752798721
+ 0.6417112299
+ 0.0
+ 0.3391428111
+ -3.85
+ -0.9233984055
+ 0
+ 0 0 0
+ 0.1
+ 1.2041
+ 0 0 1
+ 0 1 0
+ body
+ fin1
+ -4.0
+
0 0 0 0 0 1.57
@@ -164,6 +171,14 @@
+
+
+
+ model://rocket/meshes/stl/fin.stl
+
+
+
+
@@ -173,30 +188,30 @@
1 0 0
- 0
- 0
+ -1
+ 1
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+ 0.0
+ 4.752798721
+ 0.6417112299
+ 0.0
+ 0.3391428111
+ -3.85
+ -0.9233984055
+ 0
+ 0 0 0
+ 0.1
+ 1.2041
+ 0 0 1
+ 1 0 0
+ body
+ fin2
+ -4.0
+
0 0 0 0 0 3.14
@@ -222,6 +237,14 @@
+
+
+
+ model://rocket/meshes/stl/fin.stl
+
+
+
+
@@ -231,30 +254,30 @@
1 0 0
- 0
- 0
+ -1
+ 1
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+ 0.0
+ 4.752798721
+ 0.6417112299
+ 0.0
+ 0.3391428111
+ -3.85
+ -0.9233984055
+ 0
+ 0 0 0
+ 0.1
+ 1.2041
+ 0 0 1
+ 0 1 0
+ body
+ fin3
+ -4.0
+
0 0 0 0 0 -1.57
@@ -280,6 +303,14 @@
+
+
+
+ model://rocket/meshes/stl/fin.stl
+
+
+
+
@@ -289,8 +320,8 @@
1 0 0
- 0
- 0
+ -1
+ 1
@@ -317,7 +348,7 @@
0 0 0 0 0 0
- 0.8
+ 0.00001
0
@@ -345,6 +376,45 @@
rocket_motor
+
+ body
+ rocket/ground_truth
+ world
+ 100
+
+
+
+ true
+ 100
+ 100.0
+ fin1
+ rocket/fin1_torque
+
+
+
+ true
+ 100
+ 100.0
+ fin2
+ rocket/fin2_torque
+
+
+
+ true
+ 100
+ 100.0
+ fin3
+ rocket/fin3_torque
+
+
+
+ true
+ 100
+ 100.0
+ fin4
+ rocket/fin4_torque
+
+
diff --git a/ros/src/f16/scripts/rocket_controller.py b/ros/src/f16/scripts/rocket_controller.py
index 2a920b35..aa42f318 100755
--- a/ros/src/f16/scripts/rocket_controller.py
+++ b/ros/src/f16/scripts/rocket_controller.py
@@ -15,10 +15,14 @@ class RocketController:
def __init__(self):
rospy.init_node('rocket_controller')
self.time_last_plan = rospy.Time.now()
- self.pub_rear_left = rospy.Publisher('/rocket/force_rear_left', Wrench, queue_size=10)
- self.pub_rear_right = rospy.Publisher('/rocket/force_rear_right', Wrench, queue_size=10)
- self.pub_front_left = rospy.Publisher('/rocket/force_front_left', Wrench, queue_size=10)
- self.pub_front_right = rospy.Publisher('/rocket/force_front_right', Wrench, queue_size=10)
+
+ # fin torques
+ self.pub_fin1 = rospy.Publisher('/rocket/fin1_torque', Wrench, queue_size=10)
+ self.pub_fin2 = rospy.Publisher('/rocket/fin2_torque', Wrench, queue_size=10)
+ self.pub_fin3 = rospy.Publisher('/rocket/fin3_torque', Wrench, queue_size=10)
+ self.pub_fin4 = rospy.Publisher('/rocket/fin4_torque', Wrench, queue_size=10)
+
+ # where rocket is at
self.sub_ground_truth = rospy.Subscriber('/rocket/ground_truth', Odometry,
self.control_callback)
@@ -30,6 +34,7 @@ def control_callback(self, odom):
orientation = odom.pose.pose.orientation
euler = tf.transformations.euler_from_quaternion(
(orientation.x, orientation.y, orientation.z, orientation.w))
+ pitch = euler[1]
yaw = euler[2]
# rotation rate
@@ -46,67 +51,17 @@ def control_callback(self, odom):
"base_link",
"map")
- #rospy.loginfo('position %f %f %f, m', position.x, position.y, position.z)
- #rospy.loginfo('yaw %f, deg', np.rad2deg(yaw))
- #rospy.loginfo('yaw rate %f, deg/s', np.rad2deg(yaw_rate))
-
- drive_torque = 0.1
- desired_yaw = np.deg2rad(90)
-
- # steering PD controller
- steer_kP = 1
- steer_kD = 1
- yaw_error = desired_yaw - yaw
- #rospy.loginfo("yaw error %f deg", np.rad2deg(yaw_error))
- yaw_rate_error = 0 - yaw_rate
- steer_torque = steer_kP*yaw_error + steer_kD*yaw_rate_error
-
- # speed P controller
- speed_kP = 1
- speed = np.sqrt(vel.x**2 + vel.y**2)
- desired_speed = 0.1
- speed_error = desired_speed - speed
- drive_torque = speed_kP*speed_error
- #rospy.loginfo("speed error %f m/s", speed_error)
-
- # saturate torques
- if np.abs(steer_torque) > 0.1:
- steer_torque /= np.abs(steer_torque)
- if np.abs(drive_torque) > 0.1:
- drive_torque /= np.abs(drive_torque)
-
- # rear drive
+ # fin1
msg = Wrench()
msg.force.x = 0
msg.force.y = 0
msg.force.z = 0
- msg.torque.x = -drive_torque
+ msg.torque.x = 10
msg.torque.y = 0
msg.torque.z = 0
- self.pub_rear_left.publish(msg)
- self.pub_rear_right.publish(msg)
-
- # front left steering
- msg = Wrench()
- msg.force.x = 0
- msg.force.y = 0
- msg.force.z = 0
- msg.torque.x = 0
- msg.torque.y = 0
- msg.torque.z = steer_torque
- self.pub_front_left.publish(msg)
-
- # front right steering
- msg = Wrench()
- msg.force.x = 0
- msg.force.y = 0
- msg.force.z = 0
- msg.torque.x = 0
- msg.torque.y = 0
- msg.torque.z = steer_torque
- self.pub_front_right.publish(msg)
+ self.pub_fin1.publish(msg)
if __name__ == "__main__":
- CarController()
+ RocketController()
rospy.spin()