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
