def write_with_offset(outbag, topic, msg, t, offset_sec): def add_offset(t, offset): # ignore those which are not unix timestamps return t + offset if is_unix_time(t.to_sec()) else t try: offset = Duration.from_sec(offset_sec) stamp = 0 if hasattr(msg, 'header'): msg.header.stamp = add_offset(msg.header.stamp, offset) elif hasattr(msg, 'stamp'): msg.stamp = add_offset(msg.stamp, offset) elif hasattr(msg, 'transforms'): for id,_ in enumerate(msg.transforms): msg.transforms[id].header.stamp = add_offset(msg.transforms[id].header.stamp, offset) # for RealSense metadata elif hasattr(msg, 'value'): try: value = int(msg.value) if is_unix_time(float(value) / 1000): msg.value = str(value + int(offset_sec * 1000)) except ValueError: try: value = float(msg.value) if is_unix_time(value / 1000): msg.value = '%.6f' % (value + offset_sec * 1000) except ValueError: pass except TypeError: print (str(msg)) print ('Cannot process above message: topic=%s, t=%.9f, offset=%.9f' % (topic, t.to_sec(), offset_sec)) exit() else: t = add_offset(t, offset) outbag.write(topic, msg, t)
def __init__(self, controller='motion/controller/joint_state_head', tolerance=0.17, timeout=10.0, joint_topic="joint_states", outcomes=['done', 'failed', 'timeout']): super(SetJointStateBase, self).__init__(outcomes=outcomes) # Store topic parameter for later use. self._controller = controller self._joint_topic = joint_topic self._tolerance = tolerance self._timeout = Duration.from_sec(timeout) # create proxies self._action_client = ProxyActionClient( {self._controller: SetOperationalAction}) self._pose_publisher = ProxyPublisher( {self._controller + '/in_joints_ref': JointState}) self._pose_subscriber = ProxySubscriberCached( {self._joint_topic: JointState}) # timestamp self._timestamp = None # error in enter hook self._error = False
def execute(self, userdata): if self._error: return 'failed' # check if time elasped if Time.now() - self._start_timestamp > self._duration: return 'done' # check if we have tosend new point if Time.now() - self._movement_timestamp > self._movement_duration: # determine new interval self._movement_timestamp = Time.now() self._movement_duration = Duration.from_sec( random.uniform(*self._interval)) # form message msg = JointState() msg.header = Header(stamp=Time.now()) msg.name = self._joints for i in range(0, len(self._joints)): x = random.uniform(0, 1) msg.position.append(self._minimal[i] * x + self._maximal[i] * (1 - x)) # send message try: self._publisher.publish(self._controller + '/in_joints_ref', msg) except Exception as e: Logger.logwarn('Failed to send JointState message `' + self._topic + '`:\n%s' % str(e))
def __init__(self, outcomes = ['unknown'], pose_ns = 'saved_msgs/joint_state', tolerance = 0.17, joint_topic = "joint_states", timeout = 1.0): # Process supplied pose list. Note it must be processed befor superclass constructor is called, # beacuse it changes 'outcomes' list due to side effect. # check if 'unknown' outcome is present if 'unknown' not in outcomes: raise RuntimeError, '"unknown" should presents in state outcomes' # remove 'unknown' while preserving items order poses_names = [ outcome for outcome in outcomes if outcome != 'unknown' ] # Load poses from paramere server. Pose is loaded in form dict(joint name -> position), # but they are stored as a list of tuples (pose_name, pose_dict) to preserve order. self._poses = [ (name, self.load_joint_state(pose_ns, name)) for name in poses_names ] # Call superclass constructor. super(CheckJointState, self).__init__(outcomes = outcomes) # Subscribe to the joint topic. self._joint_topic = joint_topic self._pose_subscriber = ProxySubscriberCached({ self._joint_topic: JointState }) # Store topic parameter for later use. self._tolerance = tolerance self._timeout = Duration.from_sec(timeout) # timestamp self._timestamp = None
def execute(self, userdata): if self._error: return 'failed' # check if time elasped if Time.now() - self._start_timestamp > self._duration: return 'done' # check if we have tosend new point if Time.now() - self._movement_timestamp > self._movement_duration: # determine new interval self._movement_timestamp = Time.now() self._movement_duration = Duration.from_sec( random.uniform(*self._interval)) # form message msg = JointState() msg.header = Header(stamp=Time.now()) msg.name = ['joint52', 'joint53', 'eyes_pitch', 'eyes_yaw'] # random durection x = random.uniform(0, 1) y = random.uniform(0, 1) # compute head position msg.position = [0, 0, 0, 0] msg.position[0] = self._min2356[0] * x + self._max2356[0] * (1 - x) msg.position[1] = self._min2356[1] * y + self._max2356[1] * (1 - y) # compute eyes position msg.position[2] = self._min2356[2] * y + self._max2356[2] * (1 - y) msg.position[3] = self._min2356[3] * x + self._max2356[3] * (1 - x) # send message try: self._publisher.publish(self._controller + '/in_joints_ref', msg) except Exception as e: Logger.logwarn('Failed to send JointState message `' + self._topic + '`:\n%s' % str(e))
def test_extrapolation(dir, m, pose1, pose2, service, time1, time_12): # this stuff is done to test the accuracy of the extrapolation # wait some time to get the expected position rospy.sleep(Duration.from_sec(5)) resp = service("centroid,cuboid") object3 = resp.objects[0].object time3 = resp.stamp pose3 = m.transform_to(object3) # Get the duration between the first perception and now time_13 = time3 - time1 # get the relation between the first and second duration diff_time = float(time_13.to_sec()) / float(time_12.to_sec()) # table = [["stamps: ", time1, time2, time3], ["duration: ", time_12, time_13, diff_time]] # print tabulate(table) # calculate the new longer vector (scalar multiplication by hand) dir_13 = numpy.array([diff_time * dir[0], diff_time * dir[1]]) # add this new vector on the points from the first perception pose_comp = copy.deepcopy(pose1) pose_comp.primitive_poses[0].position.x += dir_13[0] pose_comp.primitive_poses[0].position.y += dir_13[1] pose_comp.id = "red_cube" print pose_comp # print out the table for the different poses for debug table = [get_position(pose1), get_position(pose2), get_position(pose3), get_position(pose_comp)] print tabulate(table) dev = get_position(pose3)[1] / get_position(pose_comp)[1] * 100 print dev return pose_comp
def __init__(self, topic='motion/controller/look_at/in_pose_ref', duration=120, interval=[3, 5], maxXYZ=[1, 0.3, 0.5], minXYZ=[1.0, -0.3, 0.0], frame_xyz='base_link', frame_out='odom_combined'): super(RandPoseGenerator, self).__init__(outcomes=['done', 'failure']) # Store topic parameter for later use. if not isinstance(topic, str): raise ValueError("Topic name must be string.") if not isinstance(duration, (int, float)) or duration <= 0: raise ValueError("Duration must be positive number.") if len(interval) != 2 or any([ not isinstance(t, (int, float)) for t in interval ]) or any([t < 0 for t in interval]) or interval[0] > interval[1]: raise ValueError("Interval must represent interval of time.") if len(minXYZ) != 3 or any( [not isinstance(v, (int, float)) for v in minXYZ]): raise ValueError("minXYZ: list of three numbers was expected.") if len(maxXYZ) != 3 or any( [not isinstance(v, (int, float)) for v in maxXYZ]): raise ValueError("maxXYZ: list of three numbers was expected.") if not isinstance(frame_xyz, str) or not isinstance(frame_out, str): raise ValueError("Frame names must be string.") self._topic = topic self._duration = Duration.from_sec(duration) self._interval = interval self._minXYZ = minXYZ self._maxXYZ = maxXYZ self._frame_in = frame_xyz self._frame_out = frame_out # create proxies self._publisher = ProxyPublisher({self._topic: PoseStamped}) self._tf_listener = ProxyTransformListener().listener() self._tf_listener.waitForTransform(self._frame_out, self._frame_in, rospy.Time(), rospy.Duration(1)) if not self._tf_listener.canTransform(self._frame_out, self._frame_in, rospy.Time(0)): raise ValueError( "Unable to perform transform between frames %s and %s." % (self._frame_out, self._frame_in)) # state self._start_timestamp = None self._movement_timestamp = None self._movement_duration = Duration() # error in enter hook self._error = False
def set_movement_parameters(self, duration, interval, min2356, max2356): if not isinstance(duration, (int, float)) or duration < 0: raise ValueError("Duration must be nonegative number.") if len(interval) != 2 or any([ not isinstance(t, (int, float)) for t in interval ]) or any([t < 0 for t in interval]) or interval[0] > interval[1]: raise ValueError("Interval must represent interval of time.") if len(min2356) != 4 or any( [not isinstance(v, (int, float)) for v in min2356]): raise ValueError("min2356: list of four numbers was expected.") if len(max2356) != 4 or any( [not isinstance(v, (int, float)) for v in max2356]): raise ValueError("max2356: list of four numbers was expected.") self._duration = Duration.from_sec(duration) self._interval = interval self._min2356 = min2356 self._max2356 = max2356
def percieve_object(self): rospy.logdebug('FastGrasp: Start percieving Object') # create service service = rospy.ServiceProxy("/suturo/GetGripper", GetGripper) # get the first perception rospy.logdebug('FastGrasp: call Perception Service') resp = service("firstConveyorCall,cuboid") if len(resp.objects) == 0 or not resp.objects[0].c_cuboid_success: rospy.logdebug('FastGrasp: objects empty or no cuboid 1') return -1 object1 = resp.objects[0].object self.__perceived_pose_time = resp.stamp # wait some time until the second perception rospy.sleep(Duration.from_sec(0.5)) resp = service("cuboid") if len(resp.objects) == 0 or not resp.objects[0].c_cuboid_success: rospy.logdebug('FastGrasp: objects empty or no cuboid 2') return -2 object2 = resp.objects[0].object time2 = resp.stamp # transform the points into odom_combined self.__perceived_pose = utils.manipulation.transform_to(object1) # utils.manipulation.get_planning_scene().add_object(self.__perceived_pose) pose2 = utils.manipulation.transform_to(object2) # calculate the vector from the points of first and second perception self.__direction = numpy.array([ (pose2.primitive_poses[0].position.x - self.__perceived_pose.primitive_poses[0].position.x), (pose2.primitive_poses[0].position.y - self.__perceived_pose.primitive_poses[0].position.y) ]) # calculate the time between the first and second perception self.__time_between_poses = time2 - self.__perceived_pose_time rospy.logdebug('FastGrasp: Return Object') return 0
def percieve_object(self): rospy.logdebug('FastGrasp: Start percieving Object') # create service service = rospy.ServiceProxy("/suturo/GetGripper", GetGripper) # get the first perception rospy.logdebug('FastGrasp: call Perception Service') resp = service("firstConveyorCall,cuboid") if len(resp.objects) == 0 or not resp.objects[0].c_cuboid_success: rospy.logdebug('FastGrasp: objects empty or no cuboid 1') return -1 object1 = resp.objects[0].object self.__perceived_pose_time = resp.stamp # wait some time until the second perception rospy.sleep(Duration.from_sec(0.5)) resp = service("cuboid") if len(resp.objects) == 0 or not resp.objects[0].c_cuboid_success: rospy.logdebug('FastGrasp: objects empty or no cuboid 2') return -2 object2 = resp.objects[0].object time2 = resp.stamp # transform the points into odom_combined self.__perceived_pose = utils.manipulation.transform_to(object1) # utils.manipulation.get_planning_scene().add_object(self.__perceived_pose) pose2 = utils.manipulation.transform_to(object2) # calculate the vector from the points of first and second perception self.__direction = numpy.array( [(pose2.primitive_poses[0].position.x - self.__perceived_pose.primitive_poses[0].position.x), (pose2.primitive_poses[0].position.y - self.__perceived_pose.primitive_poses[0].position.y)]) # calculate the time between the first and second perception self.__time_between_poses = time2 - self.__perceived_pose_time rospy.logdebug('FastGrasp: Return Object') return 0
def set_movement_parameters(self, duration, interval, joints, minimal, maximal): if not isinstance(duration, (int, float)) or duration < 0: raise ValueError("Duration must be nonegative number.") if len(interval) != 2 or any([ not isinstance(t, (int, float)) for t in interval ]) or any([t < 0 for t in interval]) or interval[0] > interval[1]: raise ValueError("Interval must represent interval of time.") if len(joints) != len(minimal) or len(joints) != len(maximal): raise ValueError( "joints, minimal and maximal arrays must have the same size.") if any([not isinstance(v, str) for v in joints]): raise ValueError("joints: list of string is expected.") if any([not isinstance(v, (int, float)) for v in minimal]): raise ValueError("minimal: list of numbers is expected.") if any([not isinstance(v, (int, float)) for v in maximal]): raise ValueError("maximal: list of numbers is expected.") self._duration = Duration.from_sec(duration) self._interval = interval self._joints = joints self._minimal = minimal self._maximal = maximal
def execute(self, userdata): if self._error: return 'failure' time_now = Time.now() # check if time elasped if time_now - self._start_timestamp > self._duration: return 'done' # check if we have tosend new point if time_now - self._movement_timestamp > self._movement_duration: # determine new interval self._movement_timestamp = time_now self._movement_duration = Duration.from_sec( random.uniform(*self._interval)) # form message msg = PoseStamped() msg.header.frame_id = self._frame_in msg.header.stamp = Time(0.0) # random position msg.pose.position.x = random.uniform(self._minXYZ[0], self._maxXYZ[0]) msg.pose.position.y = random.uniform(self._minXYZ[1], self._maxXYZ[1]) msg.pose.position.z = random.uniform(self._minXYZ[2], self._maxXYZ[2]) # transform to output frame try: # transform self._pose = self._tf_listener.transformPose( self._frame_out, msg) except tf.Exception as e: Logger.logwarn('Unable to transform from %s to %s: %s' % (self._frame_in, self._frame_out, e)) return 'failure' # publish pose msg.header.stamp = self._movement_timestamp self._publisher.publish(self._topic, msg)
def pos_calc_test(m): # create service service = rospy.ServiceProxy("/suturo/GetGripper", GetGripper) # get the first perception resp = service("firstConveyorCall,centroid,cuboid") object1 = resp.objects[0].object time1 = resp.stamp # wait some time until the second perception rospy.sleep(Duration.from_sec(0.5)) resp = service("centroid,cuboid") object2 = resp.objects[0].object time2 = resp.stamp # transform the points into odom_combined pose1 = m.transform_to(object1) pose2 = m.transform_to(object2) # calculate the vector from the points of first and second perception dir = numpy.array([(pose2.primitive_poses[0].position.x - pose1.primitive_poses[0].position.x), ( pose2.primitive_poses[0].position.y - pose1.primitive_poses[0].position.y)]) # dir_dist = sqrt(pow(dir[0], 2) + pow(dir[1], 2)) # get the duration between the two looks time_12 = time2 - time1 # set the time in 5 secs t_5 = rospy.Time.now() + rospy.Duration(8) time_13 = t_5 - time1 diff_time = float(time_13.to_sec()) / float(time_12.to_sec()) dir_13 = numpy.array([diff_time * dir[0], diff_time * dir[1]]) # add this new vector on the points from the first perception pose_comp = copy.deepcopy(pose1) pose_comp.primitive_poses[0].position.x += dir_13[0] pose_comp.primitive_poses[0].position.y += dir_13[1] pose_comp.id = "red_cube" # print out the table for the different poses for debug table = [get_position(pose1), get_position(pose2), get_position(pose_comp)] print tabulate(table) # return test_extrapolation(dir, m, pose1, pose2, service, time1, time_12) return pose_comp
def main(): topic_mocap = '/vrpn_client_node/RigidBody1/pose' frame_mocap = 'RigidBody1' odom_frame_id = 'base_odom' odom_child_frame_id = 'base_link' parser = argparse.ArgumentParser() parser.add_argument("-s", "--sync-mocap-file", type=str, help="sync file for mocap", default="") args, left = parser.parse_known_args() if len(left) != 1: print ("Usage: " + sys.argv[0] + " odom_and_mocap.bag [-s sync_time.txt]") return in_file = left[0] sync_file = args.sync_mocap_file out_file = 'preprocessed.bag' ##the time diff between mocap client and server if sync_file != '': mocap_offset = getMin_diff_ser_clit(sync_file) print ('mocap time offset: %f' % mocap_offset) mocap_offset = Duration.from_sec(mocap_offset) else: mocap_offset = None with rosbag.Bag(in_file) as inbag: print ("-------------------- Input: %s ------------------------------" % in_file) print (inbag) outbag = rosbag.Bag(out_file, 'w') writer_mocap = TopicWriter(outbag, topic_mocap) writer_mocap.set_median_filter(1.0, 0.5) gx_poses = {} gx_stamp_offset = None for topic,msg,t in inbag.read_messages(): if topic=="/gx/RosPose2d": if msg.timestamp in gx_poses: old = gx_poses[msg.timestamp] if msg == old: continue else: print old print msg print ('different odom message with identical stamp!') exit() if gx_stamp_offset is None: """ Use bag time if stamp is abnormal """ stamp = gx_msg_stamp(msg) if abs(stamp.to_sec() - t.to_sec()) > 1000: gx_stamp_offset = t.to_sec() - stamp.to_sec() print ('odom time offset: %f' % gx_stamp_offset) else: gx_stamp_offset = 0 gxPose2d_to_odom(msg,t,outbag,odom_frame_id,odom_child_frame_id,gx_stamp_offset) gxPose2d_to_tf(msg,t,outbag,odom_frame_id,odom_child_frame_id,gx_stamp_offset) gx_poses[msg.timestamp] = msg elif topic == topic_mocap: if mocap_offset is None: print ('\nWarning: found mocap topic but sync file is not specified (perhaps run again with [-s sync_time.txt]?)\n') mocap_offset = Duration.from_sec(0) msg.header.stamp += mocap_offset writer_mocap.write_sliding(msg, t) elif topic=="/gx/RosBaseImu": pass elif topic=="/gx/RosBaseTicks": pass elif topic == '/tf' and msg.transforms[0].child_frame_id == frame_mocap: # merge mocap tf into its topic msg.transforms[0].header.stamp += mocap_offset writer_mocap.write_sliding(tf_to_pose(msg), t) else: outbag.write(topic,msg,t) outbag.close() outbag = rosbag.Bag(out_file) print ("------------------- Output: %s ------------------------------" % out_file) print (outbag)
def decode(self, data): """ Generate a rospy.rostime.Duration instance based on the given data which should be a string representation of a float. """ return Duration.from_sec(float(data))
def execute(self, userdata): rospy.logdebug('FastGrasp: Executing state FastGrasp') self.__t_point.header.frame_id = "/odom_combined" # TODO: Auf 10 Min grenzen testen if not utils.manipulation.is_gripper_open(): rospy.logdebug('FastGrasp: Open Gripper') utils.manipulation.open_gripper() r = self.percieve_object() i = 0 while r == -1: if i == 2 and not userdata.request_second_object: # TODO: Zeit anpassen, ab wann gecalled wird rospy.logdebug('FastGrasp: Request next object') rospy.ServiceProxy("/euroc_interface_node/request_next_object", RequestNextObject).call() userdata.request_second_object = True if i == 19 and userdata.request_second_object: rospy.logdebug('FastGrasp: Time Expired') return 'noObjectsLeft' r = self.percieve_object() i += 1 # TODO: Was passiert wenn das Objekt nur einmal gesehen wurde rospy.logdebug('FastGrasp: Begin movement, Plan 1') for j in range(0, 4): self.extrapolate(j) self.calculate_target_point(self.__pose_comp) if utils.manipulation.move_to(self.__t_point): rospy.logdebug("FastGrasp: Plan 1: moved!") break else: rospy.logdebug("FastGrasp: Plan 1: No Plan fount in step " + str(j)) if j == 3: return 'noPlanFound' if userdata.object_index == 1: offset = rospy.Duration(2) elif userdata.object_index == 2: offset = rospy.Duration(4) else: offset = rospy.Duration(4) while rospy.get_rostime() < self.__t_point_time - offset: rospy.sleep(0.01) self.__t_point.pose.position.z -= 0.07 rospy.logdebug("FastGrasp: Plan 2") if utils.manipulation.move_to(self.__t_point): rospy.logdebug("FastGrasp: Plan 2: moved!") else: rospy.logdebug("FastGrasp: Plan 2: Cant grasp") return 'noPlanFound' utils.manipulation.close_gripper(self.__pose_comp) self.__t_point.pose.position.z += 0.1 rospy.logdebug("FastGrasp: Plan 3") for k in range(0, 4): if utils.manipulation.move_to(self.__t_point): rospy.logdebug("FastGrasp: Plan 3: moved!") break else: if k == 3: rospy.logdebug("FastGrasp: No Plan found to lift object") return 'noPlanFound' rospy.logdebug("FastGrasp: Plan 3: Cant lift: " + str(k)) if self.__t_point.pose.position.y > 0: self.__t_point.pose.position.y -= 0.1 else: self.__t_point.pose.position.y += 0.1 if self.__t_point.pose.position.x > 0: self.__t_point.pose.position.x -= 0.1 else: self.__t_point.pose.position.x += 0.1 rospy.sleep(Duration.from_sec(0.5)) tfs = TorqueForceService() if tfs.is_free(): rospy.logdebug("FastGrasp: Grasp Fail") return 'graspingFailed' rospy.logdebug("FastGrasp: objectGrasped, finished") userdata.object_index += 1 return 'objectGrasped'
def decode(self, _, data): """ Generate a rospy.rostime.Duration instance based on the given data which should be a string representation of a float. """ return Duration.from_sec(float(data))