def add_fig_cb(self,msg):
"""Callback of ROS topic to define a new projection element.
Args:
msg (object): object with the necessary parameters to define a new projection element
"""
rospy.loginfo("Received request to add a new projection element to the active coordinate system.")
if not msg.projection_group or not msg.figure_name:
rospy.logerr("projection_group or figure_name request is empty.")
else:
try:
if msg.figure_type == Figure.POLYLINE:
self.projector.create_polyline(msg)
rospy.loginfo("Line added correctly.")
elif Figure.CIRCLE <= msg.figure_type <= Figure.OVAL:
self.projector.create_curve(msg)
rospy.loginfo("Curve added correctly.")
elif msg.figure_type == Figure.TEXT:
self.projector.create_text(msg)
rospy.loginfo("Text added correctly.")
elif msg.figure_type == Figure.POINTER:
rospy.warn("Pointer has other purpose.")
else:
rospy.logerr("Figure type does not exist.")
rospy.loginfo("Figure added correctly.")
except Exception as e:
rospy.logerr(e)
def wait_ack(self):
""" Wait for an acknowledgment from the localino """
if self._DEBUG:
rospy.loginfo("Waiting for ack from localino...")
resets = 0
delayTime = 0.5 # how much we'll delay on each loop if there's no input
rospy.sleep(delayTime / 2)
while not rospy.is_shutdown():
try:
c = self.l.read(1).decode('utf-8')
if c == 'a':
c2 = self.l.read(1).decode('utf-8')
if self._DEBUG:
rospy.loginfo("Received ack #" + str(c2))
break
elif not c:
rospy.logwarn("No ack from localino")
resets += 1
rospy.sleep(
delayTime
) # delay, there was no response from the localino
else:
rospy.logwarn(
"Expecting ack code from localino, received other char: "
+ c)
if (resets * delayTime) > self.timeout:
rospy.warn("Timed out waiting for ack, resetting")
return 1
except Exception as e:
rospy.logwarn(e)
rospy.sleep(delayTime)
return 0
def send_debug_sock_msg(self, msg):
"""
Writes a string to the browser's debug web socket.
"""
rospy.warn(
'ManagedBrowser.send_debug_sock_msg() probably not yet working'
)
ws_url = 'ws://localhost:{}'.format(self.debug_port)
conn = yield websocket_connect(ws_url, connect_timeout=1)
conn.write_message(msg)
conn.close()
def send_debug_sock_msg(self, msg):
"""
Writes a string to the browser's debug web socket.
"""
rospy.warn(
'ManagedBrowser.send_debug_sock_msg() probably not yet working'
)
ws_url = 'ws://localhost:{}'.format(self.debug_port)
conn = yield websocket_connect(ws_url, connect_timeout=1)
conn.write_message(msg)
conn.close()
def get_robot_location(self):
t, r = None, None
if self.use_robot or self.teleop_only:
try:
(trans, rot) = self.tf.lookupTransform("/map", "/base_link", rospy.Time(0))
t = Transformation(*trans)
euler = euler_from_quaternion(rot)
r = Rotation(*euler)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.warn("manager: Cannot find /map or /base_link")
return (t, r)
def handle_im_out(self, msg):
"""
ROS callback
Send an instant message to the device
:param msg: ROS message, type: AcousticModemPayload
"""
if self.modem_status:
err_code, cmd_sent = self.modem.add_im_to_buffer(msg.address, msg.ack, msg.payload)
self.eval_error_code(err_code, cmd_sent)
if err_code in [ed.OK, ed.WARN_ACK_BUSY]:
self.sent_im_ack_cnt += 1
self.modem.send_line()
else:
rospy.warn("%s: Modem is disabled! Command ignored" % self.name)
def handle_burst_out(self, msg):
"""
ROS callbackself.propagation_time
Send an burst message to the device
:param msg: ROS message, type: AcousticModemPayload
"""
if self.modem_status:
err_code, cmd_sent = self.modem.add_burst_to_buffer(msg.address, msg.payload)
self.eval_error_code(err_code, cmd_sent)
# print self.sent_burst_cnt
if err_code in [ed.OK, ed.WARN_ACK_BUSY]:
self.sent_burst_cnt += 1
else:
rospy.warn("%s: Modem is disabled! Command ignored" % self.name)
def rx(self):
message = serial.read(256)
if message[0] != "\x0D" or message[-1] != "\x0A":
rospy.warn("Received corrupted packet from scooter.")
return
crc_received = chr(message[-2])
crc_computed = Scooter.crc_chr(message[:-1])
if crc_received != crc_computed:
rospy.warn("RX CRC Error.")
return
# Startup Message
if message[1] == "\xA1":
pass
# Startup Message?
elif message[1] == "\x30":
pass
# Normal message?
elif message[1] == "\x31":
# TODO: positive/negative velocity?
velocity = int(message[4]) * 100 + int(message[5]) * 10 + int(
message[6]) * 1
if self.velocity_last is not None:
# Integrate to find linear translation
linear_translation = 1 / 2. * self.dt * velocity + self.velocity_last * self.dt
delta_theta = (self.k *
self.phi) * linear_translation * self.dt
midpoint_theta = (self.theta + self.theta + delta_theta) / 2.
# Rotate linear translation with midpoint theta
delta_vector = np.array([
linear_translation * np.cos(midpoint_theta),
linear_translation * np.sin(midpoint_theta)
])
# Update position and pose
self.x += delta_vector[0]
self.y += delta_vector[1]
self.theta += delta_theta
self.velocity_last = velocity
def moduleCallback(kval):
modName = "MODULE" + str(int(kval.key.split(":")[1]) + 1)
val = int(kval.value)
mod = modules[modName]
if (val == 0):
modules[modName] = ("FULL", mod[1])
elif (val == 1):
modules[modName] = ("OK", mod[1])
elif (val == 2):
modules[modName] = (mod[0], "IDLE")
elif (val == 3):
modules[modName] = (mod[0], "INTERACT")
else:
rospy.warn("Invalid module status: " + str(val))
display()
def moduleCallback(kval):
modName = "MODULE" + str(int(kval.key.split(":")[1]) + 1)
val = int(kval.value)
mod = modules[modName]
if (val == 0):
modules[modName] = ("FULL", mod[1])
elif (val == 1):
modules[modName] = ("OK", mod[1])
elif (val == 2):
modules[modName] = (mod[0], "IDLE")
elif (val == 3):
modules[modName] = (mod[0], "INTERACT")
else:
rospy.warn("Invalid module status: " + str(val))
display()
def set_robot_pose(self, r_foot_x, r_foot_y, r_foot_z, r_foot_roll, r_foot_pitch, r_foot_yaw,\
l_foot_x, l_foot_y, l_foot_z, l_foot_roll, l_foot_pitch, l_foot_yaw,\
cob_x, cob_y, cob_z, cob_roll, cob_pitch, cob_yaw) :
msg = RobotPose()
# Right Foot
msg.global_to_right_foot.position.x = r_foot_x
msg.global_to_right_foot.position.y = r_foot_y
msg.global_to_right_foot.position.z = r_foot_z
r_foot_quaternion = euler_to_quaternion(r_foot_roll, r_foot_pitch,
r_foot_yaw)
msg.global_to_right_foot.orientation.x = r_foot_quaternion[0]
msg.global_to_right_foot.orientation.y = r_foot_quaternion[1]
msg.global_to_right_foot.orientation.z = r_foot_quaternion[2]
msg.global_to_right_foot.orientation.w = r_foot_quaternion[3]
# Left Foot
msg.global_to_left_foot.position.x = l_foot_x
msg.global_to_left_foot.position.y = l_foot_y
msg.global_to_left_foot.position.z = l_foot_z
l_foot_quaternion = euler_to_quaternion(l_foot_roll, l_foot_pitch,
l_foot_yaw)
msg.global_to_left_foot.orientation.x = l_foot_quaternion[0]
msg.global_to_left_foot.orientation.y = l_foot_quaternion[1]
msg.global_to_left_foot.orientation.z = l_foot_quaternion[2]
msg.global_to_left_foot.orientation.w = l_foot_quaternion[3]
# Centre of Body
msg.global_to_center_of_body.position.x = cob_x
msg.global_to_center_of_body.position.y = cob_y
msg.global_to_center_of_body.position.z = cob_z
cob_quaternion = euler_to_quaternion(cob_roll, cob_pitch, cob_yaw)
msg.global_to_center_of_body.orientation.x = cob_quaternion[0]
msg.global_to_center_of_body.orientation.y = cob_quaternion[1]
msg.global_to_center_of_body.orientation.z = cob_quaternion[2]
msg.global_to_center_of_body.orientation.w = cob_quaternion[3]
if self.status_msg != "Walking_Started":
self.publisher_(self.robot_pose_pub, msg)
else:
rospy.warn(
"[Walking] Robot is walking now, just please set this parameter before starting robot's walk."
)
def makeMarker(msg, color):
marker = VM.Marker()
marker.type = VM.Marker.SPHERE
marker.scale.x = msg.scale * 0.45
marker.scale.y = msg.scale * 0.45
marker.scale.z = msg.scale * 0.45
marker.color.r = 0.1
marker.color.g = 0.1
marker.color.b = 0.1
marker.color.a = 0.75
if color == "red":
marker.color.r += 0.4
elif color == "blue":
marker.color.b += 0.4
elif color == "green":
marker.color.g += 0.4
else:
rospy.warn("Marker color not recognized!")
return marker
def bestvantage(self, req):
rospy.logout('Recorder: Calculating best vantage for tag \'%s\'' % req.tagid)
d = {}
for rr in self.recorder_data: # rr is RecorderRead
if not d.has_key( rr.read.tagID ):
d[rr.read.tagID] = []
d[rr.read.tagID].append( rr )
pos_reads = []
if d.has_key( req.tagid ):
pos_reads = [ q for q in d[ req.tagid ] if q.read.rssi != -1 ] # list of RecorderReads
if not pos_reads: # check at least one positive reading
rospy.warn( 'Recorder: Desired tag had no readings.' )
rv = PoseStamped()
rv.header.frame_id = 'base_link'
rv.header.stamp = rospy.Time.now()
rv.pose.orientation.w = 1.0
return rv
# Select the RecorderRead with greatest RSSI
rssi = [ r.read.rssi for r in pos_reads ]
ind = np.argmax( rssi )
best = pos_reads[ ind ] # RecorderRead
best_read = best.read
best_ant = best.ps_ant_map
best_base = best.ps_base_map
#print best_read, best_ant, best_base
# We're going to keep the <x,y> location from the baselink (mapframe),
# but keep <ang> (mapframe) from the antenna.
rv = PoseStamped()
rv.header.stamp = rospy.Time.now()
rv.header.frame_id = best_base.header.frame_id
rv.pose.position = best_base.pose.position
rv.pose.orientation = best_ant.pose.orientation
return rv
def callback(self,original_image):
np_arr = np.fromstring(original_image.data, np.uint8)
image_in = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
self.acc = self.acc+1
if self.average_image == None or self.sum_image ==None:
self.average_image = image_in.copy()
self.sum_image = np.zeros((len(self.average_image),len(self.average_image[0]),len(self.average_image[0][0])))
gain = 1.0/self.acc
for x in range(len(self.average_image)):
for y in range(len(self.average_image[x])):
for z in range(len(self.average_image[x][y])):
self.sum_image[x][y][z] =self.sum_image[x][y][z]+image_in[x][y][z]
#self.average_image[x][y][z] =((self.acc-1.0)/self.acc)*self.average_image[x][y][z] + (1.0/self.acc)*image_in[x][y][z]
self.average_image[x][y][z] =gain*self.sum_image[x][y][z]
#self.average_image = cv2.addWeighted(self.average_image,(self.acc-1.0)/self.acc,image_in,1.0/self.acc,0)
rospy.logwarn("%s",self.average_image[0][0])
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(self.average_image, "bgr8"))
except CvBridgeError as e:
rospy.warn("Exception while publishing image")
def create_and_publish_marker(self, name, pose, ns=''):
self.my_mutex.acquire()
# Create marker
marker = Marker()
try:
marker.type = self.marker_primitives[name].type
marker.scale.x = self.marker_primitives[name].scale.x
marker.scale.y = self.marker_primitives[name].scale.y
marker.scale.z = self.marker_primitives[name].scale.z
marker.color.r = self.marker_primitives[name].color.r
marker.color.g = self.marker_primitives[name].color.g
marker.color.b = self.marker_primitives[name].color.b
marker.color.a = self.marker_primitives[name].color.a
except: # Use default if not found as template
rospy.warn(
'default marker') # TODO this doesn't disappear properly
marker.type = 2 # sphere
marker.scale.x = .1
marker.scale.y = .1
marker.scale.z = .1
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
marker.header.stamp = rospy.get_rostime()
marker.header.frame_id = "iiwa_link_0"
marker.ns = ns
if not self.markers_:
marker.id = 1
else:
marker.id = self.markers_[
-1].id + 1 # TODO: Make this counter restart from 0 if it gets too big
marker.action = 0 # Add/modify
marker.pose = pose
marker.lifetime = rospy.Duration(
self.lifetime
) # This is not very meaningful as it gets reset to the same value when the marker is updated
# append the new marker and publish it
self.markers_.append(marker)
self.markers_pub_.publish(marker)
self.my_mutex.release()
def make_marker( color ):
marker = VM.Marker()
marker.type = VM.Marker.SPHERE
marker.scale.x = 0.15
marker.scale.y = 0.15
marker.scale.z = 0.15
marker.color.r = 0.1
marker.color.g = 0.1
marker.color.b = 0.1
marker.color.a = 0.75
if color == 'red':
marker.color.r += 0.4
elif color == 'blue':
marker.color.b += 0.4
elif color == 'green':
marker.color.g += 0.4
else:
rospy.warn("Marker color not recognized!")
return marker
def makeMarker(msg, color):
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = msg.scale * 0.45
marker.scale.y = msg.scale * 0.45
marker.scale.z = msg.scale * 0.45
marker.color.r = 0.1
marker.color.g = 0.1
marker.color.b = 0.1
marker.color.a = 0.75
if color == 'red':
marker.color.r += 0.4
elif color == 'blue':
marker.color.b += 0.4
elif color == 'green':
marker.color.g += 0.4
else:
rospy.warn("Marker color not recognized!")
return marker
def make_marker(color):
marker = VM.Marker()
marker.type = VM.Marker.SPHERE
marker.scale.x = 0.15
marker.scale.y = 0.15
marker.scale.z = 0.15
marker.color.r = 0.1
marker.color.g = 0.1
marker.color.b = 0.1
marker.color.a = 0.75
if color == 'red':
marker.color.r += 0.4
elif color == 'blue':
marker.color.b += 0.4
elif color == 'green':
marker.color.g += 0.4
else:
rospy.warn("Marker color not recognized!")
return marker
def grab_url(self, window):
"""
given a window (from the director message) grab either the kiosk or
the display current url based on the viewport from the window. Apply
any tactile changes if maps.google.com is part of the url
"""
url_service = None
activity = window.get('activity', None)
if activity != 'browser':
return
viewport = window.get('presentation_viewport', None)
if viewport is None:
rospy.info("viewport was None... ignoring")
return
# display might be ok to go away, but only once we're sure
if viewport != 'kiosk' and viewport != 'wall' and viewport != 'display':
rospy.warn("Unable to determine viewport named (%s)" % viewport)
return
if viewport == 'kiosk':
url_service = self.kiosk_url_service
elif viewport == 'display' or viewport == 'wall':
url_service = self.display_url_service
state = url_service.call().state
try:
state = json.loads(state)
except:
rospy.logwarn("Unable to parse state (%s)" % state)
raise
if len(state) > 1:
rospy.logwarn('There are more than one browser active, the wrong URL might be returned')
for browser_id, browser_data in state.iteritems():
return browser_data['current_url_normalized']
if datafile == "":
rospy.logerr("No datafile found, exiting")
exit()
if prefix == "":
rospy.logerr("No prefix found, exiting")
exit()
labelfile = rospack.get_path(
'gait_hmm_ros'
) + '/scripts/new_bags/datasets/' + prefix + '_labels.mat'
fpath = rospack.get_path(
'gait_hmm_ros') + '/scripts/new_bags/datasets/' + prefix + '_'
datafile = fpath + datafile
if datafile == "":
rospy.warn("No input file given, exiting")
exit()
if "bte" in datafile:
batch_test = 1
else:
batch_test = 0
if "btr" in datafile:
batch_train = 1
else:
batch_train = 0
x = SingleClassifier(normalized, folds, datafile, labelfile, prefix,
batch_test, batch_train)
def move_to_joint_positions(self,
positions,
timeout=10.0,
threshold=0.00085,
test=None):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter to smooth the movement.
@type positions: dict({str:float})
@param positions: joint_name:angle command
@type timeout: float
@param timeout: seconds to wait for move to finish [15]
@type threshold: float
@param threshold: position threshold in radians across each joint when
move is considered successful [0.008726646]
@param test: optional function returning True if motion must be aborted
"""
if self._params._in_sim:
rospy.warn(
"ArmInterface: move_to_joint_positions not implemented for simulation. Use set_joint_positions instead."
)
return
switch_ctrl = True if self._ctrl_manager.current_controller != self._ctrl_manager.joint_trajectory_controller else False
if switch_ctrl:
active_controllers = self._ctrl_manager.list_active_controllers(
only_motion_controllers=True)
for ctrlr in active_controllers:
self._ctrl_manager.stop_controller(ctrlr.name)
rospy.loginfo(
"ArmInterface: Stopping %s for trajectory controlling" %
ctrlr.name)
rospy.sleep(0.5)
if not self._ctrl_manager.is_loaded(
self._ctrl_manager.joint_trajectory_controller):
self._ctrl_manager.load_controller(
self._ctrl_manager.joint_trajectory_controller)
# if self._ctrl_manager.joint_trajectory_controller not in self._ctrl_manager.list_active_controller_names():
self._ctrl_manager.start_controller(
self._ctrl_manager.joint_trajectory_controller)
min_traj_dur = 0.5
traj_client = JointTrajectoryActionClient(
joint_names=self.joint_names(), ns=self._ns)
traj_client.clear()
dur = []
for j in range(len(self._joint_names)):
dur.append(
max(
abs(positions[self._joint_names[j]] -
self._joint_angle[self._joint_names[j]]) /
self._joint_limits.velocity[j], min_traj_dur))
traj_client.add_point(
positions=[positions[n] for n in self._joint_names],
time=max(dur) / self._speed_ratio)
def genf(joint, angle):
def joint_diff():
return abs(angle - self._joint_angle[joint])
return joint_diff
diffs = [
genf(j, a) for j, a in positions.items() if j in self._joint_angle
]
fail_msg = "ArmInterface: {0} limb failed to reach commanded joint positions.".format(
self.name.capitalize())
def test_collision():
if self.has_collided():
rospy.logerr(' '.join(["Collision detected.", fail_msg]))
return True
return False
traj_client.start() # send the trajectory action request
# traj_client.wait(timeout = timeout)
franka_dataflow.wait_for(
test=lambda: test_collision() or \
(callable(test) and test() == True) or \
(all(diff() < threshold for diff in diffs)),
timeout=timeout,
timeout_msg=fail_msg,
rate=100,
raise_on_error=False
)
rospy.sleep(0.5)
if switch_ctrl:
self._ctrl_manager.stop_controller(
self._ctrl_manager.joint_trajectory_controller)
for ctrlr in active_controllers:
self._ctrl_manager.start_controller(ctrlr.name)
rospy.loginfo("ArmInterface: Restaring %s" % ctrlr.name)
rospy.sleep(0.5)
rospy.loginfo("ArmInterface: Trajectory controlling complete")
def moduleCallback(msg):
sensor = msg.radiation_type
if (sensor > 3) or (sensor < 0):
rospy.warn("Invalid sensor: " + str(val))
sensors[sensor] = msg.range
display()
