def readInPoses(self):
poses = rospy.get_param('~poses')
rospy.loginfo("Found %d poses on the param server", len(poses))
for key, value in poses.iteritems():
try:
# TODO: handle multiple points in trajectory
trajectory = JointTrajectory()
trajectory.joint_names = value["joint_names"]
point = JointTrajectoryPoint()
point.time_from_start = Duration(value["time_from_start"])
point.positions = value["positions"]
trajectory.points = [point]
self.poseLibrary[key] = trajectory
except:
rospy.logwarn(
"Could not parse pose \"%s\" from the param server:", key)
rospy.logwarn(sys.exc_info())
# add a default crouching pose:
if not "crouch" in self.poseLibrary:
trajectory = JointTrajectory()
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
trajectory.points = [point]
self.poseLibrary["crouch"] = trajectory
def __init__(self,
topic='motion/controller/look_at/in_pose_ref',
duration=120,
interval=[3, 5],
maxXYZ=[1, 0.3, 0.5],
minXYZ=[1.0, -0.3, 0.0],
frame_xyz='base_link',
frame_out='odom_combined'):
super(RandPoseGenerator, self).__init__(outcomes=['done', 'failure'])
# Store topic parameter for later use.
if not isinstance(topic, str):
raise ValueError("Topic name must be string.")
if not isinstance(duration, (int, float)) or duration <= 0:
raise ValueError("Duration must be positive number.")
if len(interval) != 2 or any([
not isinstance(t, (int, float)) for t in interval
]) or any([t < 0 for t in interval]) or interval[0] > interval[1]:
raise ValueError("Interval must represent interval of time.")
if len(minXYZ) != 3 or any(
[not isinstance(v, (int, float)) for v in minXYZ]):
raise ValueError("minXYZ: list of three numbers was expected.")
if len(maxXYZ) != 3 or any(
[not isinstance(v, (int, float)) for v in maxXYZ]):
raise ValueError("maxXYZ: list of three numbers was expected.")
if not isinstance(frame_xyz, str) or not isinstance(frame_out, str):
raise ValueError("Frame names must be string.")
self._topic = topic
self._duration = Duration.from_sec(duration)
self._interval = interval
self._minXYZ = minXYZ
self._maxXYZ = maxXYZ
self._frame_in = frame_xyz
self._frame_out = frame_out
# create proxies
self._publisher = ProxyPublisher({self._topic: PoseStamped})
self._tf_listener = ProxyTransformListener().listener()
self._tf_listener.waitForTransform(self._frame_out, self._frame_in,
rospy.Time(), rospy.Duration(1))
if not self._tf_listener.canTransform(self._frame_out, self._frame_in,
rospy.Time(0)):
raise ValueError(
"Unable to perform transform between frames %s and %s." %
(self._frame_out, self._frame_in))
# state
self._start_timestamp = None
self._movement_timestamp = None
self._movement_duration = Duration()
# error in enter hook
self._error = False
def write_with_offset(outbag, topic, msg, t, offset_sec):
def add_offset(t, offset):
# ignore those which are not unix timestamps
return t + offset if is_unix_time(t.to_sec()) else t
try:
offset = Duration.from_sec(offset_sec)
stamp = 0
if hasattr(msg, 'header'):
msg.header.stamp = add_offset(msg.header.stamp, offset)
elif hasattr(msg, 'stamp'):
msg.stamp = add_offset(msg.stamp, offset)
elif hasattr(msg, 'transforms'):
for id,_ in enumerate(msg.transforms):
msg.transforms[id].header.stamp = add_offset(msg.transforms[id].header.stamp, offset)
# for RealSense metadata
elif hasattr(msg, 'value'):
try:
value = int(msg.value)
if is_unix_time(float(value) / 1000):
msg.value = str(value + int(offset_sec * 1000))
except ValueError:
try:
value = float(msg.value)
if is_unix_time(value / 1000):
msg.value = '%.6f' % (value + offset_sec * 1000)
except ValueError:
pass
except TypeError:
print (str(msg))
print ('Cannot process above message: topic=%s, t=%.9f, offset=%.9f' % (topic, t.to_sec(), offset_sec))
exit()
else:
t = add_offset(t, offset)
outbag.write(topic, msg, t)
def handle_camera_direction(self, subject):
command = JointTrajectory()
command.joint_names = ["head_2_joint", "head_1_joint"]
point1 = JointTrajectoryPoint()
#point2 = JointTrajectoryPoint()
point1.velocities = [0.0, 0.0]
#point2.velocities = [0]
point1.time_from_start = Duration(2.0, 0.0)
rospy.loginfo(subject)
if subject[0] == 'up':
self.head_ud = self.head_ud + self.head_move_step_size
if subject[0] == 'down':
self.head_ud = self.head_ud - self.head_move_step_size
if subject[0] == 'left':
self.head_lr = self.head_lr + self.head_move_step_size
if subject[0] == 'right':
self.head_lr = self.head_lr - self.head_move_step_size
rospy.loginfo(self.head_ud)
point1.positions = [self.head_ud, self.head_lr]
command.points = [point1]
self.pub_head.publish(command)
return
def __init__(self,
controller='motion/controller/joint_state_head',
tolerance=0.17,
timeout=10.0,
joint_topic="joint_states",
outcomes=['done', 'failed', 'timeout']):
super(SetJointStateBase, self).__init__(outcomes=outcomes)
# Store topic parameter for later use.
self._controller = controller
self._joint_topic = joint_topic
self._tolerance = tolerance
self._timeout = Duration.from_sec(timeout)
# create proxies
self._action_client = ProxyActionClient(
{self._controller: SetOperationalAction})
self._pose_publisher = ProxyPublisher(
{self._controller + '/in_joints_ref': JointState})
self._pose_subscriber = ProxySubscriberCached(
{self._joint_topic: JointState})
# timestamp
self._timestamp = None
# error in enter hook
self._error = False
def velocitiesPublisher(output):
#Newly added
global arm_current
freq = 20
r = rospy.Rate(freq)
# ROS publishers
duration = Duration(nsecs=1e9 / float(freq) * 3.0)
arm_pub = rospy.Publisher('/arm_controller/safe_command',
JointTrajectory,
queue_size=1)
arm_msg = JointTrajectory()
correction_velocities = Vector3()
formatOutput(output, correction_velocities)
print('correction_velocities[] ', correction_velocities)
joint_angles_6 = correction_velocities.z
joint_angles_7 = correction_velocities.x
arm_pub.publish(correction_velocities)
#arm_msg.joint_names = ["arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint", "arm_6_joint",
# "arm_7_joint"]
arm_msg.joint_names = ["arm_6_joint", "arm_7_joint"]
arm_point = JointTrajectoryPoint()
arm_point.time_from_start = duration
arm_msg.points = [arm_point]
# Subscribers
arm_sub = rospy.Subscriber('/arm_controller/state',
JointTrajectoryControllerState, cb_arm_state)
def test_extrapolation(dir, m, pose1, pose2, service, time1, time_12):
# this stuff is done to test the accuracy of the extrapolation
# wait some time to get the expected position
rospy.sleep(Duration.from_sec(5))
resp = service("centroid,cuboid")
object3 = resp.objects[0].object
time3 = resp.stamp
pose3 = m.transform_to(object3)
# Get the duration between the first perception and now
time_13 = time3 - time1
# get the relation between the first and second duration
diff_time = float(time_13.to_sec()) / float(time_12.to_sec())
# table = [["stamps: ", time1, time2, time3], ["duration: ", time_12, time_13, diff_time]]
# print tabulate(table)
# calculate the new longer vector (scalar multiplication by hand)
dir_13 = numpy.array([diff_time * dir[0], diff_time * dir[1]])
# add this new vector on the points from the first perception
pose_comp = copy.deepcopy(pose1)
pose_comp.primitive_poses[0].position.x += dir_13[0]
pose_comp.primitive_poses[0].position.y += dir_13[1]
pose_comp.id = "red_cube"
print pose_comp
# print out the table for the different poses for debug
table = [get_position(pose1), get_position(pose2), get_position(pose3), get_position(pose_comp)]
print tabulate(table)
dev = get_position(pose3)[1] / get_position(pose_comp)[1] * 100
print dev
return pose_comp
def velocitiesPublisher(output):
#### newly added ####
global arm_current
#### newly added ####
# 6x1 matrix for end_effector work-space velocities
# Obtained by multiplying NPF with output
end_effector_velocities = NPS.dot(output)
correction_velocities = Vector3()
formatOutput(end_effector_velocities, correction_velocities)
#### newly added ####
# Frequency which TIAGo runs at
freq = 50
r= rospy.Rate(freq)
duration = Duration(nsecs-1e9/float(freq)*3.0)
# Publisher
arm_pub = rospy.Publisher('/arm_controller/safe_command', JointTrajectory, queue_size=1)
arm_msg = JointTrajectory()
# Test to send velocity commands to TIAGo joint 6 and joint 7
joint_angles_6 = correction_velocities.z
joint_angles_7 = correction_velocities.x
arm_pub.publish(correction_velocities)
arm_msg.joint_names = ["arm_6_joint", "arm_7_joint"]
arm_point = JointTrajectoryPoint()
arm_point.time_from_start = duration
arm_msg.points = [arm_point]
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("frequency") + ": " + prepare_number_for_representation(data_dict["frequency"]) \
+ " " + self.tr("frequency_unit") + " - "
content += self.tr("bandwidth") + ": " + prepare_number_for_representation(
data_dict["bandwidth"]) + " " \
+ self.tr("bandwidth_unit") + " - "
content += self.tr("dropped_msgs") + ": " + prepare_number_for_representation(data_dict["dropped_msgs"]) \
+ " " + self.tr("dropped_msgs_unit")
return content
def __init__(self,
controller='joint_state_head',
duration=120,
interval=[2.0, 4.0],
joints=[],
minimal=[],
maximal=[]):
super(RandJointsMovements, self).__init__(outcomes=['done', 'failed'],
input_keys=['config'])
# Store topic parameter for later use.
self._controller = 'motion/controller/' + controller
# create proxies
self._set_operational_caller = ProxyServiceCaller(
{self._controller + '/set_operational': SetBool})
self._publisher = ProxyPublisher(
{self._controller + '/in_joints_ref': JointState})
# state
self._start_timestamp = None
self._movement_timestamp = None
self._movement_duration = Duration()
# user input
self.set_movement_parameters(duration, interval, joints, minimal,
maximal)
# error in enter hook
self._error = False
def start(self):
if not self.__running:
self.__get_history()
self.__register_subscribers()
self.__running = True
self.__timer = Timer(Duration(nsecs=UPDATE_FREQUENCY),
self.__update_model)
def execute(self, userdata):
if self._error:
return 'failed'
# check if time elasped
if Time.now() - self._start_timestamp > self._duration:
return 'done'
# check if we have tosend new point
if Time.now() - self._movement_timestamp > self._movement_duration:
# determine new interval
self._movement_timestamp = Time.now()
self._movement_duration = Duration.from_sec(
random.uniform(*self._interval))
# form message
msg = JointState()
msg.header = Header(stamp=Time.now())
msg.name = ['joint52', 'joint53', 'eyes_pitch', 'eyes_yaw']
# random durection
x = random.uniform(0, 1)
y = random.uniform(0, 1)
# compute head position
msg.position = [0, 0, 0, 0]
msg.position[0] = self._min2356[0] * x + self._max2356[0] * (1 - x)
msg.position[1] = self._min2356[1] * y + self._max2356[1] * (1 - y)
# compute eyes position
msg.position[2] = self._min2356[2] * y + self._max2356[2] * (1 - y)
msg.position[3] = self._min2356[3] * x + self._max2356[3] * (1 - x)
# send message
try:
self._publisher.publish(self._controller + '/in_joints_ref',
msg)
except Exception as e:
Logger.logwarn('Failed to send JointState message `' +
self._topic + '`:\n%s' % str(e))
def __init__(self, outcomes = ['unknown'], pose_ns = 'saved_msgs/joint_state', tolerance = 0.17, joint_topic = "joint_states", timeout = 1.0):
# Process supplied pose list. Note it must be processed befor superclass constructor is called,
# beacuse it changes 'outcomes' list due to side effect.
# check if 'unknown' outcome is present
if 'unknown' not in outcomes:
raise RuntimeError, '"unknown" should presents in state outcomes'
# remove 'unknown' while preserving items order
poses_names = [ outcome for outcome in outcomes if outcome != 'unknown' ]
# Load poses from paramere server. Pose is loaded in form dict(joint name -> position),
# but they are stored as a list of tuples (pose_name, pose_dict) to preserve order.
self._poses = [ (name, self.load_joint_state(pose_ns, name)) for name in poses_names ]
# Call superclass constructor.
super(CheckJointState, self).__init__(outcomes = outcomes)
# Subscribe to the joint topic.
self._joint_topic = joint_topic
self._pose_subscriber = ProxySubscriberCached({ self._joint_topic: JointState })
# Store topic parameter for later use.
self._tolerance = tolerance
self._timeout = Duration.from_sec(timeout)
# timestamp
self._timestamp = None
def executeBodyPose(self, goal):
if not goal.pose_name in self.poseLibrary:
rospy.loginfo("Pose %s not in library, reload library from parameters..." % goal.pose_name)
self.readInPoses()
if goal.pose_name in self.poseLibrary:
rospy.loginfo("Executing body pose %s...", goal.pose_name);
if not self.stopWalkSrv is None:
self.stopWalkSrv()
trajGoal = JointTrajectoryGoal()
# time out one sec. after trajectory ended:
trajGoal.trajectory = self.poseLibrary[goal.pose_name]
timeout = trajGoal.trajectory.points[-1].time_from_start + Duration(1.0)
trajGoal.trajectory.header.stamp = rospy.Time.now()
# TODO: cancel goal after timeout is not working yet in nao_controller
self.trajectoryClient.send_goal_and_wait(trajGoal, timeout)
if self.trajectoryClient.get_state() != GoalStatus.SUCCEEDED:
self.poseServer.set_aborted(text="JointTrajectory action did not succeed (timeout?)");
self.poseServer.set_succeeded()
rospy.loginfo("Done.");
else:
msg = "pose \""+goal.pose_name+"\" not in pose_manager's library";
rospy.logwarn("%s", msg)
self.poseServer.set_aborted(text=str(msg));
def execute(self, userdata):
if self._error:
return 'failed'
# check if time elasped
if Time.now() - self._start_timestamp > self._duration:
return 'done'
# check if we have tosend new point
if Time.now() - self._movement_timestamp > self._movement_duration:
# determine new interval
self._movement_timestamp = Time.now()
self._movement_duration = Duration.from_sec(
random.uniform(*self._interval))
# form message
msg = JointState()
msg.header = Header(stamp=Time.now())
msg.name = self._joints
for i in range(0, len(self._joints)):
x = random.uniform(0, 1)
msg.position.append(self._minimal[i] * x + self._maximal[i] *
(1 - x))
# send message
try:
self._publisher.publish(self._controller + '/in_joints_ref',
msg)
except Exception as e:
Logger.logwarn('Failed to send JointState message `' +
self._topic + '`:\n%s' % str(e))
def __aggregate_topic_data(self, event):
"""
Aggregates the topic every TOPIC_AGGREGATION_FREQUENCY nsecs and pushes the updated data to
self.__calculated_data.
:param event: containing information when this method was called - not used but needed for the interface
"""
aggregated_data = {}
for key in self._attributes:
aggregated_data[key] = 0
for key in self.__calculated_data.keys():
self.__calculated_data[key].append(0)
child_count = 0
for connection in self.get_childs(
): # !assuming all childs are connection items!
values = connection.aggregate_data(
self.__aggregation_window) # average over N seconds
if values:
for key in self.add_keys:
aggregated_data[key] += values[key]
for key in self.max_keys:
if values[key] > aggregated_data[key]:
aggregated_data[key] = values[key]
for key in self.avg_keys:
aggregated_data[key] += values[key]
child_count += 1
for key in self.avg_keys:
if child_count == 0:
aggregated_data[key] = 0
else:
aggregated_data[key] /= child_count
self._data_lock.acquire()
for key in self._attributes:
self.__calculated_data[key][-1] = aggregated_data[key]
self.__calculated_data["window_start"][-1] = Time.now()
self.__calculated_data["window_stop"][-1] = Time.now() - (Duration(
secs=1) if int(Duration(
secs=1).to_sec()) <= int(Time.now().to_sec()) else Time(0))
self._data_lock.release()
def update_graphs(self, event):
"""
Updates and redraws the graphs.
"""
self.__update_graphs_lock.acquire()
if self.__draw_graphs or self.__first_update_pending:
plotable_items = self.__plotable_items[min(
self.__current_selected_combo_box_index *
self.__items_per_group, len(self.__plotable_items)):min(
(self.__current_selected_combo_box_index + 1) *
self.__items_per_group, len(self.__plotable_items))]
plotable_data = self.__model.get_root_item(
).get_items_younger_than(
Time.now() - (Duration(secs=self.__combo_box_index_to_seconds(
self.__current_range_combo_box_index)) if int(
Duration(secs=self.__combo_box_index_to_seconds(
self.__current_range_combo_box_index)).to_sec()) <=
int(Time.now().to_sec()) else Time(0)),
"window_stop", *plotable_items)
temp_time = []
temp_content = []
if plotable_data["window_stop"]:
modulo = (len(plotable_data["window_stop"]) / 200) + 1
length = len(plotable_data["window_stop"])
else:
length = 0
modulo = 1
for i in range(0, length, modulo):
# now having maximally 100 items to plot :)
temp_time.append(
int(str(plotable_data["window_stop"][i])) / 1000000000)
x = np.array(temp_time)
for key in plotable_items:
for i in range(0, length, modulo):
temp_content.append(plotable_data[key][i])
y = np.array(temp_content)
del temp_content[:]
self.__plotted_curves[key].setData(x=x, y=y)
self.__first_update_pending = False
self.__update_graphs_lock.release()
def timed_out(self):
data_dict = self.get_latest_data()
if data_dict["window_stop"] == Time(0):
return False
elif (Time.now() - data_dict["window_stop"]) > Duration(TIME_OUT):
# last entry was more than TIME_OUT, so it's offline
return True
return False
def _update_current_state(self):
"""
This method updates the current state of the AbstractItem.
:raises TypeError: at the initialization, it's possible that last_states["state"] has no entries and a TypeError occures
"""
if self.get_state():
if self.get_state() is not "error":
last_states = self.get_rated_items_younger_than(Time.now() - (
Duration(secs=WARNING_TIMEOUT) if int(Duration(secs=5).to_sec()) <= int(Time.now().to_sec()) else Time(0)),
"state")
try:
for i in range(0, len(last_states["state"])):
if last_states["state"][i] is "error":
self.set_state("warning")
break
except TypeError:
return
def __init__(self, logger, seuid, first_message, parent=None):
"""Initializes the TopicItem.
:param seuid: the seuid of the item
:type seuid: str
:param logger: a logger where to log when special events occur
:type logger: ModelLogger
:param parent: the parent-item
:type parent: AbstractItem
"""
AbstractItem.__init__(self, logger, seuid, parent)
self.__parent = parent
self._type = "topic"
self.add_keys = ["dropped_msgs", "traffic", "bandwidth", "frequency"]
self.avg_keys = [
"period_mean", "period_stddev", "stamp_age_mean",
"stamp_age_stddev"
]
self.max_keys = ["period_max", "stamp_age_max"]
self._attributes = []
self._attributes.extend([
"dropped_msgs", "traffic", "period_mean", "period_stddev",
"period_max", "stamp_age_mean", "stamp_age_stddev",
"stamp_age_max", "bandwidth", "frequency"
])
for item in self._attributes:
self._add_data_list(item)
self.__calculated_data = {}
for key in self._attributes:
self.__calculated_data[key] = []
self.__calculated_data["window_stop"] = []
self.__calculated_data["window_start"] = []
for item in self._attributes:
self._rated_attributes.append(item + ".actual_value")
self._rated_attributes.append(item + ".expected_value")
self._rated_attributes.append(item + ".state")
for item in self._rated_attributes:
self._add_rated_data_list(item)
self._logger.log("info", Time.now(), seuid, "Created a new TopicItem")
self.__timer = Timer(Duration(nsecs=TOPIC_AGGREGATION_FREQUENCY),
self.__aggregate_topic_data)
self.tree_items = []
self.__aggregation_window = rospy.get_param("~aggregation_window", 5)
### CARSON ADDED ###o
self.throttle = None
def aggregate_data(self, period):
"""
:param period: The amount in seconds over which the data should be aggregated.
:return:
"""
values = {}
for key in self._attributes:
values[key] = 0
entries = self.get_items_younger_than(Time.now() - (Duration(
secs=period) if int(Duration(secs=period).to_sec()
) <= int(Time.now().to_sec()) else Time(0)))
length = len(entries["window_stop"]) if entries["window_stop"] else 0
if length > 0:
for key in self.add_keys:
for i in range(0, length):
values[key] += entries[key][i]
for key in self.max_keys:
if type(entries[key][-1]) == genpy.rostime.Time or type(
entries[key][-1]) == genpy.rostime.Duration:
for i in range(0, length):
if entries[key][i].to_sec() > values[key]:
values[key] = entries[key][i].to_sec()
else:
for i in range(0, length):
if entries[key][i] > values[key]:
values[key] = entries[key][i]
for key in self.avg_keys:
if type(entries[key][0]) is genpy.rostime.Time or type(
entries[key][0]) is genpy.rostime.Duration:
for i in range(0, length):
values[key] += entries[key][i].to_sec()
else:
for i in range(0, length):
values[key] += entries[key][i]
values[key] = values[key] / length
return values
def make_msg(joints, points_deg, tolerance_deg, time_tolerance):
goal = control_msgs.msg.FollowJointTrajectoryGoal
goal.goal_time_tolerance = Duration(time_tolerance)
goal.path_tolerance = [
control_msgs.msg.JointTolerance(j, deg2rad(tolerance_deg), 0, 0)
for j in joints if j[:8] != 'support/'
]
goal.goal_tolerance = [
control_msgs.msg.JointTolerance(j, deg2rad(tolerance_deg), 0, 0)
for j in joints if j[:8] != 'support/'
]
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
points = [
trajectory_msgs.msg.JointTrajectoryPoint([deg2rad(p)
for p in pos], [], [], [],
Duration(time))
for time, pos in points_deg
]
goal.trajectory = trajectory_msgs.msg.JointTrajectory(
header, joints, points)
return goal
def update_graphs(self, event):
"""
Updates and redraws the graphs.
"""
self.__update_graphs_lock.acquire()
if self.__selected_item is not None:
if self.__draw_graphs or self.__first_update_pending:
#print("items per group: " + str(self.__items_per_group))
#print("combo index: " + str(self.__current_selected_combo_box_index))
#print("len plot " + str(len(self.__plotable_items)))
plotable_items = self.__plotable_items[min(self.__current_selected_combo_box_index *
self.__items_per_group, len(self.__plotable_items)):min((self.__current_selected_combo_box_index + 1)
* self.__items_per_group, len(self.__plotable_items))]
plotable_data = self.__selected_item.get_items_younger_than(
#Time.now() - Duration(secs=self.__combo_box_index_to_seconds(self.__current_range_combo_box_index)),
Time.now() - (Duration(secs=(self.__combo_box_index_to_seconds(self.__current_range_combo_box_index) + 10 )) if int(Duration(secs=self.__combo_box_index_to_seconds(self.__current_range_combo_box_index)).to_sec()) <= int(Time.now().to_sec()) else Time(0) ),
"window_stop", *plotable_items)
if "window_stop" in plotable_data:
if plotable_data["window_stop"]:
temp_time = []
temp_content = []
length = len(plotable_data["window_stop"])
modulo = (length / 200) + 1
for i in range(0, length, modulo):
# now having maximally 100 items to plot :)
temp_time.append(plotable_data["window_stop"][i].to_sec())
x = np.array(temp_time)
list_entries = self.__selected_item.get_list_items()
time_entries = self.__selected_item.get_time_items()
for key in plotable_items:
if key in list_entries:
pass
else:
if key in time_entries:
for i in range(0, length, modulo):
temp_content.append(float(str(plotable_data[key][i]))/1000000000)
else:
for i in range(0, length, modulo):
temp_content.append(plotable_data[key][i])
y = np.array(temp_content)
del temp_content[:]
self.__plotted_curves[key].setData(x=x, y=y)
else:
pass
self.__first_update_pending = False
self.__update_graphs_lock.release()
def get_short_data(self):
"""
Returns a shortend version of the item data.
:returns: data of the item
:rtype: str
"""
data_dict = {}
for key in self.__calculated_data:
if self.__calculated_data[key]:
data_dict[key] = self.__calculated_data[key][-1]
else:
data_dict[key] = self.tr("Currently no value available")
data_dict["window_stop"] = Time(0)
data_dict["window_start"] = Time(0)
data_dict["state"] = self.get_state()
try:
if type(data_dict["window_stop"]) == type(1):
data_dict["window_stop"] = Time(data_dict["window_stop"])
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!
print("test 2")
return "No data since " + prepare_number_for_representation(Time.now() - data_dict["window_stop"]) \
+ " seconds"
except Exception as e:
print(data_dict["window_stop"])
print(e)
raise UserWarning
content = ""
if data_dict["state"] is "error":
content += self.get_erroneous_entries_for_log()
else:
content += self.tr("frequency") + ": " + prepare_number_for_representation(
data_dict["frequency"]) + " " \
+ self.tr("frequency_unit") + " - "
content += self.tr("bandwidth") + ": " + prepare_number_for_representation(bytes_to_kb(data_dict["bandwidth"])) \
+ " " + self.tr("bandwidth_unit") + " - "
content += self.tr("dropped_msgs") + ": " + prepare_number_for_representation(data_dict["dropped_msgs"]) \
+ " " + self.tr("dropped_msgs_unit")
return content
def _find_pose(self, image_stamp: rospy.rostime.Time) -> Optional[geometry_msgs.msg.Pose]:
"""
Find the pose that has the smallest time difference with the image timestamp
:param image_stamp: an image time stamp
:return: a geometric pose if found, none otherwise
"""
if len(self._last_poses) == 0:
return None
chosen_stamp = None
smallest_delta = Duration(sys.maxsize) # Very big delta
for stamp in self._last_poses.keys():
delta = abs(image_stamp-stamp)
if delta < smallest_delta:
smallest_delta = delta
chosen_stamp = stamp
return self._last_poses[chosen_stamp]
def set_movement_parameters(self, duration, interval, min2356, max2356):
if not isinstance(duration, (int, float)) or duration < 0:
raise ValueError("Duration must be nonegative number.")
if len(interval) != 2 or any([
not isinstance(t, (int, float)) for t in interval
]) or any([t < 0 for t in interval]) or interval[0] > interval[1]:
raise ValueError("Interval must represent interval of time.")
if len(min2356) != 4 or any(
[not isinstance(v, (int, float)) for v in min2356]):
raise ValueError("min2356: list of four numbers was expected.")
if len(max2356) != 4 or any(
[not isinstance(v, (int, float)) for v in max2356]):
raise ValueError("max2356: list of four numbers was expected.")
self._duration = Duration.from_sec(duration)
self._interval = interval
self._min2356 = min2356
self._max2356 = max2356
def percieve_object(self):
rospy.logdebug('FastGrasp: Start percieving Object')
# create service
service = rospy.ServiceProxy("/suturo/GetGripper", GetGripper)
# get the first perception
rospy.logdebug('FastGrasp: call Perception Service')
resp = service("firstConveyorCall,cuboid")
if len(resp.objects) == 0 or not resp.objects[0].c_cuboid_success:
rospy.logdebug('FastGrasp: objects empty or no cuboid 1')
return -1
object1 = resp.objects[0].object
self.__perceived_pose_time = resp.stamp
# wait some time until the second perception
rospy.sleep(Duration.from_sec(0.5))
resp = service("cuboid")
if len(resp.objects) == 0 or not resp.objects[0].c_cuboid_success:
rospy.logdebug('FastGrasp: objects empty or no cuboid 2')
return -2
object2 = resp.objects[0].object
time2 = resp.stamp
# transform the points into odom_combined
self.__perceived_pose = utils.manipulation.transform_to(object1)
# utils.manipulation.get_planning_scene().add_object(self.__perceived_pose)
pose2 = utils.manipulation.transform_to(object2)
# calculate the vector from the points of first and second perception
self.__direction = numpy.array([
(pose2.primitive_poses[0].position.x -
self.__perceived_pose.primitive_poses[0].position.x),
(pose2.primitive_poses[0].position.y -
self.__perceived_pose.primitive_poses[0].position.y)
])
# calculate the time between the first and second perception
self.__time_between_poses = time2 - self.__perceived_pose_time
rospy.logdebug('FastGrasp: Return Object')
return 0
def parseXapPoses(self, xaplibrary):
try:
poses = xapparser.getpostures(xaplibrary)
except RuntimeError as re:
rospy.logwarn("Error while parsing the XAP file: %s" % str(re))
return
for name, pose in poses.items():
trajectory = JointTrajectory()
trajectory.joint_names = pose.keys()
joint_values = pose.values()
point = JointTrajectoryPoint()
point.time_from_start = Duration(2.0) # hardcoded duration!
point.positions = pose.values()
trajectory.points = [point]
self.poseLibrary[name] = trajectory
def percieve_object(self):
rospy.logdebug('FastGrasp: Start percieving Object')
# create service
service = rospy.ServiceProxy("/suturo/GetGripper", GetGripper)
# get the first perception
rospy.logdebug('FastGrasp: call Perception Service')
resp = service("firstConveyorCall,cuboid")
if len(resp.objects) == 0 or not resp.objects[0].c_cuboid_success:
rospy.logdebug('FastGrasp: objects empty or no cuboid 1')
return -1
object1 = resp.objects[0].object
self.__perceived_pose_time = resp.stamp
# wait some time until the second perception
rospy.sleep(Duration.from_sec(0.5))
resp = service("cuboid")
if len(resp.objects) == 0 or not resp.objects[0].c_cuboid_success:
rospy.logdebug('FastGrasp: objects empty or no cuboid 2')
return -2
object2 = resp.objects[0].object
time2 = resp.stamp
# transform the points into odom_combined
self.__perceived_pose = utils.manipulation.transform_to(object1)
# utils.manipulation.get_planning_scene().add_object(self.__perceived_pose)
pose2 = utils.manipulation.transform_to(object2)
# calculate the vector from the points of first and second perception
self.__direction = numpy.array(
[(pose2.primitive_poses[0].position.x - self.__perceived_pose.primitive_poses[0].position.x),
(pose2.primitive_poses[0].position.y - self.__perceived_pose.primitive_poses[0].position.y)])
# calculate the time between the first and second perception
self.__time_between_poses = time2 - self.__perceived_pose_time
rospy.logdebug('FastGrasp: Return Object')
return 0
def set_movement_parameters(self, duration, interval, joints, minimal,
maximal):
if not isinstance(duration, (int, float)) or duration < 0:
raise ValueError("Duration must be nonegative number.")
if len(interval) != 2 or any([
not isinstance(t, (int, float)) for t in interval
]) or any([t < 0 for t in interval]) or interval[0] > interval[1]:
raise ValueError("Interval must represent interval of time.")
if len(joints) != len(minimal) or len(joints) != len(maximal):
raise ValueError(
"joints, minimal and maximal arrays must have the same size.")
if any([not isinstance(v, str) for v in joints]):
raise ValueError("joints: list of string is expected.")
if any([not isinstance(v, (int, float)) for v in minimal]):
raise ValueError("minimal: list of numbers is expected.")
if any([not isinstance(v, (int, float)) for v in maximal]):
raise ValueError("maximal: list of numbers is expected.")
self._duration = Duration.from_sec(duration)
self._interval = interval
self._joints = joints
self._minimal = minimal
self._maximal = maximal
def __init__(self):
self.target_identifier = TargetIdentifier()
self.localizer = Localizer()
self.bridge = CvBridge()
self.sub_cam = rospy.Subscriber("/ardrone/bottom/image_raw", Image, self.identify_targets)
self.sub_pos = rospy.Subscriber("/vicon/ARDroneCarre/ARDroneCarre", TransformStamped, self.store_pos)
self.ob_locations = []
self.radius_list = []
self.lm_locations = []
self.crop_list = []
#self.yaw_list = []
self.epsilon = Duration(5)
self.frames = 0
self.obstacle_frames = 0
self.landmark_frames = 0
self.not_pixels = 0
self.time_filtered = 0
return
def pos_calc_test(m):
# create service
service = rospy.ServiceProxy("/suturo/GetGripper", GetGripper)
# get the first perception
resp = service("firstConveyorCall,centroid,cuboid")
object1 = resp.objects[0].object
time1 = resp.stamp
# wait some time until the second perception
rospy.sleep(Duration.from_sec(0.5))
resp = service("centroid,cuboid")
object2 = resp.objects[0].object
time2 = resp.stamp
# transform the points into odom_combined
pose1 = m.transform_to(object1)
pose2 = m.transform_to(object2)
# calculate the vector from the points of first and second perception
dir = numpy.array([(pose2.primitive_poses[0].position.x - pose1.primitive_poses[0].position.x), (
pose2.primitive_poses[0].position.y - pose1.primitive_poses[0].position.y)])
# dir_dist = sqrt(pow(dir[0], 2) + pow(dir[1], 2))
# get the duration between the two looks
time_12 = time2 - time1
# set the time in 5 secs
t_5 = rospy.Time.now() + rospy.Duration(8)
time_13 = t_5 - time1
diff_time = float(time_13.to_sec()) / float(time_12.to_sec())
dir_13 = numpy.array([diff_time * dir[0], diff_time * dir[1]])
# add this new vector on the points from the first perception
pose_comp = copy.deepcopy(pose1)
pose_comp.primitive_poses[0].position.x += dir_13[0]
pose_comp.primitive_poses[0].position.y += dir_13[1]
pose_comp.id = "red_cube"
# print out the table for the different poses for debug
table = [get_position(pose1), get_position(pose2), get_position(pose_comp)]
print tabulate(table)
# return test_extrapolation(dir, m, pose1, pose2, service, time1, time_12)
return pose_comp
def decode(self, _, data):
""" Generate a rospy.rostime.Duration instance based on the given data
which should be a string representation of a float.
"""
return Duration.from_sec(float(data))
def execute(self, userdata):
rospy.logdebug('FastGrasp: Executing state FastGrasp')
self.__t_point.header.frame_id = "/odom_combined"
# TODO: Auf 10 Min grenzen testen
if not utils.manipulation.is_gripper_open():
rospy.logdebug('FastGrasp: Open Gripper')
utils.manipulation.open_gripper()
r = self.percieve_object()
i = 0
while r == -1:
if i == 2 and not userdata.request_second_object:
# TODO: Zeit anpassen, ab wann gecalled wird
rospy.logdebug('FastGrasp: Request next object')
rospy.ServiceProxy("/euroc_interface_node/request_next_object", RequestNextObject).call()
userdata.request_second_object = True
if i == 19 and userdata.request_second_object:
rospy.logdebug('FastGrasp: Time Expired')
return 'noObjectsLeft'
r = self.percieve_object()
i += 1
# TODO: Was passiert wenn das Objekt nur einmal gesehen wurde
rospy.logdebug('FastGrasp: Begin movement, Plan 1')
for j in range(0, 4):
self.extrapolate(j)
self.calculate_target_point(self.__pose_comp)
if utils.manipulation.move_to(self.__t_point):
rospy.logdebug("FastGrasp: Plan 1: moved!")
break
else:
rospy.logdebug("FastGrasp: Plan 1: No Plan fount in step " + str(j))
if j == 3:
return 'noPlanFound'
if userdata.object_index == 1:
offset = rospy.Duration(2)
elif userdata.object_index == 2:
offset = rospy.Duration(4)
else:
offset = rospy.Duration(4)
while rospy.get_rostime() < self.__t_point_time - offset:
rospy.sleep(0.01)
self.__t_point.pose.position.z -= 0.07
rospy.logdebug("FastGrasp: Plan 2")
if utils.manipulation.move_to(self.__t_point):
rospy.logdebug("FastGrasp: Plan 2: moved!")
else:
rospy.logdebug("FastGrasp: Plan 2: Cant grasp")
return 'noPlanFound'
utils.manipulation.close_gripper(self.__pose_comp)
self.__t_point.pose.position.z += 0.1
rospy.logdebug("FastGrasp: Plan 3")
for k in range(0, 4):
if utils.manipulation.move_to(self.__t_point):
rospy.logdebug("FastGrasp: Plan 3: moved!")
break
else:
if k == 3:
rospy.logdebug("FastGrasp: No Plan found to lift object")
return 'noPlanFound'
rospy.logdebug("FastGrasp: Plan 3: Cant lift: " + str(k))
if self.__t_point.pose.position.y > 0:
self.__t_point.pose.position.y -= 0.1
else:
self.__t_point.pose.position.y += 0.1
if self.__t_point.pose.position.x > 0:
self.__t_point.pose.position.x -= 0.1
else:
self.__t_point.pose.position.x += 0.1
rospy.sleep(Duration.from_sec(0.5))
tfs = TorqueForceService()
if tfs.is_free():
rospy.logdebug("FastGrasp: Grasp Fail")
return 'graspingFailed'
rospy.logdebug("FastGrasp: objectGrasped, finished")
userdata.object_index += 1
return 'objectGrasped'
