def drive_to(self, place, claw_offset=0, **kwargs):
'''Drive directly to a particular point in space. The point must be in
the odometry reference frame.
Arguments:
* `place`: (`geometry_msgs.msg.Point` or `geometry_msgs.msg.Pose2D`): The place to drive.
Keyword Arguments/Returns/Raises:
* See `mobility.swarmie.Swarmie.drive`
* claw_offset to the odometry reference frame. Appropriate value
to be passed in, otherwise the reference frame remains unchanged.
'''
loc = self.get_odom_location().get_pose()
dist = math.hypot(loc.y - place.y, loc.x - place.x)
angle = angles.shortest_angular_distance(
loc.theta, math.atan2(place.y - loc.y, place.x - loc.x))
req = MoveRequest(
theta=angle,
r=dist - claw_offset,
)
return self.__drive(req, **kwargs)
def drive_to(self, place, claw_offset=0, **kwargs):
'''Drive directly to a particular point in space. The point must be in
the odometry reference frame.
Arguments:
* `place`: (`geometry_msgs.msg.Point` or `geometry_msgs.msg.Pose2D`): The place to drive.
Keyword Arguments/Returns/Raises:
* See `mobility.swarmie.Swarmie.drive`
* claw_offset to the odometry reference frame. Appropriate value
to be passed in, otherwise the reference frame remains unchanged.
'''
loc = self.get_odom_location().get_pose()
dist = math.hypot(loc.y - place.y, loc.x - place.x)
angle = angles.shortest_angular_distance(
loc.theta, math.atan2(place.y - loc.y, place.x - loc.x))
effective_dist = dist - claw_offset
self.turn(angle, **kwargs)
if effective_dist < 0:
# The driver API skips the turn state if the request distance is
# negative. This ensures the rover will perform the turn before
# backing up slightly in this case.
return self.drive(effective_dist, **kwargs)
req = MoveRequest(
theta=angle,
r=effective_dist,
)
return self.__drive(req, **kwargs)
def turn(self, theta, **kwargs):
'''Turn theta degrees
Args:
* `theta` (float) Radians to turn
Keyword Arguments/Returns/Raises:
* See `mobility.swarmie.Swarmie.drive`
'''
req = MoveRequest(theta=theta, )
return self.__drive(req, **kwargs)
def wait(self, time, **kwargs):
'''Wait for a period of time. This can be used to check for obstacles
Args:
* `time` (`float`) seconds to wait.
Keyword Arguments/Returns/Raises:
* See `mobility.swarmie.Swarmie.drive`
'''
req = MoveRequest(timer=time, )
return self.__drive(req, **kwargs)
def drive(self, distance, **kwargs):
'''Drive the specfied distance
Args:
* `distance` (`float`) Meters to drive.
Keyword arguments:
* `ignore` (`int`) - Bitmask with Obstacle messages that will be ignored while driving.
* `throw` (`bool`) - Raise a DriveException if an obstacle is encountered (default True).
* `timeout` (`int`) - The command will fail after this number of seconds (defulat: 120)
Returns:
* If `throw=False` was given returns a `mobility_msgs.msg.MoveResult` containing an integer. \
Values are described in `src/mobility/msg/MoveResult.msg`
* If `throw=True` (the default) is given the return value will be converted into an exception. \
The following exceptions are defined:
* `mobility.swarmie.DriveException` - Base class for driving exceptions.
* `mobility.swarmie.VisionException` - Base class for exceptions caused by seeing a tag
* `mobility.swarmie.TagException` - Exception caused when the target tag (0) is seen.
* `mobility.swarmie.HomeException` - Exception caused when the home tag (256) is seen.
* `mobility.swarmie.ObstacleException` - Exception caused when sonar senses the rover \
is close to an obstacle.
* `mobility.swarmie.PathException` - Exception caused when the rover encounters an \
error while driving. If the rover detects a large angle between itself and its goal this \
happens. It's usually because the rover has driven over some kind of obstacle.
* `mobility.swarmie.AbortException` - Exception caused when the user places the rover into \
manual mode while it was driving.
* `mobility.swarmie.TimeoutException` - Exception caused when the drive command fails to \
complete in the amount of time specified with the `timeout=` argument.
'''
req = MoveRequest(r=distance, )
return self.__drive(req, **kwargs)
def timed_drive(self, time, linear, angular, **kwargs):
'''Send the specified velocity command for a given period of time.
Args:
* time (float) The duration of the timed command.
* linear (float) The linear velocity of the rover (m/s)
* angular (float) The angular velocity of the rover (radians/s)
SEE: Keyword Arguments for drive commands
Keyword Arguments/Returns/Raises:
* See `mobility.swarmie.Swarmie.drive`
'''
req = MoveRequest(
timer=time,
linear=linear,
angular=angular,
)
return self.__drive(req, **kwargs)
def drive(self, distance, **kwargs):
'''Drive the specfied distance
Args:
* `distance` (`float`) Meters to drive.
Keyword arguments:
* `ignore` (`int`) - Bitmask with Obstacle messages that will be ignored while driving.
* `throw` (`bool`) - Raise a DriveException if an obstacle is encountered (default True).
* `timeout` (`int`) - The command will fail after this number of seconds (defulat: 120)
* `linear` (`float`) - The linear velocity used to execute the command (default is set with DynParam)
* `angular` (`float`) - The angular velocity used to execute the command (default is set with DynParam)
Returns:
* If `throw=False` was given returns a `mobility_msgs.msg.MoveResult` containing an integer. \
Values are described in `src/mobility/msg/MoveResult.msg`. Note: if the rover is placed in \
manual mode mid-drive, a `mobility`swarmie.AbortException` will be raised regardless of the \
`throw` keyword argument's value.
* If `throw=True` (the default) is given the return value will be converted into an exception. \
The following exceptions are defined:
* `mobility.swarmie.DriveException` - Base class for driving exceptions.
* `mobility.swarmie.VisionException` - Base class for exceptions caused by seeing a tag
* `mobility.swarmie.TagException` - Exception caused when the target tag (0) is seen.
TODO: add AprilTagBoundaryException
* `mobility.swarmie.AprilTagBoundaryException` - Exception caused when boundary tag (1) is seen.
* `mobility.swarmie.HomeException` - Exception caused when the home tag (256) is seen.
* `mobility.swarmie.ObstacleException` - Exception caused when sonar senses the rover \
is close to an obstacle.
* `mobility.swarmie.PathException` - Exception caused when the rover encounters an \
error while driving. If the rover detects a large angle between itself and its goal this \
happens. It's usually because the rover has driven over some kind of obstacle.
* `mobility.swarmie.AbortException` - Exception caused when the user places the rover into \
manual mode while it was driving.
* `mobility.swarmie.TimeoutException` - Exception caused when the drive command fails to \
complete in the amount of time specified with the `timeout=` argument.
* `mobility.swarmie.InsideHomeException` - Exception caused when the rover thinks it's inside \
of the home ring.
* `mobility.swarmie.HomeCornerException` - Exception caused when the rover sees a corner of \
the home ring.
Examples:
>>> from mobility.swarmie import swarmie
>>> swarmie.start(node_name='swarmie')
>>>
>>> # Drive the current default speed
>>> swarmie.drive(1)
>>>
>>> # Drive using the current slow speed
>>> swarmie.drive(1, **swarmie.speed_slow)
>>>
>>> # Drive using the current fast speed
>>> swarmie.drive(1, **swarmie.speed_fast)
'''
req = MoveRequest(r=distance, )
return self.__drive(req, **kwargs)