def trigger(self): sample = self.gyro.get_sample() gs = Gyro() gs.header.frame_id = self.frame_id gs.header.stamp = rospy.Time.now() gs.calibration_offset.x = 0.0 gs.calibration_offset.y = 0.0 gs.calibration_offset.z = self.offset gs.angular_velocity.x = 0.0 gs.angular_velocity.y = 0.0 gs.angular_velocity.z = (sample - self.offset) * math.pi / 180.0 gs.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1] self.pub.publish(gs) imu = Imu() imu.header.frame_id = self.frame_id imu.header.stamp = rospy.Time.now() imu.angular_velocity.x = 0.0 imu.angular_velocity.y = 0.0 imu.angular_velocity.z = (sample - self.offset) * math.pi / 180.0 imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1] imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1] self.orientation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec() self.prev_time = imu.header.stamp (imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion() self.pub2.publish(imu)
def get_initial_position(self, robot): if robot.simulation: robot_to_gob = (0.04, 0.08) else: robot_to_gob = robot.actuators.PUMPS.get(self.pump_id).get("pos") gob_to_robot = TransformStamped() gob_to_robot.transform.translation.x = -robot_to_gob[0] gob_to_robot.transform.translation.y = -robot_to_gob[1] goal_pose = TransformStamped() goal_pose.transform.translation.x = self.position[0] goal_pose.transform.translation.y = self.position[1] robot_pose = PoseStamped() robot_pose.pose.position.x = robot.get_position()[0] robot_pose.pose.position.y = robot.get_position()[1] angle = self.get_initial_orientation(robot) rot = Rotation.RotZ(angle) q = rot.GetQuaternion() goal_pose.transform.rotation.x = q[0] goal_pose.transform.rotation.y = q[1] goal_pose.transform.rotation.z = q[2] goal_pose.transform.rotation.w = q[3] gob_to_robot_kdl = transform_to_kdl(gob_to_robot) robot_goal_pose_kdl = do_transform_frame(gob_to_robot_kdl, goal_pose) return (robot_goal_pose_kdl.p.x(), robot_goal_pose_kdl.p.y())
def navigate_to_point(position, orientation, z=0.0): navigator = Navigator() goal_pose = PoseStamped() goal_pose.header.frame_id = "map" goal_pose.pose.position.x = position[0] goal_pose.pose.position.y = position[1] goal_pose.pose.position.z = z q = Rotation.RotZ(orientation).GetQuaternion() goal_pose.pose.orientation.x = q[0] goal_pose.pose.orientation.y = q[1] goal_pose.pose.orientation.z = q[2] goal_pose.pose.orientation.w = q[3] navigator.goToPose(goal_pose) while not navigator.isNavComplete(): pass
def spin(self): self.prev_time = rospy.Time.now() while not rospy.is_shutdown(): if self.calibrating: self.calibrate() self.calibrating = False self.prev_time = rospy.Time.now() # prepare Imu frame imu = Imu() imu.header.frame_id = self.frame_id # get line from device str = self.ser.readline() # timestamp imu.header.stamp = rospy.Time.now() nums = str.split() # check, if it was correct line if (len(nums) != 5): continue gyro = int(nums[2]) ref = int(nums[3]) temp = int(nums[4]) val = (ref - gyro - self.bias) * 1000 / 3 / 1024 * self.scale imu.angular_velocity.x = 0 imu.angular_velocity.y = 0 imu.angular_velocity.z = val * math.pi / 180 imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1] imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1] self.orientation += imu.angular_velocity.z * ( imu.header.stamp - self.prev_time).to_sec() self.prev_time = imu.header.stamp (imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ( self.orientation).GetQuaternion() self.pub.publish(imu)
def _getOrientation(self, data): q = Quaternion() if data: global hydro if hydro: if 'theta' in data: # Assume Yaw Orientation orientation = Rotation.RotZ(data.get('theta')) elif any(k in ['roll', 'pitch', 'yaw'] for k in data.iterkeys()): # Assume RPY Orientation orientation = Rotation.RPY(data.get('roll', 0), data.get('pitch', 0), data.get('yaw', 0)) elif any(k in ['w', 'x', 'y', 'z'] for k in data.iterkeys()): orientation = Rotation.Quaternion(data.get('x', 0), data.get('y', 0), data.get('z', 0), data.get('w', 0)) else: orientation = Rotation() orientation = orientation.GetQuaternion() else: if 'theta' in data: # Assume Yaw Orientation orientation = quaternion_from_euler( 0, 0, data.get('theta')) elif any(k in ['roll', 'pitch', 'yaw'] for k in data.iterkeys()): # Assume RPY Orientation orientation = quaternion_from_euler( data.get('roll', 0), data.get('pitch', 0), data.get('yaw', 0)) elif any(k in ['w', 'x', 'y', 'z'] for k in data.iterkeys()): orientation = (data.get('x', 0), data.get('y', 0), data.get('z', 0), data.get('w', 0)) else: orientation = (0, 0, 0, 0) q.x, q.y, q.z, q.w = orientation return q
def set_goal_pose(self, z=0.0): """Set goal pose of action in blackboard.""" msg = NavigateToPose_Goal() msg.pose.header.frame_id = "map" orientation = actions.get( self.current_action).get_initial_orientation(self) position = actions.get(self.current_action).get_initial_position(self) self.get_logger().info(f"Goal objective updated to:") self.get_logger().info(f"Position x: {position[0]}, y: {position[1]}") self.get_logger().info(f"Orientation {orientation} radians") msg.pose.pose.position.x = position[0] msg.pose.pose.position.y = position[1] msg.pose.pose.position.z = z rot = Rotation.RotZ(orientation) q = rot.GetQuaternion() msg.pose.pose.orientation.x = q[0] msg.pose.pose.orientation.y = q[1] msg.pose.pose.orientation.z = q[2] msg.pose.pose.orientation.w = q[3] self.blackboard.goal = msg
def rot_z(self, v): self.rot_frame = Frame(Rotation.RotZ(v)) * self.rot_frame