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 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()