Scribd
Upload
49 views

Algorithm

This document describes an intelligence algorithm that takes in a sequence of points and yaw angles. It loops through the yaw angle sequence, calculates quaternions from each, and appends them to a quaternion sequence list. It then loops through the points, creates poses from each point and corresponding quaternion, and sends them to a move base client to execute motion plans to each location and orientation.

Uploaded by

Farhan Zamir
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
49 views

Algorithm

This document describes an intelligence algorithm that takes in a sequence of points and yaw angles. It loops through the yaw angle sequence, calculates quaternions from each, and appends them to a quaternion sequence list. It then loops through the points, creates poses from each point and corresponding quaternion, and sends them to a move base client to execute motion plans to each location and orientation.

Uploaded by

Farhan Zamir
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 1

1: function INTELLIGENCE_ALGORITHM(node, points_seq, p_seq, points, yaw_seq)

2: node ‘move_base_sequence’
3: points_seq get_param (‘move_base_sequence/p_seq’)
4: yaweulerangles_seq get_param (‘move_base_sequence/yaw_seq’)
5: quat_seq ϕ or { } (empty list)
6: goal_cnt ϕ
7: for yawangle in yaweulerangles_seq :
𝜋
8: x quaternion_from_euler (0, 0, yawangle + )
180
9: quat_seq.append ((quaternion)*(x))
10: end for
11: n 3
12: for point ϵ points do
13: x Point(*point)
14: y Pose (x, quat_seq (n-3))
15: n ++
16: client SimpleActionClient(‘move_base’ , MoveBaseAction)
17: wait client.wait_for_server
18: if not wait :
19: signal_shutdown( )
20: movebase_client( )
21: end if
22: end for
23: end function
24: return node

You might also like