class PickupPalletServer:
def __init__(self):
self.wheel_cmd_publisher = rospy.Publisher(WHEEL_CMD_TOPIC,
Twist,
queue_size=1)
self.lift_cmd_publisher = rospy.Publisher(LIFT_CONTROL_TOPIC,
Float64,
queue_size=1)
self.tf_listener = tf.TransformListener()
rospy.wait_for_service('gazebo/get_model_state')
rospy.wait_for_service('gazebo/get_link_state')
self.modelStateService = rospy.ServiceProxy('gazebo/get_model_state',
GetModelState)
self.linkStateService = rospy.ServiceProxy('gazebo/get_link_state',
GetLinkState)
self.server = actionlib.SimpleActionServer(ACTION_SERVER_NAME,
PickupPalletAction,
self.execute, False)
self.server.start()
print('Server started')
def execute(self, goal):
rate = rospy.Rate(PUBLISH_RATE)
self.targetPallet = goal.palletName.data
self.done = False
self.state = CONTROL_STATE_ALIGN
self.start_time = rospy.get_time()
self.lookupTransfrom()
self.findTarget()
while not rospy.is_shutdown() and not self.done:
if self.server.is_preempt_requested():
rospy.loginfo('Pickup Canceled')
self.server.set_preempted()
break
self.lookupTransfrom()
if self.state == CONTROL_STATE_ALIGN:
self.control_alignment()
elif self.state == CONTROL_STATE_LIFT:
self.control_lift()
elif self.state == CONTROL_STATE_DONE:
self.server.set_succeeded()
break
rate.sleep()
def findTarget(self):
v_heading = np.array(
[math.cos(self.robot_theta),
math.sin(self.robot_theta)])
v = np.array(
[math.cos(self.pallet_theta),
math.sin(self.pallet_theta)])
if (np.dot(v, v_heading) > 0):
self.target_position = self.pallet_position - (v * ALIGN_DISTANCE)
self.target_theta = self.pallet_theta
self.target_line = Line(self.target_position,
self.target_position - v)
else:
self.target_position = self.pallet_position + (v * ALIGN_DISTANCE)
self.target_theta = (self.pallet_theta + math.pi)
self.target_line = Line(self.target_position,
self.target_position + v)
def lookupTransfrom(self):
try:
(trans,
rot) = self.tf_listener.lookupTransform(MAP_FRAME, ROBOT_FRAME,
rospy.Time(0))
euler = tf.transformations.euler_from_quaternion(
[rot[0], rot[1], rot[2], rot[3]])
self.robot_position = np.array([trans[0], trans[1]])
self.robot_theta = euler[2]
modelState = self.modelStateService(model_name=self.targetPallet)
modelPos = modelState.pose.position
self.pallet_position = np.array([modelPos.x, modelPos.y])
q = modelState.pose.orientation
euler = tf.transformations.euler_from_quaternion(
[q.x, q.y, q.z, q.w])
self.pallet_theta = euler[2]
except Exception as e:
rospy.logwarn('Failed to lookup transform')
print(e)
return 0
def control_alignment(self):
v_pallet = self.pallet_position - self.robot_position
v_target = self.target_position - self.robot_position
v_heading = np.array(
[math.cos(self.robot_theta),
math.sin(self.robot_theta)])
distance = np.linalg.norm(v_pallet)
if distance < ALIGN_DISTANCE:
#self.server.set_succeeded()
rospy.loginfo('Aligned')
self.state = CONTROL_STATE_LIFT
return
theta_target = math.atan2(v_target[1], v_target[0])
lineDot = np.dot(self.target_line.normal(), v_pallet)
lineDistance = self.target_line.distance(self.robot_position)
lineErr = np.sign(lineDot) * lineDistance
alignErr = self.target_theta - self.robot_theta
if (alignErr > math.pi):
alignErr -= 2 * math.pi
elif (alignErr < -math.pi):
alignErr += 2 * math.pi
headingErr = theta_target - self.robot_theta
if (headingErr > math.pi):
headingErr -= 2 * math.pi
elif (headingErr < -math.pi):
headingErr += 2 * math.pi
if lineDistance > 0.5:
steering = (alignErr * 0.2) + (headingErr * 1.5) + (lineErr * 1.0)
elif distance > 2.4:
steering = (alignErr * 2.0) + (headingErr * 4.0) + (lineErr * 0.5)
else:
steering = (alignErr * 3.0) + (headingErr * 0.0) + (lineErr * 1.0)
target_turn = max(min(steering, MAX_TURN), -MAX_TURN)
rospy.loginfo(
'HErr: %.2f, AErr: %.2f, PDist: %.2f, Steering: %.2f, Dist: %.2f',
headingErr, alignErr, lineErr, steering, distance)
target_speed = 0.2
angular = (target_speed / LENGTH) * math.tan(target_turn)
cmd = Twist()
cmd.linear = Vector3(target_speed, 0, 0)
cmd.angular = Vector3(0, 0, angular)
self.wheel_cmd_publisher.publish(cmd)
def control_lift(self):
lift_link = self.linkStateService(link_name=LIFT_LINK_NAME)
height = lift_link.link_state.pose.position.z
if height < 0.5:
self.lift_cmd_publisher.publish(0.5)
else:
self.lift_cmd_publisher.publish(0)
rospy.loginfo('Lift is up')
self.state = CONTROL_STATE_DONE
class FollowPathServer:
def __init__(self):
self.wheel_cmd_publisher = rospy.Publisher(WHEEL_CONTROL_TOPIC,
Twist,
queue_size=1)
self.tf_listener = tf.TransformListener()
self.pub_marker = rospy.Publisher('/path/marker',
Marker,
queue_size=10)
self.server = actionlib.SimpleActionServer(ACTION_SERVER_NAME,
FollowPathAction,
self.execute, False)
self.server.start()
print('Server started')
def execute(self, goal):
self.done = False
self.path = goal.poses
self.pathIndex = 0
self.updatePose()
self.nextPathIndex()
self.publishPathMarker(self.path, 99)
rate = rospy.Rate(PUBLISH_RATE)
while not rospy.is_shutdown() and not self.done:
if self.server.is_preempt_requested():
rospy.loginfo('Path Canceled')
self.server.set_preempted()
break
self.updatePose()
pos = np.array([self.pose[0], self.pose[1]])
v1 = self.pathLine.p2 - pos
pathDot = np.dot(self.pathLine.normal(), v1)
endDot = np.dot(v1, self.pathLine.direction())
endDistance = self.endLine.distance(pos)
pathDistance = self.pathLine.distance(pos)
pathErr = (-np.sign(pathDot)) * self.direction * pathDistance
if (pathDistance > MAX_PATH_ERROR):
self.server.set_aborted()
rospy.loginfo('Too far off path')
break
if (endDot < 0):
## Aim for the next point on the path
self.nextPathIndex()
continue
delta = self.getHeadingError()
steering = delta * 3.0 + pathErr * 2.5
target_turn = max(min(steering, MAX_TURN), -MAX_TURN)
target_speed = 0.5 * self.direction
angular = (target_speed / LENGTH) * math.tan(target_turn)
cmd = Twist()
cmd.linear = Vector3(target_speed, 0, 0)
cmd.angular = Vector3(0, 0, angular)
self.wheel_cmd_publisher.publish(cmd)
rate.sleep()
self.clearPathMarker(99)
def updatePose(self):
try:
(trans,
rot) = self.tf_listener.lookupTransform('/map', '/base_link',
rospy.Time(0))
euler = tf.transformations.euler_from_quaternion(
[rot[0], rot[1], rot[2], rot[3]])
self.pose = [trans[0], trans[1], euler[2]]
except:
rospy.logwarn('Failed to lookup transform')
def nextPathIndex(self):
self.pathIndex += 1
if (self.pathIndex >= len(self.path)):
## End of the path
position = Point(self.pose[0], self.pose[1], 0)
q = tf.transformations.quaternion_from_euler(0, 0, self.pose[2])
orientation = Quaternion(q[0], q[1], q[2], q[3])
finalPose = Pose(position, orientation)
self.server.set_succeeded(FollowPathResult(finalPose))
cmd = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0)) ## Stop
self.wheel_cmd_publisher.publish(cmd)
self.done = True
rospy.loginfo('Success')
return
targetPose = self.path[self.pathIndex]
prevPose = self.path[self.pathIndex - 1]
p1 = np.array([prevPose.position.x, prevPose.position.y])
p2 = np.array([targetPose.position.x, targetPose.position.y])
self.pathLine = Line(p1, p2)
self.endLine = Line(p2, p2 + self.pathLine.normal())
if (self.pathLine.length() < EPSILON):
self.nextPathIndex()
return
headingErr = self.getHeadingError()
if (abs(headingErr) > math.pi / 2):
self.direction = -1
else:
self.direction = 1
def getHeadingError(self):
targetHeading = self.pathLine.heading()
currentHeading = self.pose[2]
delta = targetHeading - currentHeading
if (delta > math.pi):
delta -= 2 * math.pi
elif (delta < -math.pi):
delta += 2 * math.pi
return delta
def publishPathMarker(self, path, id):
m = Marker()
m.header.frame_id = "map"
m.type = Marker.LINE_STRIP
m.action = Marker.ADD
m.points = []
for pose in path:
p = Point(pose.position.x, pose.position.y, 0.1)
m.points.append(p)
m.scale = Vector3(0.1, 0.1, 0.1)
m.color = ColorRGBA(0, 1, 0, 1)
m.id = id
self.pub_marker.publish(m)
def clearPathMarker(self, id):
m = Marker()
m.action = Marker.DELETE
m.id = id
self.pub_marker.publish(m)
