# AWS Robot Delivery Challenge

## 構成
ゴール座標指定をよくあるgoal_serverノードに任せて、移動は素直にmove_baseを使った構成となっております。

## やったこと
やったことは以下の3つです。

* 地図画像の最適化
* コストマップの最適化
* goal_serverの実装

## 地図画像の最適化
地図画像はSLAMにより生成したものを加工して通りにくい場所を通りやすくしています。

## コストマップの最適化
`/delivery-challenge-sample/robot_ws/src/turtlebot3/turtlebot3_navigation/param/costmap_common_params_burger.yaml` にある`obstacke_range`と`raytrace_range`を変更しました。以下はファイル全文です。

obstacle_range: 0.12
raytrace_range: 4.0
​
footprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]]
#robot_radius: 0.105
​
inflation_radius: 1.0
cost_scaling_factor: 3.0
​
map_type: costmap
observation_sources: scan
scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}

## goal_server
「指定の座標に到着したら次のゴール座標を/move_base/goalにPublishする」という仕組みです。

以下プログラム全文です。なおway pointの座標は最速タイムのものとは異なります。

#!/usr/bin/env python
​
import rospy
import actionlib
import tf
from nav_msgs.msg import Odometry
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseActionFeedback
​
current_goal_index = 0
​
y = (lambda x: 0.12180259886142 * x)
​
way_points_to_1st_goal = []
​
first_goal = 4.10499954224
​
separation_number = 4
​
for i in range(1, separation_number):
    x = first_goal * i / separation_number
    way_point = [(x, y(x), 0.0), (0.0,0.0,0.0453144882804,0.99897277097)]
    way_points_to_1st_goal.append(way_point)
​
goal_poses = [
    [(4.10499954224,0.49999961257,0.0),(0.0,0.0,0.0453144882804,0.998972770976)],
    [(4.63499975204,1.03999948502,0.0),(0.0,0.0,0.382683629035,0.923879451048)],
    [(5.37549893188,1.3889994278,0.0),(0.0,0.0,0.297310573387,0.954780824563)]
    ]
​
way_points_to_1st_goal.extend(goal_poses)
goal_poses = way_points_to_1st_goal
​
​
goal_poses = [
    [(4.10499954224,0.49999961257,0.0),(0.0,0.0,0.0453144882804,0.998972770976)],
    [(4.68499975204,1.03999948502,0.0),(0.0,0.0,0.382683629035,0.923879451048)],
    [(5.47549893188,1.4589994278,0.0),(0.0,0.0,0.297310573387,0.954780824563)]
    ]
​
#goal_poses = [
#    [(4.10499954224,0.49999961257,0.0),(0.0,0.0,0.0453144882804,0.998972770976)],
#    [(4.63499975204,1.03999948502,0.0),(0.0,0.0,0.382683629035,0.923879451048)],
#    [(5.55499887466,1.51999926567,0.0),(0.0,0.0,0.129932778825,0.991522805077)]
#    ]
​
​
# goal_poses = [
#     [(3.24499964714,0.439999610186,0.0),(0.0,0.0,0.0185090009621,0.999828693769)],
#     [(4.14499950409,0.499999493361,0.0),(0.0,0.0,0.0288101410471,0.999584901733)],
#     [(4.59499931335,0.969999432564,0.0),(0.0,0.0,0.921499970975,0.388378428203)],
#     [(5.52499961853,1.48999929428,0.0),(0.0,0.0,0.997297208357,0.0734729760043)]
#     ]
​
# goal_poses = [
#     [(3.47999954224,0.489999890327,0.0),(0.0,0.0,-0.607654547589,1.0)],
#     [(4.50999927521,0.799999594688,0.0),(0.0,0.0,0.49713191502,0.867674973172)],
#     [(5.36999893188,1.3499994278,0.0),(0.0,0.0,0.297310573387,0.954780824563)]
#     ]
​
def goal_pose(pose):
    goal_pose = MoveBaseGoal()
    goal_pose.target_pose.header.frame_id = 'map'
    goal_pose.target_pose.pose.position.x = pose[0][0]
    goal_pose.target_pose.pose.position.y = pose[0][1]
    goal_pose.target_pose.pose.position.z = pose[0][2]
    goal_pose.target_pose.pose.orientation.x = pose[1][0]
    goal_pose.target_pose.pose.orientation.y = pose[1][1]
    goal_pose.target_pose.pose.orientation.z = pose[1][2]
    goal_pose.target_pose.pose.orientation.w = pose[1][3]
​
    return goal_pose
​
def send_goal(pose):
    listener = tf.TransformListener()
​
    client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
    client.wait_for_server()
    listener.waitForTransform("map", "base_link", rospy.Time(), rospy.Duration(4.0))
​
    goal = goal_pose(pose)
    client.send_goal(goal)
​
def feedback_callback(data):
    global current_goal_index
    print(data.feedback.base_position.pose)
    x = data.feedback.base_position.pose.position.x
    y = data.feedback.base_position.pose.position.y
    if (current_goal_index < len(goal_poses) - 1 and abs(goal_poses[current_goal_index][0][0] - x) < 0.50 and abs(goal_poses[current_goal_index][0][1] - y) < 0.50):
        current_goal_index += 1
        send_goal(goal_poses[current_goal_index])
​
if __name__ == '__main__':
    rospy.init_node('goal_server')
​
    send_goal(goal_poses[current_goal_index])
​
    rospy.Subscriber("move_base/feedback", MoveBaseActionFeedback, feedback_callback)
    rospy.spin()

## キャプチャ動画
実際に最速タイムが出たときのキャプチャ動画です。キャプチャ動画はほぼ毎回撮っていました。

<iframe width="560" height="315" src="https://www.youtube.com/embed/m3Br_3q9aRg" frameborder="0" allow="accelerometer; autoplay; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>