#!/usr/bin/env python3
import rospy
import actionlib
from unsafe_traversal.msg import MoveToGoalAction, MoveToGoalGoal

# initialise node
rospy.init_node('test_move_action', anonymous=True)

# setup action client
move_to_goal_client = actionlib.SimpleActionClient(
    '/unsafe_traversal/move_to_goal', MoveToGoalAction)
move_to_goal_client.wait_for_server()

start = (1.3, 1.05)
end = (4.3, 1.3)

goal = MoveToGoalGoal()

# start pose
goal.start_pose.header.stamp = rospy.get_rostime()
goal.start_pose.header.frame_id = 'map'

goal.start_pose.pose.position.x = start[0]
goal.start_pose.pose.position.y = start[1]
goal.start_pose.pose.position.z = 0

# end pose
goal.end_pose.header.stamp = rospy.get_rostime()
goal.end_pose.header.frame_id = 'map'

goal.end_pose.pose.position.x = end[0]
goal.end_pose.pose.position.y = end[1]
goal.end_pose.pose.position.z = 0

# send goal
move_to_goal_client.send_goal(goal)
move_to_goal_client.wait_for_result()
