def transform_fix(t1, t2, tn):
"""
t1,t2,tn
"""
#translation
dx = tn.translation.x - t2.translation.x
dy = tn.translation.y - t2.translation.y
#rotation
euler1 = tf.transformations.euler_from_quaternion(
[t1.rotation.x, t1.rotation.y, t1.rotation.z, t1.rotation.w])
euler2 = tf.transformations.euler_from_quaternion(
[t2.rotation.x, t2.rotation.y, t2.rotation.z, t2.rotation.w])
eulern = tf.transformations.euler_from_quaternion(
[tn.rotation.x, tn.rotation.y, tn.rotation.z, tn.rotation.w])
dyaw = euler2[2] - euler1[2]
ndx = dx * np.cos(dyaw) + dy * np.sin(dyaw)
ndy = -dx * np.sin(dyaw) + dy * np.cos(dyaw)
tout = Transform()
tout.translation.x = ndx + t1.translation.x
tout.translation.y = ndy + t1.translation.y
qout = tf.transformations.quaternion_from_euler(
0, 0, eulern[2] - euler2[2] + euler1[2])
tout.rotation = Quaternion(*qout)
return tout
def simulate_mbes(self, mbes_ac):
# Find particle's mbes pose without broadcasting/listening to tf transforms
particle_tf = Transform()
particle_tf.translation = self.pose.position
particle_tf.rotation = self.pose.orientation
mat_part = matrix_from_tf(particle_tf)
trans_mat = np.dot(mat_part, self.mbes_tf_mat)
trans = TransformStamped()
trans.transform.translation.x = translation_from_matrix(trans_mat)[0]
trans.transform.translation.y = translation_from_matrix(trans_mat)[1]
trans.transform.translation.z = translation_from_matrix(trans_mat)[2]
trans.transform.rotation = Quaternion(
*quaternion_from_matrix(trans_mat))
# Build MbesSimGoal to send to action server
mbes_goal = MbesSimGoal()
mbes_goal.mbes_pose.header.frame_id = self.map_frame
# mbes_goal.mbes_pose.child_frame_id = self.mbes_frame_id # The particles will be in a child frame to the map
mbes_goal.mbes_pose.header.stamp = rospy.Time.now()
mbes_goal.mbes_pose.transform = trans.transform
# Get result from action server
mbes_ac.send_goal(mbes_goal)
mbes_ac.wait_for_result()
mbes_res = mbes_ac.get_result()
# Pack result into PointCloud2
mbes_pcloud = PointCloud2()
mbes_pcloud = mbes_res.sim_mbes
mbes_pcloud.header.frame_id = self.map_frame
return mbes_pcloud
def poseToTransformMsg(x, y, phi=0.0):
'''Write pose (x, y, phi) to Transform message'''
msg = Transform()
msg.translation.x = x
msg.translation.y = y
msg.rotation = phiToQuaternionMsg(phi)
return msg
def transform_cloud_to_map(cloud):
rospy.loginfo("to map from " + cloud.header.frame_id)
transformation_store = TransformListener()
rospy.sleep(2)
t = transformation_store.getLatestCommonTime("map", cloud.header.frame_id)
tr_r = transformation_store.lookupTransform("map", cloud.header.frame_id,
t)
tr = Transform()
tr.translation = Vector3(tr_r[0][0], tr_r[0][1], tr_r[0][2])
tr.rotation = Quaternion(tr_r[1][0], tr_r[1][1], tr_r[1][2], tr_r[1][3])
tr_s = TransformStamped()
tr_s.header = std_msgs.msg.Header()
tr_s.header.stamp = rospy.Time.now()
tr_s.header.frame_id = "map"
tr_s.child_frame_id = cloud.header.frame_id
tr_s.transform = tr
t_kdl = transform_to_kdl(tr_s)
points_out = []
for p_in in pc2.read_points(cloud, field_names=["x", "y", "z", "rgb"]):
p_out = t_kdl * PyKDL.Vector(p_in[0], p_in[1], p_in[2])
points_out.append([p_out[0], p_out[1], p_out[2]])
cloud.header.frame_id = "map"
res = pc2.create_cloud(cloud.header, cloud.fields, points_out)
rospy.loginfo(cloud.header)
return res
def callback(msg):
print "got imu"
# Offset to put the cloud in a different place than the map:
x_offset = 120
y_offset = 0
p = PoseWithCovarianceStamped()
p.header = msg.header
p.header.frame_id = "map"
p.pose.pose.orientation = msg.orientation
p.pose.pose.position.x = x_offset
p.pose.pose.position.y = y_offset
pub.publish(p)
tfmsg = TFMessage()
transformS = TransformStamped()
transformS.header = msg.header
transform = Transform()
transform.rotation = msg.orientation
transform.translation.x = x_offset
transform.translation.y = y_offset
transformS.transform = transform
transformS.child_frame_id = "base"
tfmsg.transforms.append(transformS)
pub_tf.publish(tfmsg)
def pingCB(self, auv_ping, exp_ping, auv_pose):
try:
particle_tf = Transform()
particle_tf.translation = auv_pose.pose.pose.position
particle_tf.rotation = auv_pose.pose.pose.orientation
tf_mat = self.matrix_from_tf(particle_tf)
m2auv = np.matmul(self.m2o_mat,
np.matmul(tf_mat, self.base2mbes_mat))
auv_ping_ranges = self.ping2ranges(auv_ping)
exp_ping_ranges = self.pcloud2ranges(exp_ping, m2auv)
# print "------"
# print auv_ping_ranges
# print exp_ping_ranges
self.waterfall.append(
abs(auv_ping_ranges[:self.max_height] -
exp_ping_ranges[:self.max_height]))
if len(self.waterfall) > self.max_height:
self.waterfall.pop(0)
self.new_msg = True
except rospy.ROSInternalException:
pass
def publish_transform_stamped(model_name, x, pub):
ts = TransformStamped()
ts.child_frame_id = model_name
# Header
ts.header.stamp = rospy.Time.now()
ts.header.frame_id = "world"
# Translation
translation = Vector3()
translation.x = x[0]
translation.y = x[1]
translation.z = x[2]
# Rotation
quat = Quaternion()
quat.x = x[6]
quat.y = x[7]
quat.z = x[8]
quat.w = x[9]
# Message
transform = Transform()
transform.translation = translation
transform.rotation = quat
ts.transform = transform
# Publish a transform stamped message
pub.sendTransform(ts)
def inverse_transform(transform):
"""
Get the inverse of a tf transform
Get frame 2 -> frame 1 from frame 1 -> frame 2
:param transform: Transform from frame 1 -> frame 2
:type transform: geometry_msgs/Transform
:return: Transform from frame 2 -> frame 1
:rtype: geometry_msgs/Transform
"""
tmat = translation_matrix(
(transform.translation.x, transform.translation.y,
transform.translation.z))
qmat = quaternion_matrix((transform.rotation.x, transform.rotation.y,
transform.rotation.z, transform.rotation.w))
tf_mat = np.dot(tmat, qmat)
inv_tf_mat = inverse_matrix(tf_mat)
inv_transform = Transform()
inv_transform.translation.x = translation_from_matrix(inv_tf_mat)[0]
inv_transform.translation.y = translation_from_matrix(inv_tf_mat)[1]
inv_transform.translation.z = translation_from_matrix(inv_tf_mat)[2]
inv_transform.rotation = Quaternion(*quaternion_from_matrix(inv_tf_mat))
return inv_transform
def generate_trajectory(self):
traj_to_execute = MultiDOFJointTrajectory()
traj_header = std_msgs.msg.Header()
traj_velocities = Twist()
traj_accelerations = Twist()
traj_to_execute.joint_names = ["virtual_joint"]
for i in range(0, 5):
traj_header.stamp = self.get_clock().now().to_msg(
) #rclpy.time.Time().msg()
traj_to_execute.header = traj_header
#traj_quaternion = tf2.transformations.quaternion_from_euler(roll,pitch,yaw)
traj_quaternion = [0.0, 0.0, 0.0, 0.0]
traj_transforms = Transform()
traj_transforms.translation.x = 1.0 * i
traj_transforms.translation.y = 2.0 * i
traj_transforms.translation.z = 3.0 * i
traj_transforms.rotation = Quaternion()
traj_transforms.rotation.x = traj_quaternion[0]
traj_transforms.rotation.y = traj_quaternion[1]
traj_transforms.rotation.z = traj_quaternion[2]
traj_transforms.rotation.w = traj_quaternion[3]
point_i = MultiDOFJointTrajectoryPoint()
point_i.transforms.append(traj_transforms)
point_i.velocities.append(traj_velocities)
point_i.accelerations.append(traj_accelerations)
duration_i = Duration(seconds=1.0).to_msg()
point_i.time_from_start = duration_i # self.get_clock().now().to_msg()+Duration(seconds=1.0)
traj_to_execute.points.append(point_i)
return traj_to_execute
def marker_pose(self, state, marker):
"""
Get the pose of a marker based on the provided vehicle pose. Pose values
are in the same coordinate frame as the given state
NOTE: state.yaw must be in radians!
:param state: Vehicle state (pose & speed)
:type state: State() object
:param marker: Fiducial marker
:type marker: Marker() object
:return: Pose of marker
:rtype: geometry_msgs/Pose
:return: None (if marker is not detected)
"""
marker_trans = self.vehicle_to_marker_transform(marker)
if marker_trans is None:
return None
veh_trans = Transform()
veh_trans.translation.x = state.x
veh_trans.translation.y = state.y
veh_trans.translation.z = 0
veh_trans.rotation = Quaternion(
*quaternion_from_euler(0, 0, state.yaw))
pose_as_trans = transform_multiply(veh_trans, marker_trans)
marker_pose = Pose()
marker_pose.position.x = pose_as_trans.translation.x
marker_pose.position.y = pose_as_trans.translation.y
marker_pose.position.z = pose_as_trans.translation.z
marker_pose.orientation = pose_as_trans.rotation
return marker_pose
def calculate_transform(self, data):
"""
Takes a marker pose ROS message and returns a robot-base-frame-to-marker transform ROS message.
Parameters:
data - pose ROS message of marker relative to camera
Returns:
Transform ROS message of robot base frame to marker
"""
#calculate the transform
in_x = data.position.x
in_y = data.position.y
in_z = data.position.z
input_translation = [in_x, in_y, in_z]
multiplier = np.array([[ -0.02025737, -0.31392, 0.04627322],
[-0.38235706, 0.04113464, 0.03979437],
[-0.03673691, -0.27182984, -0.36413172 ]], dtype=np.float)
offset = np.array([0.45368236, -0.14424458, 0.8933589], dtype=np.float)
output_translation = np.matmul(multiplier, input_translation)+ offset
#build the transform
output_transform = Transform()
output_transform.translation.x = output_translation[0]
output_transform.translation.y = output_translation[1]
output_transform.translation.z = output_translation[2]
#TODO: Check that the rotation transform is correct.
output_transform.rotation = data.orientation
return output_transform
def transformFromPose(cls, p):
t = Transform()
t.translation.x = p.position.x
t.translation.y = p.position.y
t.translation.z = p.position.z
t.rotation = deepcopy(p.orientation)
return t
def update_kalman(ar_tags):
print "started update"
rospy.wait_for_service('innovation')
update = rospy.ServiceProxy('innovation', NuSrv)
listener = tf.TransformListener()
while True:
try:
try:
(trans, rot) = listener.lookupTransform(ar_tags['arZ'], ar_tags['ar1'], rospy.Time(0))
except:
print "Couldn't look up transform"
continue
lin = Vector3()
quat = Quaternion()
lin.x = trans[0]
lin.y = trans[1]
lin.z = trans[2]
quat.x = rot[0]
quat.y = rot[1]
quat.z = rot[2]
quat.w = rot[3]
transform = Transform()
transform.translation = lin
transform.rotation = quat
test = update(transform, ar_tags['ar1'])
print "Service call succeeded"
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def transformFromPose(cls, p):
t = Transform()
t.translation.x = p.position.x
t.translation.y = p.position.y
t.translation.z = p.position.z
t.rotation = deepcopy(p.orientation)
return t
def publish_transform_stamped_relative(model_name, parent_name, struct_x, struct_y, pub):
ts = TransformStamped()
ts.child_frame_id = model_name
# Header
ts.header.stamp = rospy.Time.now()
ts.header.frame_id = parent_name
# Translation
translation = Vector3()
translation.x = struct_x
translation.y = struct_y
translation.z = 0
# Rotation
quat = Quaternion()
quat.x = 0
quat.y = 0
quat.z = 0
quat.w = 1
# Message
transform = Transform()
transform.translation = translation
transform.rotation = quat
ts.transform = transform
# Publish a transform stamped message
pub.sendTransform(ts)
def get_object_tf_data(self, initial_pose):
# tf_time = rospy.Time(0)
trans = Transform()
trans.translation = copy.deepcopy(initial_pose.position)
trans.translation.z = 0.105
trans.rotation = copy.deepcopy(initial_pose.orientation)
return trans
def matrix_to_transform(H):
''' Convert a 4x4 homogeneous transform matrix to a ros Transform message '''
scale, shear, angles, trans, persp = transformations.decompose_matrix(H)
q = transformations.quaternion_from_euler(*angles)
transform_msg = Transform()
transform_msg.translation = Vector3(*trans)
transform_msg.rotation = Quaternion(*q)
return transform_msg
def convert_pose_to_tf(pose):
# convert a ros pose to a ros transform
transform = Transform()
transform.translation.x = pose.position.x
transform.translation.y = pose.position.y
transform.translation.z = pose.position.z
transform.rotation = pose.orientation
return transform
def list2Transform(xyzrpy):
transform = Transform()
# transform.header.frame_id = "iiwa_link_0"
transform.translation.x = xyzrpy[0]
transform.translation.y = xyzrpy[1]
transform.translation.z = xyzrpy[2]
transform.rotation = quaternion_from_euler(xyzrpy[3], xyzrpy[4], xyzrpy[5])
return transform
def pose_to_transform(self, pose):
"""
:param pose: Pose
:return: Transform
"""
t = Transform()
t.translation = pose.position
t.rotation = pose.orientation
return t
def map_transform(values):
"""
Map the values generated with the hypothesis-ros transform strategy to a rospy Transform type.
"""
if not isinstance(values, _Transform):
raise TypeError('Wrong type. Use appropriate hypothesis-ros type.')
ros_transform = Transform()
ros_transform.translation = map_vector3(values.translation)
ros_transform.rotation = map_quaternion(values.rotation)
return ros_transform
def tf_to_msg(transform):
"""
Convert a transform expressed as a 4x4 numpy array to geometry_msgs/Transform.
"""
translation = transform[0:3, 3]
rotation = transformations.quaternion_from_matrix(transform)
msg = Transform()
msg.translation = Vector3(*translation)
msg.rotation = Quaternion(*rotation)
return msg
def movePlayer(tf_broadcaster, player_name, transform_now, vel, angle,
max_vel):
"""
Moves a player given its currrent pose, a velocity, and angle, and a maximum velocity
:param tf_broadcaster: Used to publish the new pose of the player
:param player_name: string with the name of the player (must coincide with the name of the tf frame_id)
:param transform_now: a geometry_msgs.msg.Transform() containing the current pose. This variable is updated with
the new player pose
:param vel: velocity of displacement to take in x axis
:param angle: angle to turn, limited by max_angle (pi/30)
:param max_vel: maximum velocity or displacement based on the selected animal
"""
max_angle = math.pi / 30
if angle > max_angle:
angle = max_angle
elif angle < -max_angle:
angle = -max_angle
if vel > max_vel:
vel = max_vel
T1 = transform_now
T2 = Transform()
T2.rotation = tf.transformations.quaternion_from_euler(0, 0, angle)
T2.translation.x = vel
matrix_trans = tf.transformations.translation_matrix(
(T2.translation.x, T2.translation.y, T2.translation.z))
matrix_rot = tf.transformations.quaternion_matrix(
(T2.rotation[0], T2.rotation[1], T2.rotation[2], T2.rotation[3]))
matrixT2 = np.matmul(matrix_trans, matrix_rot)
matrix_trans = tf.transformations.translation_matrix(
(T1.translation.x, T1.translation.y, T1.translation.z))
matrix_rot = tf.transformations.quaternion_matrix(
(T1.rotation.x, T1.rotation.y, T1.rotation.z, T1.rotation.w))
matrixT1 = np.matmul(matrix_trans, matrix_rot)
matrix_new_transform = np.matmul(matrixT1, matrixT2)
quat = tf.transformations.quaternion_from_matrix(matrix_new_transform)
trans = tf.transformations.translation_from_matrix(matrix_new_transform)
T1.rotation = Quaternion(quat[0], quat[1], quat[2], quat[3])
T1.translation.x = trans[0]
T1.translation.y = trans[1]
T1.translation.z = trans[2]
tf_broadcaster.sendTransform(trans, quat, rospy.Time.now(), player_name,
"world")
def initializeBackgroundSubtractor(robot_list, gazebo_set_pose_client):
"""!
Create the background subtractor. Then, manipulate the Gazebo environment to allow
the capture of a background image. This uses the MOG2 algorithm.
@param robot_list A list of the robots that shouldn't be part of the background.
@param gazebo_set_pose_client A server client used to move every robot within Gazebo.
@return The background subtractor used in the script.
"""
# Set a large history to maximize the detection of differences from background.
background_history = ParameterLookup.lookupWithDefault(
parameter='~background_subtractor/history_length', default=100)
# Explore how shadow detection impacts the results.
var_threshold = ParameterLookup.lookupWithDefault(
parameter='~background_subtractor/var_threshold', default=500)
detect_shadows = ParameterLookup.lookupWithDefault(
parameter='~background_subtractor/detect_shadows', default=False)
background_subtractor = cv2.createBackgroundSubtractorMOG2(
history=background_history, varThreshold=var_threshold, detectShadows=detect_shadows)
# Create a client to determine where each robot is located within the Gazebo environment.
# This is only done at initialization to move every robot out of the way. It is assumed the
# robots start in a stable location. So the system is safe to just move them up in the air,
# but leave their X/Y the same. Given Gazebo's propensity to send colliding objects flying,
# this seems like the best course of action. The user is unlikely to start the node if
# the simulation freaks out right away. Using the bag file first location is possible, but
# would require passing the tf_buffer into this function. After initialization, the bag file
# positions are used for the rest of the node.
gazebo_get_pose_name = '/gazebo/get_model_state'
rospy.loginfo('Waiting to find robot poses...')
rospy.wait_for_service(gazebo_get_pose_name)
gazebo_get_pose_client = rospy.ServiceProxy(
name=gazebo_get_pose_name, service_class=GetModelState)
# Move all the robots way up in the sky, presumably outside of the view of the camera.
rospy.loginfo('Moving robots and capturing background...')
for robot in robot_list:
# Create each message via the robot object.
robot_current_state = gazebo_get_pose_client(robot.getName(), '')
robot_transform_msg = Transform()
robot_transform_msg.translation.x = robot_current_state.pose.position.x
robot_transform_msg.translation.y = robot_current_state.pose.position.y
robot_transform_msg.translation.z = robot_current_state.pose.position.z
robot_transform_msg.rotation = robot_current_state.pose.orientation
robot.recordTransform(robot_transform_msg)
robot_new_pose_request = robot.createSetModelStateRequest()
robot_new_pose_request.pose.position.z = 1e6
moveRobot(gazebo_set_pose_client, robot_new_pose_request)
# Capture N images and apply to subtractor
rospy.sleep(2.0)
for _ in range(background_history):
# Sleep long enough for any noise to update
(image, _) = captureImage(sleep_duration=0.25)
background_subtractor.apply(image)
return background_subtractor
def tf_mat2msg(self, transform_mat):
""" Convert 4x4 transform numpy matrix to a Transform message type.
param transform_mat : 4x4 numpy Matrix transformation
"""
quat = tf.transformations.quaternion_from_matrix(transform_mat)
trans = tf.transformations.translation_from_matrix(transform_mat)
result = Transform()
result.translation = Vector3(trans[0], trans[1], trans[2])
result.rotation = Quaternion(quat[0], quat[1], quat[2], quat[3])
return result
def pose_to_matrix(pose): # type: (Union[Pose, PoseStamped]) -> np.ndarray
"""
Converts a Pose to the corresponding homogenous transformation matrix.
"""
if isinstance(pose, PoseStamped):
pose = pose.pose
transform = Transform()
transform.translation.x = pose.position.x
transform.translation.y = pose.position.y
transform.translation.z = pose.position.z
transform.rotation = pose.orientation
return transform_to_matrix(transform)
def carla_transform_to_ros_transform(carla_transform):
"""
Convert carla transform to a ROS transform
Considers the conversion from the left-handed system (unreal) to the right-handed system (ROS)
See carla_location_to_ros_vector3() and carla_rotation_to_ros_quaternion() for details
:param carla_transform: The Carla Transform
:type carla_transform: carla.Transform
:return: a ROS transform
:rtype: geometry_msgs.msg.Transform
"""
ros_transform = Transform()
ros_transform.translation = carla_location_to_ros_vector3(carla_transform.location)
ros_transform.rotation = carla_rotation_to_ros_quaternion(carla_transform.rotation)
return ros_transform
def cogOdomCallback(self, msg):
t = Transform()
t.translation.x = msg.pose.pose.position.x
t.translation.y = msg.pose.pose.position.y
t.translation.z = msg.pose.pose.position.z
t.rotation = msg.pose.pose.orientation
t_np = ros_numpy.numpify(t)
t_np = tft.inverse_matrix(t_np)
ts = TransformStamped()
ts.header.stamp = msg.header.stamp
ts.header.frame_id = 'cog'
ts.child_frame_id = 'world'
ts.transform = ros_numpy.msgify(Transform, t_np)
self.br.sendTransform(ts)
def object_to_map(self, pose, object_type, time):
obj_in_map = self.tf_listener.transformPose("/map", pose)
obj_in_map.header.frame_id = object_type + "_in_map"
transform = Transform()
transform.translation = obj_in_map.pose.position
transform.rotation = obj_in_map.pose.orientation
# Insert new Transform into a TransformStamped object and add to the tf tree
tf_to_map = TransformStamped()
tf_to_map.child_frame_id = object_type + "_in_map"
tf_to_map.header.frame_id = "/map"
tf_to_map.transform = transform
tf_to_map.header.stamp = time
#rospy.loginfo(new_tfstamped)
# add to tf list
self.or_map_pub.publish(obj_in_map)
self.tf_message = tfMessage(transforms=[tf_to_map])
self.tf_publisher.publish(self.tf_message)
def carla_transform_to_ros_transform(carla_transform):
"""
Convert a carla transform to a ROS transform
See carla_location_to_ros_vector3() and carla_rotation_to_ros_quaternion() for details
:param carla_transform: the carla transform
:type carla_transform: carla.Transform
:return: a ROS transform
:rtype: geometry_msgs.msg.Transform
"""
ros_transform = Transform()
ros_transform.translation = carla_location_to_ros_vector3(
carla_transform.location)
ros_transform.rotation = carla_rotation_to_ros_quaternion(
carla_transform.rotation)
return ros_transform
def transform_cloud_to_map(self,cloud):
rospy.loginfo("VIEW EVAL: to map from " + cloud.header.frame_id)
if("temporal" in cloud.header.frame_id):
rospy.loginfo("un-breaking this")
cloud.header.frame_id = "head_xtion_depth_optical_frame"
t = self.transformation_store.getLatestCommonTime("map", cloud.header.frame_id)
tr_r = self.transformation_store.lookupTransform("map", cloud.header.frame_id, t)
tr = Transform()
tr.translation = Vector3(tr_r[0][0],tr_r[0][1],tr_r[0][2])
tr.rotation = Quaternion(tr_r[1][0],tr_r[1][1],tr_r[1][2],tr_r[1][3])
tr_s = TransformStamped()
tr_s.header = std_msgs.msg.Header()
tr_s.header.stamp = rospy.Time.now()
tr_s.header.frame_id = "map"
tr_s.child_frame_id = cloud.header.frame_id
tr_s.transform = tr
t_kdl = self.transform_to_kdl(tr_s)
points_out = []
rospy.loginfo("CLOUD FIELDS:")
rospy.loginfo(cloud.fields)
filtered_fields = []
# christ, why. this is a hack to fix something with the temporal smoothed pc
for k in cloud.fields:
if(k.offset == 0):
filtered_fields.append(k)
if(k.offset == 4):
filtered_fields.append(k)
if(k.offset == 8):
filtered_fields.append(k)
if(k.offset == 7):
filtered_fields.append(k)
for p_in in pc2.read_points(cloud,field_names=["x","y","z","rgb"]):
p_out = t_kdl * PyKDL.Vector(p_in[0], p_in[1], p_in[2])
points_out.append([p_out[0],p_out[1],p_out[2]])
cloud.header.frame_id = "map"
res = pc2.create_cloud(cloud.header, filtered_fields, points_out)
rospy.loginfo(cloud.header)
return res
def update(self, meas_mbes, odom):
# Compute AUV MBES ping ranges
particle_tf = Transform()
particle_tf.translation = odom.pose.pose.position
particle_tf.rotation = odom.pose.pose.orientation
tf_mat = matrix_from_tf(particle_tf)
m2auv = np.matmul(self.m2o_mat, np.matmul(tf_mat, self.base2mbes_mat))
# this will not work in simulation ***
mbes_meas_ranges = self.ping2ranges(meas_mbes)
self.pf_mbes_pub.publish(meas_mbes)
# Measurement update of each particle
for i in range(0, self.pc, 8):
for j in range(i, i + 8):
if j < self.pc:
mbes_goal = self.particles[j].get_mbes_goal()
#print(mbes_goal)
self.ac_mbes[j].send_goal(mbes_goal)
else:
break
for j in range(i, i + 8):
if j < self.pc:
if self.ac_mbes[j].wait_for_result(rospy.Duration(0.03)):
mbes_res = self.ac_mbes[j].get_result()
got_result = True
else:
mbes_res = None
got_result = False
self.particles[j].meas_update(mbes_res, mbes_meas_ranges,
got_result)
else:
break
weights = []
for i in range(self.pc):
weights.append(self.particles[i].w)
self.miss_meas = weights.count(1.e-50)
weights_array = np.asarray(weights)
# Add small non-zero value to avoid hitting zero
weights_array += 1.e-30
return weights_array
def multi_dof_joint_state(cls, msg, obj):
obj.header = decode.header(msg, obj.header, "")
obj.joint_names = msg["joint_names"]
for i in msg['_length_trans']:
trans = Transform()
trans.translation = decode.translation(msg, trans.translation, i)
trans.rotation = decode.rotation(msg, trans.rotation, i)
obj.transforms.append(trans)
for i in msg['_length_twist']:
tw = Twist()
tw.linear = decode.linear(msg, tw.linear, i)
tw.angular = decode.angular(msg, tw.angular, i)
obj.twist.append(twist)
for i in msg['_length_twist']:
wr = Wrench()
wr.force = decode.force(msg, wr.force, i)
wr.torque = decode.torque(msg, wr.torque, i)
obj.wrench.append(wr)
return(obj)
def update(self, meas_mbes, odom):
# Compute AUV MBES ping ranges
particle_tf = Transform()
particle_tf.translation = odom.pose.pose.position
particle_tf.rotation = odom.pose.pose.orientation
tf_mat = matrix_from_tf(particle_tf)
m2auv = np.matmul(self.m2o_mat, np.matmul(tf_mat, self.base2mbes_mat))
mbes_meas_ranges = pcloud2ranges(meas_mbes, m2auv)
# Measurement update of each particle
weights = []
for i in range(self.pc):
self.particles[i].meas_update(mbes_meas_ranges)
weights.append(self.particles[i].w)
weights_array = np.asarray(weights)
# Add small non-zero value to avoid hitting zero
weights_array += 1.e-30
return weights_array
def read_files(self):
#initialize the reader
j=0
tmp_transform=Transform()
self.camera_marker_file=open(self.name_cam_marker_file,'r')
#read camera marker pose
for i in range(self.num_of_lines_of_cam_marker_file):
line=self.camera_marker_file.readline()
if i==j*3+1:
b=list()
k=5
while k<len(line):
if line[k]==" " or line[k]=="\n":
b.append(k)
k+=1
tmp_transform.translation.x=float(line[5:b[0]])
tmp_transform.translation.y=float(line[(b[0]+1):b[1]])
tmp_transform.translation.z=float(line[(b[1]+1):b[2]])
if i==j*3+2:
b=list()
k=5
while k<len(line):
if line[k]==" " or line[k]=="\n":
b.append(k)
k+=1
tmp_transform.rotation.x=float(line[5:b[0]])
tmp_transform.rotation.y=float(line[(b[0]+1):b[1]])
tmp_transform.rotation.z=float(line[(b[1]+1):b[2]])
tmp_transform.rotation.w=float(line[(b[2]+1):b[3]])
self.camera_marker_samples.transforms.append(copy.deepcopy(tmp_transform))
j+=1
# for tr in self.camera_marker_samples.transforms:
# print tr
# print "\n"
self.base_ee_file=open(self.name_base_ee_file,'r')
for i in range(self.num_of_lines_of_base_ee_file):
line=self.base_ee_file.readline()
k=0
b=list()
transform=Transform()
while k<len(line):
if line[k]==" " or line[k]=="\n":
b.append(k)
k+=1
#get translation
transform.translation.x=float(line[0:b[0]])
transform.translation.y=float(line[(b[1]+1):b[2]])
transform.translation.z=float(line[(b[3]+1):b[4]])
#get rotation
r=(float(line[(b[5]+1):b[6]])/180)*math.pi
p=(float(line[(b[7]+1):b[8]])/180)*math.pi
y=(float(line[(b[9]+1):b[10]])/180)*math.pi
quat=transformations.quaternion_from_euler(r, p, y, 'sxyz')
transform.rotation=quat
self.base_ee_samples.transforms.append(transform)
#print self.base_ee_samples
self.ready=True
def toTransformMsg(tq):
t = TransformMSG()
t.rotation = toRotMsg(tq)
t.translation = toTranslationMsg(tq)
return t
rospy.init_node('virtual_drone', anonymous=True)
sub_ref_vel = rospy.Subscriber('cmd_vel', Twist, refVelCallback)
sub_ref_traj = rospy.Subscriber('/cmd_traj', JointTrajectory, trajCallback)
#sub_ref_pos = rospy.Subscriber('cmd_tf', TransformStamped, reftfCallback)
#pub_ref_tf = tf.TransformBroadcaster()
pub_joint_states = rospy.Publisher('joint_states', JointState)
# use drone name parameter as prefix to joint names
if rospy.has_param('drone_name'):
drone_name = rospy.get_param('drone_name')
my_joint_names=[drone_name + "_" + joint_name for joint_name in my_joint_names]
js_msg.name=my_joint_names
# and for reference position
ref_transform = Transform()
# default altitude
ref_transform.translation.z = 1.2
# default orientation
ref_transform.rotation = quaternion_from_euler(0.0, 0.0, 0.0)
# and the reference velocity
ref_velocity = Twist()
# update rate and time step
updateRateHz = 10
delta_t = 1./updateRateHz
rate = rospy.Rate(updateRateHz)
while not rospy.is_shutdown():
refUpdate()
rate.sleep()
