def __init__(self):
rospy.init_node('detection_node')
rospy.on_shutdown(self.shutdownhook)
self.coord = CameraCoordinates()
self.lock_capture = threading.Lock()
self.video_captures = []
self.frames = [None, None]
self.frame_times = [None, None]
self.detect_time = rospy.Time.now()
self.detect_count = 0
flips = [2, 2]
self.start_cameras(flips)
rospy.loginfo(rospy.get_name() + " Opened cameras")
self.last_image_sent = rospy.Time.now()
# Load model
if debug_detect:
rospy.loginfo(rospy.get_name() + " Loading model...")
tensorflow.keras.backend.clear_session()
start = rospy.Time.now()
self.detect_fn = tensorflow.saved_model.load(path_to_checkpoint)
rospy.loginfo(rospy.get_name() +
" ...model loaded in {} seconds".format(
(rospy.Time.now() - start) / 1000000000))
# Initialize the node and name it.
self.detectionarray_pub = rospy.Publisher("detectionarray",
detectionarray,
queue_size=10)
self.detectionimage_pub = rospy.Publisher("detectionimage",
detectionimage,
queue_size=2)
if debug_detect:
rospy.Timer(Duration.from_sec(1.0 / detect_fps),
self.callback_detect)
if debug_show:
rospy.Timer(Duration.from_sec(1.0 / cam_fps), self.callback_show)
# Read images from cameras at a bit faster than the frame rate (to never have buffer overrun)
# Doing this in two separate threads (timers) guarantees that we have full speed on both channels
rospy.Timer(Duration.from_sec(1.0 / (cam_fps * 1.1)),
self.callback_readimg0)
rospy.Timer(Duration.from_sec(1.0 / (cam_fps * 1.1)),
self.callback_readimg1)
rospy.Service('detection_node/save_image', Empty,
self.save_image) #Can be used to debug
rospy.loginfo(rospy.get_name() + " started detection_node.")
rospy.spin()
def _createTimeRange(randomBag, startTime, stopTime):
if startTime == _startTime0 and stopTime == _stopTime_1:
msgSamples = range(len(randomBag))
else:
if startTime == _startTime0:
startTime = randomBag.timestamps[0]
if stopTime == _stopTime_1:
stopTime = randomBag.timestamps[-1]
probeTime = startTime - Duration.from_sec(_timeFuzzyOffset)
if probeTime >= randomBag.timestamps[0]:
startTime = probeTime
msgSamples = randomBag.desample(-1, True, startTime, stopTime)
return msgSamples
def positionAt(self, time):
"""
Returns position at requested time using interpolation
"""
if (isinstance(time, float)):
if (time < 0 or time > self.duration.to_sec()):
raise ValueError("Offset value is outside bag timestamp range")
time = self.timestamps[0] + Duration.from_sec(time)
elif (isinstance(time, Time)):
if (time < self.timestamps[0] or time > self.timestamps[-1]):
raise ValueError("Timestamp value is outside the bag range")
_t1 = bisect_left(self.timestamps, time)
if _t1 == len(self.timestamps) - 1:
_t1 = len(self.timestamps) - 2
t1 = self.timestamps[_t1]
t2 = self.timestamps[_t1 + 1]
r = ((time - t1).to_sec()) / (t2 - t1).to_sec()
# return self.coordinates[_t1] + (self.coordinates[_t1+1] - self.coordinates[_t1])*r
return GeographicTrajectory.interpolate(self.coordinates[_t1],
self.coordinates[_t1 + 1], r)
def test_Time(self):
# This is a copy of test_roslib_rostime
from rospy import Time, Duration
# #1600 Duration > Time should fail
failed = False
try:
v = Duration.from_sec(0.1) > Time.from_sec(0.5)
failed = True
except:
pass
self.failIf(failed, "should have failed to compare")
try:
v = Time.from_sec(0.4) > Duration.from_sec(0.1)
failed = True
except:
pass
self.failIf(failed, "should have failed to compare")
try: # neg time fails
Time(-1)
failed = True
except:
pass
self.failIf(failed, "negative time not allowed")
try:
Time(1, -1000000001)
failed = True
except:
pass
self.failIf(failed, "negative time not allowed")
# test Time.now() is within 10 seconds of actual time (really generous)
import time
t = time.time()
v = Time.from_sec(t)
self.assertEquals(v.to_sec(), t)
# test from_sec()
self.assertEquals(Time.from_sec(0), Time())
self.assertEquals(Time.from_sec(1.), Time(1))
self.assertEquals(Time.from_sec(v.to_sec()), v)
self.assertEquals(v.from_sec(v.to_sec()), v)
# test to_time()
self.assertEquals(v.to_sec(), v.to_time())
# test addition
# - time + time fails
try:
v = Time(1, 0) + Time(1, 0)
failed = True
except:
pass
self.failIf(failed, "Time + Time must fail")
# - time + duration
v = Time(1, 0) + Duration(1, 0)
self.assertEquals(Time(2, 0), v)
v = Duration(1, 0) + Time(1, 0)
self.assertEquals(Time(2, 0), v)
v = Time(1, 1) + Duration(1, 1)
self.assertEquals(Time(2, 2), v)
v = Duration(1, 1) + Time(1, 1)
self.assertEquals(Time(2, 2), v)
v = Time(1) + Duration(0, 1000000000)
self.assertEquals(Time(2), v)
v = Duration(1) + Time(0, 1000000000)
self.assertEquals(Time(2), v)
v = Time(100, 100) + Duration(300)
self.assertEquals(Time(400, 100), v)
v = Duration(300) + Time(100, 100)
self.assertEquals(Time(400, 100), v)
v = Time(100, 100) + Duration(300, 300)
self.assertEquals(Time(400, 400), v)
v = Duration(300, 300) + Time(100, 100)
self.assertEquals(Time(400, 400), v)
v = Time(100, 100) + Duration(300, -101)
self.assertEquals(Time(399, 999999999), v)
v = Duration(300, -101) + Time(100, 100)
self.assertEquals(Time(399, 999999999), v)
# test subtraction
try:
v = Time(1, 0) - 1
failed = True
except:
pass
self.failIf(failed, "Time - non Duration must fail")
class Foob(object):
pass
try:
v = Time(1, 0) - Foob()
failed = True
except:
pass
self.failIf(failed, "Time - non TVal must fail")
# - Time - Duration
v = Time(1, 0) - Duration(1, 0)
self.assertEquals(Time(), v)
v = Time(1, 1) - Duration(-1, -1)
self.assertEquals(Time(2, 2), v)
v = Time(1) - Duration(0, 1000000000)
self.assertEquals(Time(), v)
v = Time(2) - Duration(0, 1000000000)
self.assertEquals(Time(1), v)
v = Time(400, 100) - Duration(300)
self.assertEquals(Time(100, 100), v)
v = Time(100, 100) - Duration(0, 101)
self.assertEquals(Time(99, 999999999), v)
# - Time - Time = Duration
v = Time(100, 100) - Time(100, 100)
self.assertEquals(Duration(), v)
v = Time(100, 100) - Time(100)
self.assertEquals(Duration(0, 100), v)
v = Time(100) - Time(200)
self.assertEquals(Duration(-100), v)
def test_Duration(self):
Duration = rospy.Duration
# test from_sec
v = Duration(1000)
self.assertEquals(v, Duration.from_sec(v.to_sec()))
self.assertEquals(v, v.from_sec(v.to_sec()))
v = Duration(0, 1000)
self.assertEquals(v, Duration.from_sec(v.to_sec()))
self.assertEquals(v, v.from_sec(v.to_sec()))
# test neg
v = -Duration(1, -1)
self.assertEquals(-1, v.secs)
self.assertEquals(1, v.nsecs)
v = -Duration(-1, -1)
self.assertEquals(1, v.secs)
self.assertEquals(1, v.nsecs)
v = -Duration(-1, 1)
self.assertEquals(0, v.secs)
self.assertEquals(999999999, v.nsecs)
# test addition
failed = False
try:
v = Duration(1, 0) + Time(1, 0)
failed = True
except:
pass
self.failIf(failed, "Duration + Time must fail")
try:
v = Duration(1, 0) + 1
failed = True
except:
pass
self.failIf(failed, "Duration + int must fail")
v = Duration(1, 0) + Duration(1, 0)
self.assertEquals(2, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(2, 0), v)
v = Duration(-1, -1) + Duration(1, 1)
self.assertEquals(0, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(), v)
v = Duration(1) + Duration(0, 1000000000)
self.assertEquals(2, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(2), v)
v = Duration(100, 100) + Duration(300)
self.assertEquals(Duration(400, 100), v)
v = Duration(100, 100) + Duration(300, 300)
self.assertEquals(Duration(400, 400), v)
v = Duration(100, 100) + Duration(300, -101)
self.assertEquals(Duration(399, 999999999), v)
# test subtraction
try:
v = Duration(1, 0) - 1
failed = True
except:
pass
self.failIf(failed, "Duration - non duration must fail")
try:
v = Duration(1, 0) - Time(1, 0)
failed = True
except:
pass
self.failIf(failed, "Duration - Time must fail")
v = Duration(1, 0) - Duration(1, 0)
self.assertEquals(Duration(), v)
v = Duration(-1, -1) - Duration(1, 1)
self.assertEquals(Duration(-3, 999999998), v)
v = Duration(1) - Duration(0, 1000000000)
self.assertEquals(Duration(), v)
v = Duration(2) - Duration(0, 1000000000)
self.assertEquals(Duration(1), v)
v = Duration(100, 100) - Duration(300)
self.assertEquals(Duration(-200, 100), v)
v = Duration(100, 100) - Duration(300, 101)
self.assertEquals(Duration(-201, 999999999), v)
# test abs
self.assertEquals(abs(Duration()), Duration())
self.assertEquals(abs(Duration(1)), Duration(1))
self.assertEquals(abs(Duration(-1)), Duration(1))
self.assertEquals(abs(Duration(0, -1)), Duration(0, 1))
self.assertEquals(abs(Duration(-1, -1)), Duration(1, 1))
def or_to_ros_trajectory(robot, traj, time_tolerance=0.01):
""" Convert an OpenRAVE trajectory to a ROS trajectory.
@param robot: OpenRAVE robot
@type robot: openravepy.Robot
@param traj: input trajectory
@type traj: openravepy.Trajectory
@param time_tolerance: minimum time between two waypoints
@type time_tolerance: float
"""
import numpy
from rospy import Duration
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
assert time_tolerance >= 0.
if traj.GetEnv() != robot.GetEnv():
raise ValueError(
'Robot and trajectory are not in the same environment.')
cspec = traj.GetConfigurationSpecification()
dof_indices, _ = cspec.ExtractUsedIndices(robot)
traj_msg = JointTrajectory(
joint_names=[ robot.GetJointFromDOFIndex(dof_index).GetName()
for dof_index in dof_indices ]
)
time_from_start = 0.
prev_time_from_start = 0.
for iwaypoint in xrange(traj.GetNumWaypoints()):
waypoint = traj.GetWaypoint(iwaypoint)
dt = cspec.ExtractDeltaTime(waypoint)
q = cspec.ExtractJointValues(waypoint, robot, dof_indices, 0)
qd = cspec.ExtractJointValues(waypoint, robot, dof_indices, 1)
qdd = cspec.ExtractJointValues(waypoint, robot, dof_indices, 2)
if dt is None:
raise ValueError('Trajectory is not timed.')
elif q is None:
raise ValueError('Trajectory does not contain joint values')
elif qdd is not None and qd is None:
raise ValueError('Trajectory contains accelerations,'
' but not velocities.')
# Duplicate waypoints break trajectory execution, so we explicitly
# filter them out. Note that we check the difference in time between
# the current and the previous waypoint, not the raw "dt" value. This
# is necessary to support very densely sampled trajectories.
time_from_start += dt
deltatime = time_from_start - prev_time_from_start
if iwaypoint > 0 and deltatime < time_tolerance:
logger.warning('Skipped waypoint %d because deltatime is %.3f < %.3f.',
deltatime, time_tolerance)
continue
prev_time_from_start = time_from_start
# Create the waypoint.
traj_msg.points.append(
JointTrajectoryPoint(
positions=list(q),
velocities=list(qd) if qd is not None else [],
accelerations=list(qdd) if qdd is not None else [],
time_from_start=Duration.from_sec(time_from_start)
)
)
assert abs(time_from_start - traj.GetDuration()) < time_tolerance
return traj_msg
def test_Time(self):
# This is a copy of test_roslib_rostime
from rospy import Time, Duration
# #1600 Duration > Time should fail
failed = False
try:
v = Duration.from_sec(0.1) > Time.from_sec(0.5)
failed = True
except: pass
self.failIf(failed, "should have failed to compare")
try:
v = Time.from_sec(0.4) > Duration.from_sec(0.1)
failed = True
except: pass
self.failIf(failed, "should have failed to compare")
try: # neg time fails
Time(-1)
failed = True
except: pass
self.failIf(failed, "negative time not allowed")
try:
Time(1, -1000000001)
failed = True
except: pass
self.failIf(failed, "negative time not allowed")
# test Time.now() is within 10 seconds of actual time (really generous)
import time
t = time.time()
v = Time.from_sec(t)
self.assertEquals(v.to_sec(), t)
# test from_sec()
self.assertEquals(Time.from_sec(0), Time())
self.assertEquals(Time.from_sec(1.), Time(1))
self.assertEquals(Time.from_sec(v.to_sec()), v)
self.assertEquals(v.from_sec(v.to_sec()), v)
# test to_time()
self.assertEquals(v.to_sec(), v.to_time())
# test addition
# - time + time fails
try:
v = Time(1,0) + Time(1, 0)
failed = True
except: pass
self.failIf(failed, "Time + Time must fail")
# - time + duration
v = Time(1,0) + Duration(1, 0)
self.assertEquals(Time(2, 0), v)
v = Duration(1, 0) + Time(1,0)
self.assertEquals(Time(2, 0), v)
v = Time(1,1) + Duration(1, 1)
self.assertEquals(Time(2, 2), v)
v = Duration(1, 1) + Time(1,1)
self.assertEquals(Time(2, 2), v)
v = Time(1) + Duration(0, 1000000000)
self.assertEquals(Time(2), v)
v = Duration(1) + Time(0, 1000000000)
self.assertEquals(Time(2), v)
v = Time(100, 100) + Duration(300)
self.assertEquals(Time(400, 100), v)
v = Duration(300) + Time(100, 100)
self.assertEquals(Time(400, 100), v)
v = Time(100, 100) + Duration(300, 300)
self.assertEquals(Time(400, 400), v)
v = Duration(300, 300) + Time(100, 100)
self.assertEquals(Time(400, 400), v)
v = Time(100, 100) + Duration(300, -101)
self.assertEquals(Time(399, 999999999), v)
v = Duration(300, -101) + Time(100, 100)
self.assertEquals(Time(399, 999999999), v)
# test subtraction
try:
v = Time(1,0) - 1
failed = True
except: pass
self.failIf(failed, "Time - non Duration must fail")
class Foob(object): pass
try:
v = Time(1,0) - Foob()
failed = True
except: pass
self.failIf(failed, "Time - non TVal must fail")
# - Time - Duration
v = Time(1,0) - Duration(1, 0)
self.assertEquals(Time(), v)
v = Time(1,1) - Duration(-1, -1)
self.assertEquals(Time(2, 2), v)
v = Time(1) - Duration(0, 1000000000)
self.assertEquals(Time(), v)
v = Time(2) - Duration(0, 1000000000)
self.assertEquals(Time(1), v)
v = Time(400, 100) - Duration(300)
self.assertEquals(Time(100, 100), v)
v = Time(100, 100) - Duration(0, 101)
self.assertEquals(Time(99, 999999999), v)
# - Time - Time = Duration
v = Time(100, 100) - Time(100, 100)
self.assertEquals(Duration(), v)
v = Time(100, 100) - Time(100)
self.assertEquals(Duration(0,100), v)
v = Time(100) - Time(200)
self.assertEquals(Duration(-100), v)
def test_Duration(self):
Duration = rospy.Duration
# test from_sec
v = Duration(1000)
self.assertEquals(v, Duration.from_sec(v.to_sec()))
self.assertEquals(v, v.from_sec(v.to_sec()))
v = Duration(0,1000)
self.assertEquals(v, Duration.from_sec(v.to_sec()))
self.assertEquals(v, v.from_sec(v.to_sec()))
# test neg
v = -Duration(1, -1)
self.assertEquals(-1, v.secs)
self.assertEquals(1, v.nsecs)
v = -Duration(-1, -1)
self.assertEquals(1, v.secs)
self.assertEquals(1, v.nsecs)
v = -Duration(-1, 1)
self.assertEquals(0, v.secs)
self.assertEquals(999999999, v.nsecs)
# test addition
failed = False
try:
v = Duration(1,0) + Time(1, 0)
failed = True
except: pass
self.failIf(failed, "Duration + Time must fail")
try:
v = Duration(1,0) + 1
failed = True
except: pass
self.failIf(failed, "Duration + int must fail")
v = Duration(1,0) + Duration(1, 0)
self.assertEquals(2, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(2, 0), v)
v = Duration(-1,-1) + Duration(1, 1)
self.assertEquals(0, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(), v)
v = Duration(1) + Duration(0, 1000000000)
self.assertEquals(2, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(2), v)
v = Duration(100, 100) + Duration(300)
self.assertEquals(Duration(400, 100), v)
v = Duration(100, 100) + Duration(300, 300)
self.assertEquals(Duration(400, 400), v)
v = Duration(100, 100) + Duration(300, -101)
self.assertEquals(Duration(399, 999999999), v)
# test subtraction
try:
v = Duration(1,0) - 1
failed = True
except: pass
self.failIf(failed, "Duration - non duration must fail")
try:
v = Duration(1, 0) - Time(1,0)
failed = True
except: pass
self.failIf(failed, "Duration - Time must fail")
v = Duration(1,0) - Duration(1, 0)
self.assertEquals(Duration(), v)
v = Duration(-1,-1) - Duration(1, 1)
self.assertEquals(Duration(-3, 999999998), v)
v = Duration(1) - Duration(0, 1000000000)
self.assertEquals(Duration(), v)
v = Duration(2) - Duration(0, 1000000000)
self.assertEquals(Duration(1), v)
v = Duration(100, 100) - Duration(300)
self.assertEquals(Duration(-200, 100), v)
v = Duration(100, 100) - Duration(300, 101)
self.assertEquals(Duration(-201, 999999999), v)
# test abs
self.assertEquals(abs(Duration()), Duration())
self.assertEquals(abs(Duration(1)), Duration(1))
self.assertEquals(abs(Duration(-1)), Duration(1))
self.assertEquals(abs(Duration(0,-1)), Duration(0,1))
self.assertEquals(abs(Duration(-1,-1)), Duration(1,1))
def __init__(self):
self.pub = Pub('/trace', Path, queue_size=100)
self.loc = LocationFinder()
self.path = Path()
self.path.header.frame_id = 'odom'
Timer(Duration.from_sec(1), self._path)
'SUCCEEDED', 'ABORTED', 'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST', ] GRIPPER_DIST = 0.15 #Distance from the brick when setting a goal GRIPPER_TOLERANCE = 0.003 #Max distance in m from gripper in order to pick up the piece GRIPPER_POS_DOWN = 0.95 # Servo position when gripper is down GRIPPER_POS_UP = 0.25 # Servo position when gripper is up (default position) GRIPPER_MOVE_TIME= Duration.from_sec(2.5) # time to move gripper from one position to another GRIPPER_GRIP_TIME= Duration.from_sec(1.5) # time to open/close gripper GOAL_STATUS_LOG_INTERVAL = Duration.from_sec(5.0) enc_per_m=11500.0 #encoder units per cm #TODO: centralize this value (shared with odom_data) MAX_DISTANCE_SLOWDOWN = 0.50 # Distance when robot will start slowing down as it is approaching the brick MIN_DISTANCE_SLOWDOWN = 0.15 # Distance when robot will have reached minimum speed MAX_SPEED = 0.21 # Max speed - keep this in line with navigation params MAX_ANG_SPEED = 0.18 # Max speed - keep this in line with navigation params MIN_SPEED = 0.05 # Min speed, to be used when we get really close to brick / approaching the brick. Must be > penalty_epsilon MIN_ANG_SPEED = 0.02 # Min angular speed, to be used when approaching #Tolerance of base_link to waypoint before the target waypoint is updated to the next one WAYPOINT_TOLERANCE = 0.5 #Defines a path which the robot should follow if there is no brick around to collect IDLE_WAYPOINTS = [
def or_to_ros_trajectory(robot, traj, time_tolerance=0.01):
""" Convert an OpenRAVE trajectory to a ROS trajectory.
@param robot: OpenRAVE robot
@type robot: openravepy.Robot
@param traj: input trajectory
@type traj: openravepy.Trajectory
@param time_tolerance: minimum time between two waypoints
@type time_tolerance: float
"""
import numpy
from rospy import Duration
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
assert time_tolerance >= 0.
if traj.GetEnv() != robot.GetEnv():
raise ValueError(
'Robot and trajectory are not in the same environment.')
cspec = traj.GetConfigurationSpecification()
dof_indices, _ = cspec.ExtractUsedIndices(robot)
traj_msg = JointTrajectory(joint_names=[
robot.GetJointFromDOFIndex(dof_index).GetName()
for dof_index in dof_indices
])
time_from_start = 0.
prev_time_from_start = 0.
for iwaypoint in xrange(traj.GetNumWaypoints()):
waypoint = traj.GetWaypoint(iwaypoint)
dt = cspec.ExtractDeltaTime(waypoint)
q = cspec.ExtractJointValues(waypoint, robot, dof_indices, 0)
qd = cspec.ExtractJointValues(waypoint, robot, dof_indices, 1)
qdd = cspec.ExtractJointValues(waypoint, robot, dof_indices, 2)
if dt is None:
raise ValueError('Trajectory is not timed.')
elif q is None:
raise ValueError('Trajectory does not contain joint values')
elif qdd is not None and qd is None:
raise ValueError('Trajectory contains accelerations,'
' but not velocities.')
# Duplicate waypoints break trajectory execution, so we explicitly
# filter them out. Note that we check the difference in time between
# the current and the previous waypoint, not the raw "dt" value. This
# is necessary to support very densely sampled trajectories.
time_from_start += dt
deltatime = time_from_start - prev_time_from_start
# openrave includes the first trajectory point as current, with time zero
# ros ignores this. so if time zero, then skip
if time_from_start == 0:
continue
if iwaypoint > 0 and deltatime < time_tolerance:
logger.warning(
'Skipped waypoint %d because deltatime is %.3f < %.3f.',
iwaypoint, deltatime, time_tolerance)
continue
prev_time_from_start = time_from_start
# Create the waypoint.
traj_msg.points.append(
JointTrajectoryPoint(
positions=list(q),
velocities=list(qd) if qd is not None else [],
accelerations=list(qdd) if qdd is not None else [],
time_from_start=Duration.from_sec(time_from_start)))
assert abs(time_from_start - traj.GetDuration()) < time_tolerance
return traj_msg
