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 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 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 _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 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,
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 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 __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 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 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 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 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 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 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 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 __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, *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 __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 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 execute_cb_cf3_follow_cf2(self, goal):
#"""MOVES DRONE goal.id TO POSE goal.point"""
#"""detect_perimeter1 ACTION SERVER [name='detect_perimeter1', drone: goal.id, variable:'_cf3_follow_cf2', callback:'execute_cb_cf3_follow_cf2']"""
#speak (engine, "Moving to point.")
#rospy.wait_for_message('/tf', tf2_msgs.msg.TFMessage, timeout=None)
self.PoseListener()
print ("FOLLOW callback")
print ("point is", goal.point)
print("id is " + str(goal.id))
self._feedback_cf3.position = Pose()
self._feedback_cf3.time_elapsed = Duration(5)
self.success_cf3 = False
y_offset = 0
x_offset = -0.3
speak (engine, "YO")
# for cf in self.allcfs.crazyflies:
# if cf.id == goal.id:
# print("send COMMANDS to cf...", goal.id)
# #cf.takeoff(0.5, 5.0)
# #cf.goTo(self.waypoint, yaw=0, duration=2.0)
while self.success_cf3 == False:
self.PoseListener()
print("in false loop...")
if self._as_cf3.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name_cf3)
self._as_cf3.set_preempted()
for cf in self.allcfs.crazyflies:
if cf.id == goal.id:
print("LANDING cf...", goal.id)
cf.land(0.04, 2.5)
break
print ("FOLLOW Not yet...")
self.waypoint = np.array([self.cf2_pose.x + x_offset, self.cf2_pose.y + y_offset, self.cf2_pose.z])
print("waypoint is", self.waypoint)
for cf in self.allcfs.crazyflies:
if cf.id == goal.id:
#print("TRYING TO REACH GOAL...", goal.id)
#cf.takeoff(0.5, 5.0)
cf.goTo(self.waypoint, yaw=0, duration=self.justenoughsleeptime)
#rospy.sleep(self.justenoughsleeptime)
self.stayclose_transition() #STOP WHEN HAND SIGN APPEARS.
if self.success_cf3 == True:
print("Reached the perimeter!!")
self.success_cf3 = False
self._result_cf3.time_elapsed = Duration(5)
self._result_cf3.updates_n = 1
rospy.loginfo('My feedback: %s' % self._feedback_cf3)
rospy.loginfo('%s: Succeeded' % self._action_name_cf3)
self._as_cf3.set_succeeded(self._result_cf3)
def execute_cb_cf4_go(self, goal):
#"""MOVING cf2 TO GOAL goal.point
#detectperimeter1 ACTION SERVER [name='cf4_go', drone: cf4, variable:'_cf4_go', callback:'execute_cb_cf4_go']"""
#speak (engine, "Moving to point.")
self.PoseListener()
print ("CF2 Action Server callback")
print ("point is", goal.point)
print("id is " + str(goal.id))
self._feedback_cf4_go.position = Pose()
self._feedback_cf4_go.time_elapsed = Duration(5)
self.success_cf4 = False
self.enable_cf4 = True
self.waypoint = np.array([goal.point.x, goal.point.y, goal.point.z])
#speak (engine, "YO")
for cf in self.allcfs.crazyflies:
if cf.id == goal.id:
print("send COMMANDS to cf...", goal.id)
self._feedback_cf4_go.position.position.x = cf.position()[0]
self._feedback_cf4_go.position.position.y = cf.position()[1]
self._feedback_cf4_go.position.position.z = cf.position()[2]
for cf in self.allcfs.crazyflies:
if self.enable_cf4 == True: #ENABLE
if cf.id == goal.id:
#print("drone moves.")
cf.goTo(self.waypoint, yaw=0, duration=3.0)
#print("drone pose is", self.cf3_pose)
#self.enable_cfx == False
while self.success_cf4 == False:
self.PoseListener()
print ("point is", goal.point)
print("id is " + str(goal.id))
if self._as_cf4_go.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name_cf4_go)
self._as_cf4_go.set_preempted()
for cf in self.allcfs.crazyflies:
if cf.id == goal.id:
print("LANDING cf...", goal.id)
cf.land(0.04, 2.5)
break
print ("Not yet...")
#now we test if he has reached the desired point.
self.takeoff_transition(goal.id, goal.point)
try:
# publish the feedback
self._as_cf4_go.publish_feedback(self._feedback_cf4_go)
except rospy.ROSInterruptException:
rospy.loginfo("except clause opened")
pass
if self.success_cf4 == True:
#for cf in self.allcfs.crazyflies:
#print(cf.id)
#print("press button to continue")
#self.swarm.input.waitUntilButtonPressed()
#cf.land(0.04, 2.5)
print("Reached the perimeter!!")
self.success_cf4 = False
self._result_cf4_go.time_elapsed = Duration(5)
self._result_cf4_go.updates_n = 1
rospy.loginfo('My feedback: %s' % self._feedback_cf4_go)
rospy.loginfo('%s: Succeeded' % self._action_name_cf4_go)
self._as_cf4_go.set_succeeded(self._result_cf4_go)
def functional(axis, angle):
pub_rosey = rospy.Publisher(
'/cyton_joint_trajectory_action_controller/command',
JointTrajectory, queue_size=10)
rospy.init_node('traj_maker', anonymous=True)
time.sleep(1)
axis = int(axis)
angle = float(angle)
rate = rospy.Rate(0.01)
while not rospy.is_shutdown():
# first way to define a point
traj_waypoint_1_rosey = JointTrajectoryPoint()
traj_waypoint_1_rosey.positions = [0,0,0,0,0,0,0]
traj_waypoint_1_rosey.time_from_start = Duration(2)
# second way to define a point
traj_waypoint_2_rosey = traj_waypoint_1_rosey
traj_waypoint_2_rosey.time_from_start = Duration(4)
traj_waypoint_2_rosey.positions[axis-1] = angle
time.sleep(1)
traj_waypoint_3_rosey = traj_waypoint_2_rosey
traj_waypoint_3_rosey.time_from_start = Duration(7)
#traj_waypoint_2_rosey = JointTrajectoryPoint(positions=[.31, -.051, .33, -.55, .28, .60,0],
#time_from_start = Duration(4))
#traj_waypoint_3_rosey = JointTrajectoryPoint(positions=[.14726, -.014151, .166507, -.33571, .395997, .38657,0],
#time_from_start = Duration(6))
#traj_waypoint_4_rosey = JointTrajectoryPoint(positions=[-.09309, .003150, .003559, .16149, .524427, -.1867,0],
#time_from_start = Duration(8))
#traj_waypoint_5_rosey = JointTrajectoryPoint(positions=[-.27752, .077886, -.1828, .38563, .682589, -.44665,0],
#time_from_start = Duration(10))
#traj_waypoint_6_rosey = JointTrajectoryPoint(positions=[0,0,0,0,0,0,0],time_from_start = Duration(12))
#debug in terminal
print traj_waypoint_2_rosey.positions
# making message
message_rosey = JointTrajectory()
# required headers
header_rosey = std_msgs.msg.Header(stamp=rospy.Time.now())
message_rosey.header = header_rosey
# adding in joints
joint_names = ['shoulder_roll_joint', \
'shoulder_pitch_joint', 'shoulder_yaw_joint', 'elbow_pitch_joint', \
'elbow_yaw_joint', 'wrist_pitch_joint', 'wrist_roll_joint']
message_rosey.joint_names = joint_names
# appending up to 100 points
# ex. for i in enumerate(len(waypoints)): append(waypoints[i])
message_rosey.points.append(traj_waypoint_1_rosey)
message_rosey.points.append(traj_waypoint_2_rosey)
message_rosey.points.append(traj_waypoint_3_rosey)
#message_rosey.points.append(traj_waypoint_4_rosey)
#message_rosey.points.append(traj_waypoint_5_rosey)
#message_rosey.points.append(traj_waypoint_6_rosey)
# publishing to ROS node
pub_rosey.publish(message_rosey)
rate.sleep()
if rospy.is_shutdown():
break
