def _get_transforms(self, time=None): """ Samples the transforms at the given time. :param time: sampling time (now if None) :type time: None|Time :rtype: dict[str, ((float, float, float), (float, float, float, float))] """ if time is None: time = Time.now() # here we trick the library (it is actually made for eye_on_hand only). Trust me, I'm an engineer if self.handeye_parameters.eye_on_hand: rob = self.tfBuffer.lookup_transform( self.handeye_parameters.robot_base_frame, self.handeye_parameters.robot_effector_frame, time, Duration(10)) else: rob = self.tfBuffer.lookup_transform( self.handeye_parameters.robot_effector_frame, self.handeye_parameters.robot_base_frame, time, Duration(10)) opt = self.tfBuffer.lookup_transform( self.handeye_parameters.tracking_base_frame, self.handeye_parameters.tracking_marker_frame, time, Duration(10)) return {'robot': rob, 'optical': opt}
def __init__(self, randomBag=None, eastingShift=0.0, northingShift=0.0, heightShift=0.0, startTime=_startTime0, stopTime=_stopTime_1): if (randomBag is None): self.timestamps = [] self.coordinates = [] self.duration = Duration(0) return if (randomBag.type() == 'sensor_msgs/NavSatFix'): self.timestamps, self.coordinates = GeographicTrajectory._parseFromNavSatFix( randomBag, eastingShift, northingShift, heightShift, startTime, stopTime) elif (randomBag.type() == 'nmea_msgs/Sentence'): self.timestamps, self.coordinates = GeographicTrajectory._parseFromNmea( randomBag, eastingShift, northingShift, heightShift, startTime, stopTime) else: raise ValueError("Input bag is of unknown type") self.duration = self.timestamps[-1] - self.timestamps[0] self.frame_id = randomBag[0].header.frame_id
def make_drum_song(t, measures): s = Song(start_sec=t) for x in range(measures): s.song.append(Note(time=Duration((x * 4 + 0) * X), key='hi-hat')) s.song.append(Note(time=Duration((x * 4 + 1) * X), key='hi-hat')) s.song.append(Note(time=Duration((x * 4 + 2) * X), key='hi-hat')) s.song.append(Note(time=Duration((x * 4 + 3) * X), key='snare')) return s
def split(self, secs): t = self.to_param(secs) curve1, curve2 = self.bezier_curve.de_casteljau(t) duration1 = Duration(secs) duration2 = Duration(self.duration.to_sec() - secs) path1 = BezierPath(bezier_curve=curve1, duration=duration1) path2 = BezierPath(bezier_curve=curve2, duration=duration2) return path1, path2
def _wait_for_tf_init(self): """ Waits until all needed frames are present in tf. :rtype: None """ self.tfBuffer.lookup_transform( self.handeye_parameters.robot_base_frame, self.handeye_parameters.robot_effector_frame, Time(0), Duration(20)) self.tfBuffer.lookup_transform( self.handeye_parameters.tracking_base_frame, self.handeye_parameters.tracking_marker_frame, Time(0), Duration(60))
def __init__(self, linear_vel: float, angular_vel: float): super().__init__(outcomes=['ok', 'preempted']) self.cmd_vel_pub = Publisher(CMD_VEL_TOPIC, Twist, queue_size=1) self.rate = Rate(10) self.duration = Duration(1) self.linear_vel = linear_vel self.angular_vel = angular_vel
def retry_action(client=None, goal=None, timeout=0, msg=''): """ Meta function that creates partials to test action servers. :param :client The instance of the client we will use for the test. :param :goal The goal we will send to the action server. :param :timeout The amount of time the client will wait before attempts a retry. :param :msg A text for debugging. """ timeout = Duration(timeout) log.info(msg) while True: client.wait_for_server() client.send_goal(goal) success = client.wait_for_result(timeout=timeout) goal_state = client.get_state() if success: if goal_state not in FAILURE_STATES.keys(): break else: verbose_err = FAILURE_STATES[goal_state] log.error('%s responded with %s', msg, verbose_err) else: log.error("Couldn't test %s in time...", msg) sleep(2) log.info('Retrying...')
def get_short_data(self): """ Returns a shortend version of the item data. :returns: data of the item :rtype: str """ data_dict = self.get_latest_data() if data_dict["window_stop"] == Time(0): return "No data yet" elif (Time.now() - data_dict["window_stop"]) > Duration(MAXIMUM_OFFLINE_TIME): # last entry was more than MAXIMUM_OFFLINE_TIME ago, it could be offline! return "No data since " + prepare_number_for_representation(Time.now() - data_dict["window_stop"]) \ + " seconds" content = "" if data_dict["state"] is "error": content += self.get_erroneous_entries_for_log() else: content += self.tr("cpu_usage_mean") + ": " + prepare_number_for_representation(data_dict["cpu_usage_mean"]) \ + " " + self.tr("cpu_usage_mean_unit") + " - " content += self.tr("ram_usage_mean") + ": " + prepare_number_for_representation(data_dict["ram_usage_mean"]) \ + " " + self.tr("ram_usage_mean_unit") + " - " content += self.tr("cpu_temp_mean") + ": " + prepare_number_for_representation(data_dict["cpu_temp_mean"]) \ + " " + self.tr("cpu_temp_mean_unit") return content
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 dicttotraj(dic): rt = RobotTrajectory() rt.joint_trajectory.joint_names = dic["joint_names"] for p in dic["points"]: jtp = JointTrajectoryPoint() jtp.positions = p["positions"] jtp.time_from_start = Duration(p["time_from_start"]) rt.joint_trajectory.points.append(jtp) return rt
def numpy_to_trajectory(trajectory, joint_names, duration): rt = RobotTrajectory() rt.joint_trajectory.joint_names = joint_names for point_idx, point in enumerate(trajectory): time = point_idx * duration / float(len(trajectory)) jtp = JointTrajectoryPoint(positions=map(float, point), time_from_start=Duration(time)) rt.joint_trajectory.points.append(jtp) return rt
def convert_points_dict(points: dict): """Convert the new point dictionary structure to the old structure""" for point in points: nanoseconds = point["time_from_start"] duration = Duration(nsecs=nanoseconds) point["time_from_start"] = { "secs": duration.secs, "nsecs": duration.nsecs }
def __init__(self, *args, **kwargs): self.bezier_curve = None self.duration = None if args: if len(args) == 1 and type(args[0]) is BezierPathMsg: self.bezier_curve = BezierCurve(args[0].order, args[0].control_points) self.duration = args[0].duration.data elif len(args) == 2: if type(args[0]) is BezierCurve: self.bezier_curve = args[0] elif type(args[0]) is list: self.bezier_curve = BezierCurve(args[0]) else: raise ValueError(f'Unknown argument {args[0]!r}') if type(args[1]) is Duration: self.duration = args[1] elif type(args[1]) is float: self.duration = Duration(args[1]) else: raise ValueError(f'Unknown argument {args[1]!r}') else: raise ValueError(f'Unknown arguments {args!r}') if kwargs: for k, v in kwargs.items(): if k == 'msg': if type(v) is not BezierPathMsg: raise ValueError( f'{k!r} must be {BezierPathMsg}, got {type(v)}') self.bezier_curve = v.bezier_curve self.duration = v.duration elif k == 'bezier_curve': if type(v) is not BezierCurve: raise ValueError( f'{k!r} must be {BezierCurve}, got {type(v)}') self.bezier_curve = v elif k == 'duration': if type(v) is not Duration: raise ValueError( f'{k!r} must be {Duration}, got {type(v)}') self.duration = v else: raise ValueError(f'Unknown keyword argument {k!r}')
def calculate_error_from_tag(self): # Calculate_TF and get Pose Error if self.can_transform() and self.marker_detected: # Get the most recent TF's tf_ = self.tf.lookup_transform(self.ar_pose_frame, self.drone_camera_frame, Time(0), Duration(1.0)) h_tf = self.tf.lookup_transform(self.drone_base_footprint, self.drone_base_link, Time(0), Duration(1.0)) # The Height that Drone is currently fly height = h_tf.transform.translation.z # Calculate Yaw from TF (_, _, yaw) = euler_from_quaternion([ tf_.transform.rotation.x, tf_.transform.rotation.y, tf_.transform.rotation.z, tf_.transform.rotation.w ]) yaw += pi # Manipulate Yaw to Rotate Drone in the Right Direction if np.rad2deg(yaw) > 180: yaw -= 2 * pi # Get the Rotation Matrix (r1, r2, r3, _) = quaternion_matrix([ tf_.transform.rotation.x, tf_.transform.rotation.y, tf_.transform.rotation.z, tf_.transform.rotation.w ]) rotation_mat = np.asmatrix([r1[0:3], r2[0:3], r3[0:3]]) # Get Position in Matrix Form pose_mat = np.asmatrix([[tf_.transform.translation.x], [tf_.transform.translation.y], [tf_.transform.translation.z]]) # Calculate the Cartesian Error error = rotation_mat * pose_mat return [error.item(0), error.item(1), error.item(2), yaw, height] else: return [-1, -1, -1, -1, -1]
def join_tree_srv_function(self, command): if self.debug: print('\x1b[33;1m' + str('Joining the Two Separate Frame Tree!') + '\x1b[0m') # Set Flag to True self.join_trees = command.status # Check if we can calculate the TF Between Drones Bottom Camera and Ar Tag while not self.tf_listen.can_transform( self.drone_odom_frame, self.map_frame, Time(0), Duration(1.0)): pass # Return From Service Once Tree is Joint return True
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 to_ros_markers(shape, stamp, frame_name, X_FG, color_rgba): assert isinstance(shape, Shape), shape assert isinstance(frame_name, str), frame_name assert isinstance(X_FG, RigidTransform), X_FG marker = Marker() marker.header.stamp = stamp marker.header.frame_id = frame_name marker.pose = to_ros_pose(X_FG) marker.action = Marker.ADD marker.lifetime = Duration(0.) marker.frame_locked = True def use_color(): nonlocal color_rgba if color_rgba is None: color_rgba = DEFAULT_RGBA (marker.color.r, marker.color.g, marker.color.b, marker.color.a) = (color_rgba.r(), color_rgba.g(), color_rgba.b(), color_rgba.a()) if type(shape) == Box: marker.type = Marker.CUBE marker.scale.x, marker.scale.y, marker.scale.z = shape.size() use_color() return [marker] if type(shape) == Sphere: marker.type = Marker.SPHERE marker.scale.x = shape.radius() marker.scale.y = shape.radius() marker.scale.z = shape.radius() use_color() return [marker] elif type(shape) == Cylinder: marker.type = Marker.CYLINDER marker.scale.x = shape.radius() marker.scale.y = shape.radius() marker.scale.z = shape.length() use_color() return [marker] elif type(shape) in (Mesh, Convex): marker.type = Marker.MESH_RESOURCE marker.mesh_resource = f"file://{shape.filename()}" # TODO(eric.cousineau): Is this the correct way to handle color vs. # texture? use_color() marker.mesh_use_embedded_materials = True marker.scale.x, marker.scale.y, marker.scale.z = 3 * [shape.scale()] return [marker] else: # TODO(eric): Support Capsule, Ellipse, all dat jazz. assert False, f"Unsupported type: {shape}"
def pose_error(self, x, y, z): target_to_map = PoseStamped() target_to_map.pose.position.x = x target_to_map.pose.position.y = y target_to_map.pose.position.z = z transform = self.tf.lookup_transform(self.drone_base_link, self.map_frame, Time(0), Duration(1.0)) error = do_transform_pose(target_to_map, transform) return error.pose.position.x, error.pose.position.y, error.pose.position.z
def __init__(self): # TF Listener self.tf = Buffer() self.tf_listener = TransformListener(self.tf) # Flags self.initialize = False self.marker_detected = False # True if Drone Camera has detected a Tag self.recovery_behavior = False # Used in recovery() Function if Marker is Lost # PID Parameters self.last_t = 0.0 self.lin_i_x = 0.0 self.lin_i_y = 0.0 self.lin_i_z = 0.0 self.rot_i_z = 0.0 # Gains ( with Initial Values) self.kp = 0.3 self.ki = 0.01 self.kd = 0.06 self.r_kp = 3.0 self.r_ki = 1.5 self.r_kd = 0.57 self.r_kp_s = 0.6 self.r_ki_s = 0.13 self.r_kd_s = 0.07 self.error = 0.25 self.max_speed = 1.0 self.max_z_speed = 1.5 # Parameter from YAML File self.drone_base_link = get_param('drone_base_link') self.drone_base_footprint = get_param('drone_base_footprint') self.map_frame = get_param('map_frame') self.ar_pose_frame = get_param('ar_pose_frame') self.drone_camera_frame = get_param('drone_camera_frame') self.drone_speed_topic = get_param('drone_speed_topic') self.ar_pose_topic = get_param('ar_pose_topic') self.time_to_considered_marker_lost = get_param( 'time_to_considered_marker_lost') # Parameters self.last_time_marker_seen = 0.0 # Hold the time that Tag last seen, for Setting Marker Detected flag ON/OFF # Ar Marker Topic Subscriber self.marker_subscriber = Subscriber('/ar_pose_marker', ARMarker, self.marker_cb) # Timer for Checking if Marker is Lost self.check_marker_timer = Timer(Duration(0.05), self.check_marker_existence)
def main(): rospy.init_node("rudder_and_sail_angle_publisher", anonymous=True) rospy.Subscriber("/desired_heading_degrees", heading, desiredHeadingCallBack) rospy.Subscriber("/windSensor", windSensor, windSensorCallBack) rospy.Subscriber("/GPS", GPS, gpsCallBack) rospy.Subscriber("/min_voltage", Float32, minVoltageCallBack) rospy.Subscriber('/lowWindConditions', Bool, lowWindCallBack) # Callbacks to update rudders and winches Timer(Duration(sailbot_constants.RUDDER_UPDATE_PERIOD), updateRudders) Timer(Duration(sailbot_constants.WINCH_UPDATE_PERIOD), updateWinches) # Callback to publish Timer(Duration(sailbot_constants.PUBLISHING_PERIOD), publishRudderWinchAngle) if (FIXED_CTRL_STATE is not None) and (DISABLE_LOW_POWER): rospy.logerr( "Cannot disable low power and fix the control state at the same time!" ) rospy.signal_shutdown( "Low power mode is disabled and the control state is fixed.") else: if DISABLE_LOW_POWER: rospy.logwarn( "Low power mode is DISABLED! If you don't want this, change DISABLE_LOW_POWER to False.\n" ) elif FIXED_CTRL_STATE is not None: rospy.logwarn( "The controller is fixed to the {} state! ".format( CONTROL_MODES[FIXED_CTRL_STATE]) + "If you don't want this, change FIXED_CTRL_STATE to None.") rospy.loginfo( "Boat controller started successfully. Waiting on sensor data...\n" ) rospy.spin()
def publish_laser_scan(self, collisions, publisher, color): marker = Marker() marker.header.frame_id = "/map" marker.type = marker.POINTS marker.action = marker.ADD marker.lifetime = Duration(secs=1) marker.pose.orientation.w = 1 marker.points = map(self.createpoint, collisions) marker.scale.x = 0.02 marker.scale.y = 0.02 marker.color.a = 1.0 marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] publisher.publish(marker)
def make_song(config, midi_file): song_builder = MidiSongBuilder(config) MidiInFile(song_builder, midi_file).read() print '' song = song_builder.song song.sort(key=lambda n: n[0]) print song, '\n' # song = strip_leading_silence(song) # print song, '\n' rs = Song() for note in song: key = str(config['keys'][note[1]]) rs.song.append(Note(time=Duration(note[0]), key=key)) return rs
def reset_pose(self): # Creates a goal to send to the action server. goal = JointTrajectoryGoal() # http://doc.aldebaran.com/2-5/family/pepper_technical/bodyparts_pep.html goal.trajectory.joint_names = self.body_parts angle_list_deg = [ 85.0, 10.0, -70.0, -20.0, -40.0, 85.0, -10.0, 70.0, 20.0, 40.0, -2.0, -5.0, 2.0, 0.0, 0.0 ] angle_list_rad = map(self.deg_to_rad, angle_list_deg) goal.trajectory.points = [] goal.trajectory.points.append( JointTrajectoryPoint(time_from_start=Duration(4.0), positions=angle_list_rad)) self.movement(goal)
def __init__(self, map_frame, odom_frame, projection_namespace, kitchen_namespace): # Frame names of the map and odom frame self.map_frame = map_frame self.odom_frame = odom_frame # Namespaces self.projection_namespace = projection_namespace self.kitchen_namespace = kitchen_namespace # TF tf_stampeds and static_tf_stampeds of the Robot in the BulletWorld: # These are initialized with the function init_transforms_from_urdf and are # used to update the local transformer with the function update_local_transformer_from_btr self.tf_stampeds = [] self.static_tf_stampeds = [] self.local_transformer = TransformerROS(interpolate=True, cache_time=Duration(10.0)) self.init_local_tf_with_tfs_from_urdf()
def generate_trajectory(self, randomness=1e-10, duration=-1): """ Generate a new trajectory from the given demonstrations and parameters :param duration: Desired duration, auto if duration < 0 :return: the generated RobotTrajectory message """ trajectory_array = self.promp.generate_trajectory(randomness) rt = RobotTrajectory() rt.joint_trajectory.joint_names = self.joint_names duration = float(self.mean_duration) if duration < 0 else duration for point_idx, point in enumerate(trajectory_array): time = point_idx * duration / float(self.num_points) jtp = JointTrajectoryPoint(positions=map(float, point), time_from_start=Duration(time)) rt.joint_trajectory.points.append(jtp) return rt
def createMarker(self, color, points): marker = Marker() marker.header.frame_id = "/map" marker.type = marker.POINTS marker.action = marker.ADD marker.lifetime = Duration(secs=20) marker.pose.orientation.w = 1 marker.points = points marker.scale.x = 0.02 marker.scale.y = 0.02 if color: marker.color = color else: marker.color.a = 1.0 marker.color.r = 0 marker.color.g = 255 marker.color.b = 0 return marker
def mark(positions: list, colour: MarkerColour = DEFAULT_COLOUR, scale: float = DEFAULT_SCALE, duration: float = DEFAULT_LIFETIME, channel: MarkerPublisherChannel = DEFAULT_MARKER_CHANNEL, marker_type: MarkerType = DEFAULT_MARKER_TYPE): """ Spawn a marker at given location and pass data to the "points" part of the marker. """ marker = Marker() marker.header.frame_id = FRAME_ID marker.type = marker_type.value marker.action = MARKER_ADD_ACTION # Scale for points is uniform within each direction for points, or handles line width for line-type markers marker.scale.x = scale if marker_type == MarkerType.POINTS: marker.scale.y = scale # Colour should be passed by the caller marker.color.r = colour.r marker.color.g = colour.g marker.color.b = colour.b marker.color.a = 1.0 # Position should be passed by the caller marker.pose.orientation.w = 1.0 # When should the marker disappear marker.lifetime = Duration(duration) for position_x, position_y in positions: point = Point() point.x, point.y, point.z = position_x, position_y, 0 marker.points.append(point) # Add first point again to close the polygon if marker_type == MarkerType.LINE_STRIPS: point = Point() position_x, position_y = positions[0] point.x, point.y, point.z = position_x, position_y, 0 marker.points.append(point) channel.value.publish(marker)
def __init__(self, model, id, parent): self.model = model self._id = id self.parent = parent self.model_name = "{}_{}".format(self.model, self._id) self.ns = "/{}".format(self.model_name) self.vel_pub = Publisher(self.ns + "/mobile_base/commands/velocity", Twist, queue_size=5) self.state_pub = Publisher("/gazebo/set_model_state", ModelState, queue_size=2) self.tf_listener = TransformListener(False, Duration(5.0)) self.init_service_proxies() self.start_tf_broadcaster() self.t_scale_f = self.parent.t_scale_f self.r_scale_f = self.parent.r_scale_f self.act_low = np.array([-0.1, -np.pi / 4]) self.act_high = np.array([0.25, np.pi / 4])
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 joint_angle_client(): #inhibitWalkSrv = rospy.ServiceProxy("inhibit_walk", std_srvs.srv.Empty) #uninhibitWalkSrv = rospy.ServiceProxy("uninhibit_walk", std_srvs.srv.Empty) client = actionlib.SimpleActionClient("joint_trajectory", naoqi_bridge_msgs.msg.JointTrajectoryAction) stiffness_client = actionlib.SimpleActionClient("joint_stiffness_trajectory", naoqi_bridge_msgs.msg.JointTrajectoryAction) angle_client = actionlib.SimpleActionClient("joint_angles_action", naoqi_bridge_msgs.msg.JointAnglesWithSpeedAction) rospy.loginfo("Waiting for joint_trajectory and joint_stiffness servers...") client.wait_for_server() stiffness_client.wait_for_server() angle_client.wait_for_server() rospy.loginfo("Done.") #inhibitWalkSrv() try: goal = naoqi_bridge_msgs.msg.JointTrajectoryGoal() # move head: single joint, multiple keypoints goal.trajectory.joint_names = ["HeadYaw"] goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.0), positions = [1.0])) goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(2.0), positions = [-1.0])) goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(2.5), positions = [0.0])) rospy.loginfo("Sending goal...") client.send_goal(goal) client.wait_for_result() result = client.get_result() rospy.loginfo("Results: %s", str(result.goal_position.position)) # Test for preemption rospy.loginfo("Sending goal again...") client.send_goal(goal) rospy.sleep(0.5) rospy.loginfo("Preempting goal...") client.cancel_goal() client.wait_for_result() if client.get_state() != GoalStatus.PREEMPTED or client.get_result() == result: rospy.logwarn("Preemption does not seem to be working") else: rospy.loginfo("Preemption seems okay") # crouching pose: single keypoint, multiple joints: goal.trajectory.joint_names = ["Body"] point = JointTrajectoryPoint() point.time_from_start = Duration(1.5) point.positions = [0.0,0.0, # head 1.545, 0.33, -1.57, -0.486, 0.0, 0.0, # left arm -0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # left leg -0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # right leg 1.545, -0.33, 1.57, 0.486, 0.0, 0.0] # right arm goal.trajectory.points = [point] rospy.loginfo("Sending goal...") client.send_goal(goal) client.wait_for_result() rospy.loginfo("Getting results...") result = client.get_result() print "Result:", ', '.join([str(n) for n in result.goal_position.position]) # multiple joints, multiple keypoints: goal.trajectory.joint_names = ["HeadYaw", "HeadPitch"] goal.trajectory.points = [] goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5), positions = [1.0, 1.0])) goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.0), positions = [1.0, 0.0])) goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.5), positions = [0.0, 0.0])) rospy.loginfo("Sending goal...") client.send_goal(goal) client.wait_for_result() rospy.loginfo("Getting results...") result = client.get_result() print "Result:", ', '.join([str(n) for n in result.goal_position.position]) # multiple joints, single keypoint: goal.trajectory.joint_names = ["HeadYaw", "HeadPitch"] goal.trajectory.points = [] goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5), positions = [0.5, 0.5])) rospy.loginfo("Sending goal...") client.send_goal(goal) client.wait_for_result() rospy.loginfo("Getting results...") result = client.get_result() print "Result:", ', '.join([str(n) for n in result.goal_position.position]) # Control of joints with relative speed angle_goal = naoqi_bridge_msgs.msg.JointAnglesWithSpeedGoal() angle_goal.joint_angles.relative = False angle_goal.joint_angles.joint_names = ["HeadYaw", "HeadPitch"] angle_goal.joint_angles.joint_angles = [1.0, 0.0] angle_goal.joint_angles.speed = 0.2 rospy.loginfo("Sending joint angles goal...") angle_client.send_goal_and_wait(angle_goal) result = angle_client.get_result() rospy.loginfo("Angle results: %s", str(result.goal_position.position)) # Test for preemption angle_goal.joint_angles.joint_angles = [-1.0, 0.0] angle_goal.joint_angles.speed = 0.05 rospy.loginfo("Sending goal again...") angle_client.send_goal(angle_goal) rospy.sleep(0.5) rospy.loginfo("Preempting goal...") angle_client.cancel_goal() angle_client.wait_for_result() if angle_client.get_state() != GoalStatus.PREEMPTED or angle_client.get_result() == result: rospy.logwarn("Preemption does not seem to be working") else: rospy.loginfo("Preemption seems okay") # Test stiffness actionlib stiffness_goal = naoqi_bridge_msgs.msg.JointTrajectoryGoal() stiffness_goal.trajectory.joint_names = ["Body"] stiffness_goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5), positions = [1.0])) rospy.loginfo("Sending stiffness goal...") stiffness_client.send_goal(stiffness_goal) stiffness_client.wait_for_result() result = stiffness_client.get_result() rospy.loginfo("Stiffness results: %s", str(result.goal_position.position)) # Test for preemption stiffness_goal.trajectory.points = [JointTrajectoryPoint(time_from_start = Duration(0.5), positions = [0.0])] rospy.loginfo("Sending goal again...") stiffness_client.send_goal(stiffness_goal) rospy.sleep(0.25) rospy.loginfo("Preempting goal...") stiffness_client.cancel_goal() stiffness_client.wait_for_result() if stiffness_client.get_state() != GoalStatus.PREEMPTED or stiffness_client.get_result() == result: rospy.logwarn("Preemption does not seem to be working") else: rospy.loginfo("Preemption seems okay") finally: pass #uninhibitWalkSrv()
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 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 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