From d4886ac57d18ca57de920cb04c55ef67c218e2a0 Mon Sep 17 00:00:00 2001 From: Siddharth Kannan Date: Thu, 1 Oct 2015 21:51:45 +0530 Subject: [PATCH 1/5] Remove IMU remap, instead publish from remap.py - The message type is converted from sensor_msgs/Imu to kraken_msgs/imuData. Signed-off-by: Siddharth Kannan --- .../thruster_remap_g500/launch/remap.launch | 4 +-- .../thruster_remap_g500/src/remap.py | 29 ++++++++++++++++--- 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/simulator_stack/thruster_remap_g500/launch/remap.launch b/simulator_stack/thruster_remap_g500/launch/remap.launch index 3bd519c..2aaa39d 100644 --- a/simulator_stack/thruster_remap_g500/launch/remap.launch +++ b/simulator_stack/thruster_remap_g500/launch/remap.launch @@ -7,11 +7,11 @@ - + - + diff --git a/simulator_stack/thruster_remap_g500/src/remap.py b/simulator_stack/thruster_remap_g500/src/remap.py index d7b1ab5..41de22a 100755 --- a/simulator_stack/thruster_remap_g500/src/remap.py +++ b/simulator_stack/thruster_remap_g500/src/remap.py @@ -7,15 +7,20 @@ from kraken_msgs.msg._forceData6Thruster import forceData6Thruster from kraken_msgs.msg._forceData4Thruster import forceData4Thruster -from std_msgs.msg import Float64MultiArray +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 +from tf.transformations import euler_from_quaternion +from sensor_msgs.msg import Imu + 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) +pose_pub= rospy.Publisher(topicHeader.SIMULATOR_POSE, gs.Pose, queue_size=10) rospy.wait_for_service('/dynamics/reset') reset=rospy.ServiceProxy('/dynamics/reset', Empty) @@ -44,6 +49,21 @@ def remapandpublish4(data): msg.data=new_thrusters pub.publish(msg) +def remapImuAndPublish(dataIn): + ''' + Converts the message from sensors_msgs/Imu to kraken_msgs/imuData + ''' + + orientation = dataIn.orientation + rpy = np.array(euler_from_quaternion([orientation.x, orientation.y, orientation.z, orientation.w])) * 180 / np.pi + + newImuData = imuData() + newImuData.data = [0] * 13 + newImuData.data[0] = rpy[0] + newImuData.data[1] = rpy[1] + newImuData.data[2] = rpy[2] + + imu_pub.publish(newImuData) def publish_pose(data): pose_pub.publish(data) @@ -51,6 +71,7 @@ def publish_pose(data): rospy.init_node('remapper', anonymous=False) +rospy.Subscriber('/g500/imu', Imu, remapImuAndPublish) rospy.Subscriber(topicHeader.SIMULATOR_MODEL_FORCE_DATA_6_THRUSTERS, forceData6Thruster, remapandpublish) rospy.Subscriber(topicHeader.SIMULATOR_MODEL_FORCE_DATA_4_THRUSTERS, forceData4Thruster, remapandpublish4) rospy.Subscriber(pose_topic,gs.Pose,publish_pose) From 7ca42a60fab8e08e3db46b634fb0942f86b667fb Mon Sep 17 00:00:00 2001 From: Siddharth Kannan Date: Thu, 1 Oct 2015 22:02:22 +0530 Subject: [PATCH 2/5] Add the remap code for linear acceleration. Signed-off-by: Siddharth Kannan --- simulator_stack/thruster_remap_g500/src/remap.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/simulator_stack/thruster_remap_g500/src/remap.py b/simulator_stack/thruster_remap_g500/src/remap.py index 41de22a..b06357d 100755 --- a/simulator_stack/thruster_remap_g500/src/remap.py +++ b/simulator_stack/thruster_remap_g500/src/remap.py @@ -54,15 +54,22 @@ 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 = imuData() - newImuData.data = [0] * 13 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 publish_pose(data): From 6ba276b63148942e0fafd5b607ec629c58180f89 Mon Sep 17 00:00:00 2001 From: Siddharth Kannan Date: Thu, 1 Oct 2015 22:17:03 +0530 Subject: [PATCH 3/5] Add code to convert DVL message to kraken_msgs. Signed-off-by: Siddharth Kannan --- .../thruster_remap_g500/launch/remap.launch | 2 -- .../thruster_remap_g500/src/remap.py | 19 ++++++++++++++++--- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/simulator_stack/thruster_remap_g500/launch/remap.launch b/simulator_stack/thruster_remap_g500/launch/remap.launch index 2aaa39d..0cb3c43 100644 --- a/simulator_stack/thruster_remap_g500/launch/remap.launch +++ b/simulator_stack/thruster_remap_g500/launch/remap.launch @@ -7,8 +7,6 @@ - - diff --git a/simulator_stack/thruster_remap_g500/src/remap.py b/simulator_stack/thruster_remap_g500/src/remap.py index b06357d..617470f 100755 --- a/simulator_stack/thruster_remap_g500/src/remap.py +++ b/simulator_stack/thruster_remap_g500/src/remap.py @@ -12,14 +12,16 @@ from std_srvs.srv import Empty import numpy as np -from kraken_msgs.msg import imuData +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) 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') @@ -72,6 +74,18 @@ def remapImuAndPublish(dataIn): 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[5] = dataIn.depth + + dvl_pub.publish(newDvlData) + def publish_pose(data): pose_pub.publish(data) @@ -79,10 +93,9 @@ def publish_pose(data): rospy.init_node('remapper', anonymous=False) rospy.Subscriber('/g500/imu', Imu, remapImuAndPublish) +rospy.Subscriber('/g500/dvl', DVL, remapDvlAndPublish) rospy.Subscriber(topicHeader.SIMULATOR_MODEL_FORCE_DATA_6_THRUSTERS, forceData6Thruster, remapandpublish) rospy.Subscriber(topicHeader.SIMULATOR_MODEL_FORCE_DATA_4_THRUSTERS, forceData4Thruster, remapandpublish4) rospy.Subscriber(pose_topic,gs.Pose,publish_pose) - - rospy.spin() From 7ccde4fc544f352f5c85c913755bebccc1b6dd83 Mon Sep 17 00:00:00 2001 From: Siddharth Kannan Date: Fri, 2 Oct 2015 01:33:53 +0530 Subject: [PATCH 4/5] Remap velocity variables per kraken_msgs dvlData Signed-off-by: Siddharth Kannan --- simulator_stack/thruster_remap_g500/src/remap.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/simulator_stack/thruster_remap_g500/src/remap.py b/simulator_stack/thruster_remap_g500/src/remap.py index 617470f..fd918da 100755 --- a/simulator_stack/thruster_remap_g500/src/remap.py +++ b/simulator_stack/thruster_remap_g500/src/remap.py @@ -82,7 +82,13 @@ def remapDvlAndPublish(dataIn): newDvlData = dvlData() newDvlData.data = [0] * 10 - newDvlData.data[5] = dataIn.depth + 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) From 4728f9ce67ae944664c562a6c2ad3a09f7320225 Mon Sep 17 00:00:00 2001 From: Siddharth Kannan Date: Fri, 2 Oct 2015 03:58:12 +0530 Subject: [PATCH 5/5] Fix remap to remove thruster_force_converter - Now, thruster_force_converter is not required anymore. - The data published by control.py Node is directly subscribed to, by the remap node and then, remap publishes that to g500 's thruster input topic. Signed-off-by: Siddharth Kannan --- sensor_stack/seabotix/src/control.py | 18 +++++++++++------- .../thruster_remap_g500/src/remap.py | 18 +++++++++++------- 2 files changed, 22 insertions(+), 14 deletions(-) diff --git a/sensor_stack/seabotix/src/control.py b/sensor_stack/seabotix/src/control.py index ed10555..0541e0a 100644 --- a/sensor_stack/seabotix/src/control.py +++ b/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() diff --git a/simulator_stack/thruster_remap_g500/src/remap.py b/simulator_stack/thruster_remap_g500/src/remap.py index fd918da..40f829b 100755 --- a/simulator_stack/thruster_remap_g500/src/remap.py +++ b/simulator_stack/thruster_remap_g500/src/remap.py @@ -7,6 +7,10 @@ from kraken_msgs.msg._forceData6Thruster import forceData6Thruster from kraken_msgs.msg._forceData4Thruster import forceData4Thruster +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 @@ -31,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) @@ -100,8 +104,8 @@ def publish_pose(data): rospy.Subscriber('/g500/imu', Imu, remapImuAndPublish) rospy.Subscriber('/g500/dvl', DVL, remapDvlAndPublish) -rospy.Subscriber(topicHeader.SIMULATOR_MODEL_FORCE_DATA_6_THRUSTERS, forceData6Thruster, remapandpublish) -rospy.Subscriber(topicHeader.SIMULATOR_MODEL_FORCE_DATA_4_THRUSTERS, forceData4Thruster, remapandpublish4) +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()