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 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 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()