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 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 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 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 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 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 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 __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 _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 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 __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 __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 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 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 __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 get_detailed_data(self):
"""
Returns the detailed data of the ConnectionItem.
:returns: str
"""
data_dict = self.get_latest_data()
if Time.now() - data_dict["window_stop"] > Duration(secs=5):
return "No recent data"
content = "<p class=\"detailed_data\">"
content += self.get_erroneous_entries()
if "frequency" in self._attributes:
content += self.tr("frequency") + ": " + prepare_number_for_representation(data_dict["frequency"]) \
+ " " + self.tr("frequency_unit") + " <br>"
content += self.tr("dropped_msgs") + ": " + prepare_number_for_representation(data_dict["dropped_msgs"]) + " " \
+ self.tr("dropped_msgs_unit") + " <br>"
content += self.tr("bandwidth") + ": " + prepare_number_for_representation(data_dict["bandwidth"]) + " " \
+ " " + self.tr("bandwidth_unit") + " <br>"
content += self.tr("period_mean") + ": " + prepare_number_for_representation(data_dict["period_mean"]) \
+ " " + self.tr("period_mean_unit") + " <br>"
content += self.tr("period_stddev") + ": " + prepare_number_for_representation(data_dict["period_stddev"]) \
+ " " + self.tr("period_stddev_unit") + " <br>"
content += self.tr("period_max") + ": " + prepare_number_for_representation(data_dict["period_max"]) + " " \
+ self.tr("period_max_unit") + " <br>"
content += self.tr("stamp_age_mean") + ": " + prepare_number_for_representation(data_dict["stamp_age_mean"]) \
+ " " + self.tr("stamp_age_mean_unit") + " <br>"
content += self.tr("stamp_age_stddev") + ": " + prepare_number_for_representation(data_dict["stamp_age_stddev"]) \
+ " " + self.tr("stamp_age_stddev_unit") + " <br>"
content += self.tr("stamp_age_max") + ": " + prepare_number_for_representation(data_dict["stamp_age_max"]) \
+ " " + self.tr("stamp_age_max_unit") + " <br>"
content += "</p>"
return content
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 " + str(round((Time.now() - data_dict["window_stop"]).to_sec(), ROUND_DIGITS)) \
+ " seconds"
content = ""
if data_dict["state"] is "error":
content += self.get_erroneous_entries_for_log()
else:
content += self.tr(
"node_cpu_usage_mean"
) + ": " + prepare_number_for_representation(
data_dict["node_cpu_usage_mean"]) + " " + self.tr(
"node_cpu_usage_mean_unit") + " - "
content += self.tr("node_ramusage_mean") + ": " + prepare_number_for_representation(
data_dict["node_ramusage_mean"]) \
+ " " + self.tr("node_ramusage_mean_unit") + " - "
content += self.tr("node_message_frequency_mean") + ": " + prepare_number_for_representation(
data_dict["node_message_frequency_mean"]) \
+ " " + self.tr("node_message_frequency_mean_unit") + " - "
content += self.tr("node_bandwidth_mean") + ": " + prepare_number_for_representation(
data_dict["node_bandwidth_mean"]) \
+ " " + self.tr("node_bandwidth_mean_unit")
return content
def execute_cb(self, goal):
# Waits until the action server has started up and started
# listening for goals.
number_of_joints = len(self.names_actions)
number_of_points_to_follow = len(goal.trajectory.points)
for i in range(len(self.names_actions)):
print(self.names_actions[i])
while not self.actions_list[i].wait_for_server(Duration(1.0)):
print("Waiting for" + self.names_actions[i] + "service")
msg_actions = []
for joint_number in range(number_of_joints):
msg_actions.append(FollowJointTrajectoryGoal())
for t in range(number_of_points_to_follow):
for joint_number in range(number_of_joints):
p = JointTrajectoryPoint()
p.positions.append(
goal.trajectory.points[t].positions[joint_number])
p.velocities.append(
goal.trajectory.points[t].velocities[joint_number])
p.accelerations.append(
goal.trajectory.points[t].accelerations[joint_number])
p.time_from_start.secs = goal.trajectory.points[
t].time_from_start.secs
p.time_from_start.nsecs = goal.trajectory.points[
t].time_from_start.nsecs
msg_actions[joint_number].trajectory.points.append(p)
for joint_number in range(number_of_joints):
# client.send_goal(goal_client, feedback_cb=self.feedback_cb)
self.actions_list[joint_number].send_goal(
msg_actions[joint_number])
time_to_wait = (goal.trajectory.points[number_of_points_to_follow -
1].time_from_start.secs +
goal.trajectory.points[number_of_points_to_follow - 1].
time_from_start.nsecs / 1e+9) + 2
# Waits for the server to finish performing the action
for joint_number in range(number_of_joints):
# client.send_goal(goal_client, feedback_cb=self.feedback_cb)
self.actions_list[joint_number].wait_for_result(
Duration(time_to_wait))
success = True
for joint_number in range(number_of_joints):
if self.actions_list[joint_number].get_state(
) == GoalStatus.SUCCEEDED:
rospy.loginfo('%s: Succeeded' % self._action_name)
if self._as.is_active():
print("Motor " + self.names_actions[joint_number] +
" finished well")
else:
success = False
else:
print("Motor " + self.names_actions[joint_number] + " failed")
success = False
if (success):
self._as.set_succeeded(self._result)
else:
self._as.set_aborted(text="JointTrajectory action did not succeed")
# ROS publishers
pub_control = rospy.Publisher("/arm_controller/command", JointTrajectory, queue_size=1)
# Subscriber
#sub_correction = rospy.Subscriber("/end_effector/correction_velocity", Vector3, cb_correction)
#sub_joints = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, cb_joints)
msg_arm = JointTrajectory()
msg_arm.joint_names = ["arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint", "arm_6_joint", "arm_7_joint"]
msg_arm_point = JointTrajectoryPoint()
freq = 10
r = rospy.Rate(freq)
duration_mult = 2
msg_arm_point.time_from_start = Duration(nsecs=1e9/freq*2.0)
while not rospy.is_shutdown(): # Create a loop that will go until someone stops the program execution
r.sleep()
if keyboard.read_key() == "w":
print("You pressed w")
joint_angles = joint_angles + joint_angles_6_positive
elif keyboard.read_key() == "a":
print("You pressed a")
joint_angles = joint_angles + joint_angles_7_positive
elif keyboard.read_key() == "s":
print("You pressed s")
joint_angles = joint_angles + joint_angles_6_negative
elif keyboard.read_key() == "d":
print("You pressed d")
joint_angles = joint_angles + joint_angles_7_negative
def forward(self):
T = self.T
dU = self.dU
dX = self.dX
# joint names of the four wheels
wheel_joint_bl = 'wheel_joint_bl'
wheel_joint_br = 'wheel_joint_br'
wheel_joint_fl = 'wheel_joint_fl'
wheel_joint_fr = 'wheel_joint_fr'
base_link = 'base_footprint'
start_time = Duration(secs = 0, nsecs = 0) # start asap
rospy.loginfo("stepped into forward pass of algorithm")
reference_frame = None #'empty/world/map'
# update the states and controls
for t in range(T):
# self.pool_derivatives.apply_async(self.get_action_cost_jacs, \
# args=(t))
stage_jacs = self.get_action_cost_jacs(t)
# update states
self.traj_distr.delta_state[t,:] = self.traj_distr.state[t,:] - self.traj_distr.nominal_state[t,:]
self.traj_distr.delta_action[t,:] = self.traj_distr.delta_action[t,:] \
+ self.traj_distr.gu[t, :] \
+ self.traj_distr.Gu[t, :].dot(self.traj_distr.delta_state[t,:])
theta = self.traj_distr.delta_state[t,:][-1]
rospy.logdebug('action nominal: {}'.format(self.traj_distr.action_nominal[t]))
rospy.logdebug('delta action : {}'.format(self.traj_distr.delta_action[t]))
rospy.logdebug('action : {}'.format(self.traj_distr.action[t]))
duration = Duration(secs = 5, nsecs = 0) # apply effort continuously without end duration = -1
# update state at t+1
if t < T-1:
self.traj_distr.state[t+1,:] = self.get_new_state(theta, t+1)
torques = self.traj_distr.delta_action[t,:]
# send in this order: 'wheel_joint_bl', 'wheel_joint_br', 'wheel_joint_fl', 'wheel_joint_fr'
# create four different asynchronous threads for each wheel
"""
wheel_joint_bl_thread = threading.Thread(group=None, target=send_joint_torques,
name='wheel_joint_bl_thread',
args=(wheel_joint_bl, torques[0], start_time, duration))
wheel_joint_br_thread = threading.Thread(group=None, target=send_joint_torques,
name='wheel_joint_br_thread',
args=(wheel_joint_br, torques[1], start_time, duration))
wheel_joint_fl_thread = threading.Thread(group=None, target=send_joint_torques,
name='wheel_joint_fl_thread',
args=(wheel_joint_fl, torques[2], start_time, duration))
wheel_joint_fr_thread = threading.Thread(group=None, target=send_joint_torques,
name='wheel_joint_fr_thread',
args=(wheel_joint_fr, torques[3], start_time, duration))
"""
# calculate the genralized force and torques
sign_phi = np.sign(self.Phi_dot)
bdyn = self.assemble_dynamics()
F1 = (torques[0] - self.wheel_rad * sign_phi[0] * bdyn.f[0]) * \
(-(np.cos(theta) - np.sin(theta))/self.wheel_rad) + \
(torques[1] - self.wheel_rad * sign_phi[1] * bdyn.f[0]) * \
(-(np.cos(theta) + np.sin(theta))/self.wheel_rad) + \
(torques[2] - self.wheel_rad * sign_phi[2] * bdyn.f[0]) * \
((np.cos(theta) - np.sin(theta))/self.wheel_rad) + \
(torques[3] - self.wheel_rad * sign_phi[3] * bdyn.f[0]) * \
((np.cos(theta) + np.sin(theta))/self.wheel_rad)
F2 = (torques[0] - self.wheel_rad * sign_phi[0] * bdyn.f[0]) * \
(-(np.cos(theta) + np.sin(theta))/self.wheel_rad) + \
(torques[1] - self.wheel_rad * sign_phi[1] * bdyn.f[0]) * \
(-(-np.cos(theta) + np.sin(theta))/self.wheel_rad) + \
(torques[2] - self.wheel_rad * sign_phi[2] * bdyn.f[0]) * \
((np.cos(theta) + np.sin(theta))/self.wheel_rad) + \
(torques[3] - self.wheel_rad * sign_phi[3] * bdyn.f[0]) * \
((-np.cos(theta) + np.sin(theta))/self.wheel_rad)
F3 = np.sum(torques) * (-np.sqrt(2)*self.l * np.sin( np.pi/4 - self.alpha)/self.wheel_rad) \
+ (sign_phi[0] * bdyn.f[0] + sign_phi[1] * bdyn.f[0] + sign_phi[2] * bdyn.f[0] + sign_phi[3] * bdyn.f[0]) \
* (np.sqrt(2)* self.l * np.sin(np.pi/4 - self.alpha))
rospy.loginfo('F1: {}, F2: {}, F3: {}'.format(F1, F2, F3))
wrench_base = Wrench()
wrench_base.force.x = F1
wrench_base.force.y = F2
base_angle = Twist()
base_angle.angular.z = F3
# send the torques to the base footprint
# self.pub.publish(base_angle)
resp_bf = send_body_wrench('base_footprint', reference_frame,
None, wrench_base, start_time,
duration )
clear_bf = clear_active_wrenches('base_footprint')
"""
print("Initializing client node...")
rospy.init_node('traj_action_client')
print("Generating client...")
client = actionlib.SimpleActionClient('left_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
print("Waiting for server...")
client.wait_for_server()
# create goal
goal = FollowJointTrajectoryGoal()
# define goal trajectory
waypoint1 = JointTrajectoryPoint()
waypoint1.positions = [0, 1, .75, 0.5]
waypoint1.time_from_start = Duration(0.0)
traj1 = JointTrajectory()
traj1.joint_names = ["l_shoulder_y", "l_shoulder_x", "l_arm_z", "l_elbow_y"]
traj1.points = [waypoint1]
# set goal trajectory
goal.trajectory = traj1
# send goal
client.send_goal(goal, feedback_cb=feedback_cb)
print("Sent goal")
client.wait_for_result()
print('[Result] State: %d'%(client.get_state()))
# Joint trajectory message is used to specify joint angles
msg_arm = JointTrajectory()
msg_arm.joint_names = [
"arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint",
"arm_6_joint", "arm_7_joint"
]
msg_arm_point = JointTrajectoryPoint()
rospy.init_node("ik_node")
freq = 50 # Frequency for new messages to be published
duration_mult = 2 # Used to create an 'overlap' between messages, keep above 1 (otherwise arm will try to
# move to the next position before the next message, causing it to rapidly start/stop
joint_vel_max = 1.0 # Maximum velocity of a single joint (rad/sec)
r = rospy.Rate(freq)
msg_arm_point.time_from_start = Duration(
nsecs=1e9 * duration_mult / freq) # (duration_mult / freq) = deltatime
while not rospy.is_shutdown():
r.sleep()
if (
joint_angles is None or inverse_jacobian is None
): # joint_angles and inverse_jacobian are required for calculation, need these to be published first
continue
# Transform end-effector velocity to correct frame of reference
end_effector_vel_command = np.dot(rot_mat, end_effector_desired_vel)
# Use jacobian pseudoinverse to calculate desired joint velocity
joint_vel_command = np.dot(inverse_jacobian, end_effector_vel_command)
# Scale joint velocity down if too high
if joint_vel_max < np.max(joint_vel_command):
joint_vel = joint_vel_command / np.max(
joint_vel_command) * joint_vel_max
def __init__(self, model):
"""
Initializes the Widget.
:param model: the model of the widget
:type model: ROSModel
"""
super(SelectionWidget, self).__init__()
self.setObjectName('selection_widget')
self.__model = model
# Get path to UI file which is a sibling of this file
self.rp = rospkg.RosPack()
ui_file = os.path.join(self.rp.get_path('arni_rqt_detail_plugin'), 'resources', 'SelectionWidget.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self)
self.setObjectName('SelectionWidgetUi')
self.__selected_item = None
self.__draw_graphs = True
self.__current_combo_box_index = 0
self.__last_update = rospy.Time.now()
try:
if rospy.get_param("/enable_statistics") == False:
raise KeyError('/enable_statistics')
except KeyError:
self.__overview_widget = None
raise EnvironmentError("/enable_statistics is either not set or set to false - arni gui would not work correctly. Please make sure to start "
"roscore with the neccesary parameters or to load these while running (see init_params.launch)")
self.__values_dict = {
"bandwidth_mean": 0,
"bandwidth_stddev": 0,
"bandwidth_max": 0,
}
self.__logger = self.__model.get_logger()
self.__log_filter_proxy = LogFilterProxy()
self.__log_filter_proxy.filter_by_item(self.__selected_item)
self.__log_filter_proxy.setDynamicSortFilter(True)
self.__log_filter_proxy.setSourceModel(self.__logger.get_representation())
self.log_tab_tree_view.setModel(self.__log_filter_proxy)
self.__log_delegate = LogDelegate()
self.log_tab_tree_view.setItemDelegate(self.__log_delegate)
self.__style_string = ".detailed_data {\n" \
" font-size: 12\n;" \
"}\n"
self.__style_string = ".erroneous_entry {\n" \
" color: red\n;" \
"}\n"
self.information_tab_text_browser.setStyleSheet(self.__style_string)
self.range_combo_box.clear()
self.range_combo_box.addItem("10 " + self.tr("Seconds"))
self.range_combo_box.addItem("30 " + self.tr("Seconds"))
self.range_combo_box.addItem("60 " + self.tr("Seconds"))
self.range_combo_box.setCurrentIndex(0)
#self.scrollAreaWidgetContents_2.setWidget(self.host_node_label)
self.tab_widget.setTabText(0, self.tr("Information"))
self.tab_widget.setTabText(1, self.tr("Graphs"))
self.tab_widget.setTabText(2, self.tr("Log"))
self.tab_widget.setTabText(3, self.tr("Actions"))
self.stop_push_button.setText(self.tr("Stop Node"))
self.restart_push_button.setText(self.tr("Restart Node"))
self.stop_push_button.setEnabled(False)
self.restart_push_button.setEnabled(False)
### CARSON ADDED ###
# set default values for throttle rate and window sliders
self.throttle_rate_slider.setFocusPolicy(Qt.StrongFocus)
self.throttle_rate_slider.setValue(5000)
self.throttle_window_slider.setFocusPolicy(Qt.StrongFocus)
self.throttle_window_slider.setValue(500)
# set up validator for throttle rate and window text fields
# only allows floating point numbers
regex = QRegExp(r'[0-9]*\.?[0-9]+')
validator = QRegExpValidator(regex)
self.throttle_rate.setValidator(validator)
self.throttle_window.setValidator(validator)
# set up QButtonGroup for message/bandwidth throttle type radio buttons
self.throttle_radio_group = QButtonGroup()
self.throttle_radio_group.addButton(self.throttle_message_radio)
self.throttle_radio_group.addButton(self.throttle_bandwidth_radio)
self.throttle_radio_group.buttonClicked.connect(self.__on_type_button_clicked)
### ###
self.selected_label.setText(self.tr("Selected") + ":")
self.range_label.setText(self.tr("Range") + ":")
self.log_tab_tree_view.setRootIsDecorated(False)
self.log_tab_tree_view.setAlternatingRowColors(True)
self.log_tab_tree_view.setSortingEnabled(True)
self.log_tab_tree_view.sortByColumn(1, Qt.AscendingOrder)
self.__current_range_combo_box_index = 0
self.__current_selected_combo_box_index = 0
self.set_selected_item(self.__selected_item)
self.__model.layoutChanged.connect(self.update)
self.__state = "ok"
self.__previous_state = "ok"
self.__selected_item_changed = True
self.__deleted = False
pg.setConfigOption('background', 'w')
pg.setConfigOption('foreground', 'k')
self.__graph_layout = ResizeableGraphicsLayoutWidget(self.__on_graph_window_size_changed)
self.graph_scroll_area.setWidget(self.__graph_layout)
self.__plotable_items = None#self.__selected_item.get_plotable_items()
self.__items_per_group = 1
self.__expected_items_per_group = 1
self.__number_of_groups = 1
self.__update_graphs_lock = Lock()
self.__first_update_pending = True
self.__graph_dict = {}
self.__plotted_curves = {}
#self.create_graphs()
self.__timer = Timer(Duration(secs=1.0), self.update_graphs)
def update_model(self):
"""
Updates the model by using the items of the list. The items will be of the message types.
:param rated_statistics: the rated_statistics buffer
:type rated_statistics: list
:param topic_statistics: the topic_statistics buffer
:type topic_statistics: list
:param node_statistics: the node_statistics buffer
:type node_statistics: list
:param host_statistics: the host_statistics buffer
:type host_statistics: list
:param master_api_data: data from the master api
:type master_api_data: MasterApi
"""
self.__model_lock.acquire()
self.layoutAboutToBeChanged.emit()
if self.__buffer_thread:
rated_statistics, topic_statistics, host_statistics, node_statistics, master_api_data = self.__buffer_thread.get_state()
amount_of_entries = 0
for item in self.__identifier_dict.values():
if item is not self.__root_item:
amount_of_entries += item.get_amount_of_entries()
# enables "intelligent" updates when there are only few elements in the model so that most of the history is kept
# maybe use something loglike for the range
for i in range(5, 0, -1):
if amount_of_entries > MAXIMUM_AMOUNT_OF_ENTRIES:
for item in self.__identifier_dict.values():
if item is not self.__root_item:
item.delete_items_older_than(Time.now() - (Duration(secs=i * MINIMUM_RECORDING_TIME) if int(
Duration(secs=i * MINIMUM_RECORDING_TIME).to_sec()) <= int(Time.now().to_sec()) else Time(
0)))
if self.__root_item.get_amount_of_entries() > MAXIMUM_AMOUNT_OF_ENTRIES:
self.__root_item.delete_items_older_than(Time.now() - (
Duration(secs=360) if int(Duration(secs=360).to_sec()) <= int(Time.now().to_sec()) else Time(0)))
# in order of their appearance in the treeview for always having valid parents
for item in host_statistics:
self.__transform_host_statistics_item(item)
for item in node_statistics:
self.__transform_node_statistics_item(item)
for item in topic_statistics:
self.__transform_topic_statistics_item(item)
if master_api_data is not None:
self.__transform_master_api_data(master_api_data)
# rating last because it needs the time of the items before
for item in rated_statistics:
self.__transform_rated_statistics_item(item)
data_dict = {
"state": "ok",
"total_traffic": 0,
"connected_hosts": 0,
"connected_nodes": 0,
"topic_counter": 0,
"connection_counter": 0,
"cpu_usage_max": 0,
"cpu_temp_mean": 0,
"ram_usage_mean": 0,
"cpu_usage_mean": 0,
"cpu_temp_max": 0,
"ram_usage_max": 0,
}
connected_hosts = 0
connected_nodes = 0
topic_counter = 0
connection_counter = 0
state = "ok"
# generate the general information
for host_item in self.__root_item.get_childs():
# hostinfo
connected_hosts += 1
if host_item.get_state() is "warning" and state is not "error":
state = "warning"
elif host_item.get_state() is "error":
state = "error"
last_entry = {}
data = host_item.get_items_younger_than(Time.now() - (
Duration(secs=10) if int(Duration(secs=10).to_sec()) <= int(Time.now().to_sec()) else Time(0)),
"bandwidth_mean", "cpu_usage_max",
"cpu_temp_mean", "cpu_usage_mean", "cpu_temp_max", "ram_usage_max",
"ram_usage_mean")
if data["window_stop"]:
for key in data:
if key is not "window_stop":
last_entry[key] = data[key][-1]
else:
data = host_item.get_latest_data("bandwidth_mean", "cpu_usage_max", "cpu_temp_mean", "cpu_usage_mean",
"cpu_temp_max", "ram_usage_max", "ram_usage_mean")
for key in data:
last_entry[key] = data[key]
for key in last_entry:
if last_entry[key]:
if key is "bandwidth_mean":
for entry in last_entry[key]:
if type(entry) is not unicode:
if entry is not 0:
data_dict["total_traffic"] += entry
elif key is "cpu_temp_max" or key is "ram_usage_max":
# very unprobably the temp might be 0 then the programm is not showing this value!
if type(last_entry[key]) is not unicode:
if last_entry[key] is not 0:
if data_dict[key] < last_entry[key]:
data_dict[key] = last_entry[key]
else:
if type(last_entry[key]) is not unicode:
if last_entry[key] is not 0:
data_dict[key] += last_entry[key]
for node_item in host_item.get_childs():
# nodeinfo
connected_nodes += 1
if node_item.get_state() is "warning" and state is not "error":
state = "warning"
elif node_item.get_state() is "error":
state = "error"
for topic_item in node_item.get_childs():
# topic info
topic_counter += 1
if topic_item.get_state() is "warning" and state is not "error":
state = "warning"
elif topic_item.get_state() is "error":
state = "error"
for connection_item in topic_item.get_childs():
# connection info
connection_counter += 1
if connection_item.get_state() is "warning" and state is not "error":
state = "warning"
elif connection_item.get_state() is "error":
state = "error"
for key in data_dict:
if key != "state" and key != "cpu_temp_max" and key != "total_traffic" and key != "ram_usage_max" \
and self.__root_item.child_count():
data_dict[key] /= self.__root_item.child_count()
data_dict["connected_hosts"] = connected_hosts
data_dict["connected_nodes"] = connected_nodes
data_dict["topic_counter"] = topic_counter
data_dict["connection_counter"] = connection_counter
data_dict["state"] = state
data_dict["window_end"] = Time.now()
# now give this information to the root :)
self.__root_item.append_data_dict(data_dict)
self.__model_lock.release()
# self.__checkIfAlive()
self.layoutChanged.emit()
