Add missing remap to make sure IMU and DVL data is extracted properly

sensor_stack/seabotix/src/control.py

@@ -5,13 +5,20 @@
 import roslib; roslib.load_manifest(PKG)
 import serial
 import rospy
+import sys
 from kraken_msgs.msg import thrusterData6Thruster
 from kraken_msgs.msg import thrusterData4Thruster
 from kraken_msgs.msg import imuData
+from kraken_msgs.msg import absoluteRPY
 from resources import topicHeader
 
 yaw = 0.0
-goal = 150.0
+
+if len(sys.argv) < 2:
+	print 'You must give your desired Yaw as the first argument.'
+	exit(0)
+
+goal = float(sys.argv[1])
 
 Kp_left = 1.27;
 Kd_left = 0.016;
@@ -33,7 +40,7 @@ def imuCB(dataIn):
 	global errorD
 	global prevError
 
-	yaw = dataIn.data[2]
+        yaw = dataIn.yaw
 
 	prevError = errorP
 	errorP = goal - yaw
@@ -48,9 +55,9 @@ def imuCB(dataIn):
 if __name__ == '__main__':
 	thruster4Data=thrusterData4Thruster();
 	thruster6Data=thrusterData6Thruster();
-	
+
 	rospy.init_node('Control', anonymous=True)
-	sub = rospy.Subscriber(topicHeader.SENSOR_IMU, imuData, imuCB)
+	sub = rospy.Subscriber(topicHeader.ABSOLUTE_RPY, absoluteRPY, imuCB)
 	pub4 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER4, thrusterData4Thruster, queue_size = 2)
 	pub6 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER6, thrusterData6Thruster, queue_size = 2)
 
@@ -73,8 +80,5 @@ def imuCB(dataIn):
 
 		#pub4.publish(thruster4Data)
 		pub6.publish(thruster6Data)
-
-
-		r.sleep()
 
 

simulator_stack/thruster_remap_g500/launch/remap.launch

@@ -7,11 +7,9 @@
 
 	<arg name="scene" default="cirs.xml" />
 	<node name="uwsim" pkg="uwsim" type="uwsim" args="--configfile $(arg scene)" output="screen" respawn="false" >
-		<remap from="/g500/imu" to="/kraken/sensor/imu"/>
-		<remap from="/g500/dvl" to="/kraken/sensor/dvl"/>
 	</node>
 	<arg name="thrusters_topic" default="/g500/thrusters_input" />
-	
+
 	<node name="keyboard_command" pkg="underwater_vehicle_dynamics" type="keyboard.py" args="$(arg thrusters_topic)" output="screen" respawn="false"/>
     <node name="remap" pkg="thruster_remap_g500" type="remap.py" />
 </launch>

simulator_stack/thruster_remap_g500/src/remap.py

@@ -7,15 +7,26 @@
 from kraken_msgs.msg._forceData6Thruster import forceData6Thruster
 from kraken_msgs.msg._forceData4Thruster import forceData4Thruster
 
-from std_msgs.msg import Float64MultiArray 
+from kraken_msgs.msg._thrusterData4Thruster import thrusterData4Thruster
+from kraken_msgs.msg._thrusterData6Thruster import thrusterData6Thruster
+
+
+from std_msgs.msg import Float64MultiArray
 from resources import topicHeader
 from std_srvs.srv import Empty
 
+import numpy as np
+from kraken_msgs.msg import imuData, dvlData
+from tf.transformations import euler_from_quaternion
+from sensor_msgs.msg import Imu
+from underwater_sensor_msgs.msg import DVL
+
 thrusters_topic="/g500/thrusters_input"
 pose_topic ='/g500/pose'
-pub = rospy.Publisher(thrusters_topic, Float64MultiArray,queue_size=10)
-pose_pub= rospy.Publisher(topicHeader.SIMULATOR_POSE, gs.Pose,queue_size=10)
-
+pub = rospy.Publisher(thrusters_topic, Float64MultiArray, queue_size=10)
+imu_pub= rospy.Publisher(topicHeader.SENSOR_IMU, imuData, queue_size=10)
+dvl_pub= rospy.Publisher(topicHeader.SENSOR_DVL, dvlData, queue_size=10)
+pose_pub= rospy.Publisher(topicHeader.SIMULATOR_POSE, gs.Pose, queue_size=10)
 
 rospy.wait_for_service('/dynamics/reset')
 reset=rospy.ServiceProxy('/dynamics/reset', Empty)
@@ -24,11 +35,11 @@ def remapandpublish(data):
 	new_thrusters=[0]*5
 
 	msg = Float64MultiArray()
-	new_thrusters[0]=data.data[1]
-	new_thrusters[1]=data.data[0]
-	new_thrusters[2]=-data.data[5]
-	new_thrusters[3]=-data.data[4]
-	new_thrusters[4]=-data.data[2]-data.data[3]
+	new_thrusters[0]=data.data[4]
+	new_thrusters[1]=data.data[5]
+	new_thrusters[2]=data.data[0]
+	new_thrusters[3]=data.data[1]
+	new_thrusters[4]=0 #-data.data[2]-data.data[3]
 
 	msg.data=new_thrusters
 	pub.publish(msg)
@@ -44,17 +55,57 @@ def remapandpublish4(data):
 	msg.data=new_thrusters
 	pub.publish(msg)
 
+def remapImuAndPublish(dataIn):
+	'''
+	Converts the message from sensors_msgs/Imu to kraken_msgs/imuData
+	'''
+
+	newImuData = imuData()
+	newImuData.data = [0] * 13
+
+	## Roll, pitch and yaw
+	orientation = dataIn.orientation
+	rpy = np.array(euler_from_quaternion([orientation.x, orientation.y, orientation.z, orientation.w])) * 180 / np.pi
+
+	newImuData.data[0] = rpy[0]
+	newImuData.data[1] = rpy[1]
+	newImuData.data[2] = rpy[2]
+
+	## ax, ay, az
+	newImuData.data[3] = dataIn.linear_acceleration.x
+	newImuData.data[4] = dataIn.linear_acceleration.y
+	newImuData.data[5] = dataIn.linear_acceleration.z
+
+	imu_pub.publish(newImuData)
+
+def remapDvlAndPublish(dataIn):
+	'''
+	Convert the message underwater_sensor_msgs/DVL to kraken_msgs/DvlData
+	'''
+
+	newDvlData = dvlData()
+	newDvlData.data = [0] * 10
+
+	newDvlData.data[7] = dataIn.depth
+
+	## vx, vy and vz
+
+	newDvlData.data[3] = dataIn.bi_x_axis
+	newDvlData.data[4] = dataIn.bi_y_axis
+	newDvlData.data[5] = dataIn.bi_z_axis
+
+	dvl_pub.publish(newDvlData)
 
 def publish_pose(data):
 	pose_pub.publish(data)
 
 
 rospy.init_node('remapper', anonymous=False)
 
-rospy.Subscriber(topicHeader.SIMULATOR_MODEL_FORCE_DATA_6_THRUSTERS, forceData6Thruster, remapandpublish)
-rospy.Subscriber(topicHeader.SIMULATOR_MODEL_FORCE_DATA_4_THRUSTERS, forceData4Thruster, remapandpublish4)
+rospy.Subscriber('/g500/imu', Imu, remapImuAndPublish)
+rospy.Subscriber('/g500/dvl', DVL, remapDvlAndPublish)
+rospy.Subscriber(topicHeader.CONTROL_PID_THRUSTER4, thrusterData4Thruster, remapandpublish4)
+rospy.Subscriber(topicHeader.CONTROL_PID_THRUSTER6, thrusterData6Thruster, remapandpublish)
 rospy.Subscriber(pose_topic,gs.Pose,publish_pose)
 
-
-
 rospy.spin()