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
