def path_follow_cb(ud):
ret = 'continue'
if ud.current_index_in < (ud.max_index-1):
if ud.boom_state_array[ud.current_index_in] == "up":
ud.direction_out = move_tool_simpleGoal.UP
else:
ud.direction_out = move_tool_simpleGoal.DOWN
wp = follow_pathGoal()
wp.path.poses.append(ud.path.poses[ud.current_index_in])
wp.path.poses.append(ud.path.poses[ud.current_index_in + 1])
ud.current_wp_out = wp.path
if ud.current_index_in == 0:
ret = 'update'
elif ud.boom_state_array[ud.current_index_in] != ud.boom_state_array[ud.current_index_in-1]:
ret = 'update'
ud.current_index_out = ud.current_index_in + 1
return ret
else:
return 'done'
def generate_local_path():
p = follow_pathGoal()
ps = PoseStamped()
ps.pose.position.x = 5
ps.pose.position.y = 1
ps.pose.orientation.x = 1
ps.header.frame_id="base_footprint"
ps.header.stamp = rospy.Time.now()
p.path.poses.append(ps)
ps = PoseStamped()
ps.pose.position.x = 10
ps.pose.position.y = 10
ps.pose.orientation.x = 1
ps.header.frame_id="base_footprint"
ps.header.stamp = rospy.Time.now()
p.path.poses.append(ps)
ps = PoseStamped()
ps.pose.position.x = 30
ps.pose.position.y = 10
ps.pose.orientation.x = 1
ps.header.frame_id="base_footprint"
ps.header.stamp = rospy.Time.now()
p.path.poses.append(ps)
ps = PoseStamped()
ps.pose.position.x = 30
ps.pose.position.y = 30
ps.pose.orientation.x = 1
ps.header.frame_id="base_footprint"
ps.header.stamp = rospy.Time.now()
p.path.poses.append(ps)
return p
