def __init__(self, config, callback, last_pose):
"""
Instantiate a Linear move command object.
:param config: (dict) Configuration dictionary, with parameters:
* "start_position": (list|np.ndarray) (Optional)[Default: last_pose.position)
Starting position [x, y] of the robot.
* "end_position": (list|np.ndarray) Ending position [x, y] of the robot.
* "steps": (int) Number of desired steps/poses for the movement
* "deterministic": (bool) (Optional)[Default: None] Overrides general deterministic
configuration for this move. If None, then general config applies.
* "scans": (int) (Optional)[Default: None] Overrides general number of scans per move
configuration for this move. If None, then general config applies.
:param callback: (callable) Function, lambda or other callable object to be executed
when calling the command object.
:param last_pose: (Pose) Last pose of the robot before this command.
"""
if 'start_position' in config:
start = to_np(config['start_position'])
else:
start = last_pose.position
end = to_np(config['end_position'])
diff = end - start
theta = np.arctan2(diff[1], diff[0])
if theta > np.pi:
theta -= 2 * np.pi
if theta < -np.pi:
theta += 2 * np.pi
config['end_orientation'] = theta
if 'start_position' in config or 'start_orientation' in config or abs(
theta - last_pose.orientation) > 1e-6:
config['start_orientation'] = theta
super(MoveLinearCommand, self).__init__(config, callback, last_pose)
def set_center(self, position, compute_poses=True):
"""
Sets the position of the circular trajectory's center.
:param position: (list|np.ndarray) Center position [x, y] of the circular arch to follow.
:param compute_poses: (bool)[Default: True] Recompute the robot pose list if True.
:return: (None)
"""
self._center = to_np(position)
if compute_poses:
self.compute_poses()
def set_end_position(self, position, compute_poses=True):
"""
Sets the target position of the robot.
:param position: (list|np.ndarray) Target position [x, y] of the robot.
:param compute_poses: (bool)[Default: True] Recompute the robot pose list if True.
:return: (None)
"""
self._end_position = to_np(position)
if compute_poses:
self.compute_poses()
def __init__(self, pose, orientation):
self.position = to_np(pose)
self.orientation = normalize_angles(to_np(orientation))