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