我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用rospy.logwarn()。
def update_telemetry(navsat_msg, compass_msg): """Telemetry subscription callback. Args: navsat_msg: sensor_msgs/NavSatFix message. compass_msg: std_msgs/Float64 message in degrees. """ try: client.post_telemetry(navsat_msg, compass_msg) except (ConnectionError, Timeout) as e: rospy.logwarn(e) return except (ValueError, HTTPError) as e: rospy.logerr(e) return except Exception as e: rospy.logfatal(e) return
def set_piksi_settings(self): save_needed = False for s in nested_dict_iter(self.piksi_update_settings): cval = self.piksi_settings[s[0][0]][s[0][1]] if len(cval) != 0: if cval != str(s[1]): rospy.loginfo('Updating piksi setting %s:%s to %s.' % (s[0][0], s[0][1], s[1])) self.piksi_set(s[0][0], s[0][1], s[1]) self.update_dr_param(s[0][0], s[0][1], s[1]) save_needed = True else: rospy.loginfo('Piksi setting %s:%s already set to %s.' % (s[0][0], s[0][1], s[1])) else: rospy.logwarn('Invalid piksi setting: %s' % ':'.join(s[0])) if self.piksi_save_settings and save_needed: rospy.loginfo('Saving piksi settings to flash') m = MsgSettingsSave() self.piksi_framer(m)
def _read_unit_offsets(self): if not rospy.has_param('~num_of_units'): rospy.logwarn("No unit offset parameters found!") num_of_units = rospy.get_param('~num_of_units', 0) self._unit_offsets = np.zeros((num_of_units, 3)) self._unit_coefficients = np.zeros((num_of_units, 2)) for i in xrange(num_of_units): unit_params = rospy.get_param('~unit_{}'.format(i)) x = unit_params['x'] y = unit_params['y'] z = unit_params['z'] self._unit_offsets[i, :] = [x, y, z] p0 = unit_params['p0'] p1 = unit_params['p1'] self._unit_coefficients[i, :] = [p0, p1] rospy.loginfo("Unit offsets: {}".format(self._unit_offsets)) rospy.loginfo("Unit coefficients: {}".format(self._unit_coefficients))
def send_reset_to_rovio_service_callback(self, request): response = std_srvs.srv.TriggerResponse() if self._automatic_rovio_reset_sent_once or self._send_reset_automatically: message = "Reset sent automatically after %d IMU messages, rosservice call refused." % \ (self._samples_before_reset) rospy.logwarn(rospy.get_name() + " " + message) response.success = False response.message = message elif self._num_imu_msgs_read <= 0: response.success = False response.message = "No external IMU message received, at least one single orientation is needed." elif self._num_gps_transform_msgs_read <= 0: response.success = False response.message = "No external GPS transform message received, at least one single position is needed." else: # everything's fine, send reset (success, message) = self.send_reset_to_rovio() response.success = success response.message = message return response
def wait(self): """ Waits for and verifies trajectory execution result """ #create a timeout for our trajectory execution #total time trajectory expected for trajectory execution plus a buffer last_time = self.goal.trajectory.points[-1].time_from_start.to_sec() time_buffer = rospy.get_param(self._param_ns + 'goal_time', 0.0) + 1.5 timeout = rospy.Duration(self._slow_move_offset + last_time + time_buffer) finish = self._limb_client.wait_for_result(timeout) result = (self._limb_client.get_result().error_code == 0) #verify result if all([finish, result]): return True else: msg = ("Trajectory action failed or did not finish before " "timeout/interrupt.") rospy.logwarn(msg) return False
def raise_event(self, perception_item, event): if not perception_item in self.conf_data.keys(): rospy.logwarn('<%s> perception is not member of perception configuration...'%perception_item) return if not event in self.conf_data[perception_item]['events']: rospy.logwarn('<%s> event is not member of event list of perception configuration...'%event) return if not self.is_enable_perception: return msg = ForwardingEvent() msg.header.stamp = rospy.Time.now() msg.event = event msg.by = perception_item self.pub_event.publish(msg)
def save_to_memory(self, perception_name, data={}): if data == {}: rospy.logwarn('Empty data inserted...') return for item in data.keys(): if not item in self.conf_data[perception_name]['data'].keys(): rospy.logwarn('[%s -- wrong data inserted [%s]...'%(perception_name, item)) return srv_req = WriteDataRequest() srv_req.perception_name = perception_name srv_req.data = json.dumps(data) srv_req.by = rospy.get_name() target_memory = self.conf_data[perception_name]['target_memory'] try: rospy.wait_for_service('/%s/write_data'%target_memory) self.dict_srv_wr[target_memory](srv_req) except rospy.ServiceException as e: pass
def read_from_memory(self, target_memory, data): if data == {}: rospy.logwarn('Empty data requested...') return req = ReadDataRequest() req.perception_name = data['perception_name'] req.query = data['query'] for item in data['data']: req.data.append(item) resonse = self.dict_srv_rd[target_memory](req) if not response.result: return {} results = json.loads(response.data) return results
def poll(self): if self.pseudo: self.o2 = 19.3 return if self.sensor_is_connected: try: self.serial.write(('adc read {}\r'.format(self.analog_port)).encode()) response = self.serial.read(25) voltage = float(response[10:-3]) * 5 / 1024 if voltage == 0: return self.o2 = voltage * 0.21 / 2.0 * 100 # percent rospy.logdebug('o2 = {}'.format(self.o2)) except: rospy.logwarn("O2 SENSOR> Failed to read value during poll") self.o2 = None self.sensor_is_connected = False else: self.connect()
def __init__(self): # ROS initialization: rospy.init_node('pose_manager') self.poseLibrary = dict() self.readInPoses() self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction, execute_cb=self.executeBodyPose, auto_start=False) self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction) if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)): try: rospy.wait_for_service("stop_walk_srv", timeout=2.0) self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty) except: rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. " +"This is normal if there is no nao_walker running.") self.stopWalkSrv = None self.poseServer.start() rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys()); else: rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?"); rospy.signal_shutdown("Required component missing");
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, memory_service, memory_key, latch=False, prefix='/qi/'): self.memory_service = memory_service try: self._pub = rospy.Publisher(prefix + memory_key, String, latch=latch, queue_size=1) self._sub = self.memory_service.subscriber(memory_key) self._sub.signal.connect(self._on_event) if latch: hist = self.memory_service.getEventHistory(memory_key) if len(hist) > 0: try: self._on_event(hist[-1][0]) except Exception: pass rospy.loginfo('subscribed to %s on Qi' % memory_key) except Exception as e: rospy.logwarn("Cannot set up for %s: %s" % (memory_key, str(e)))
def abort(self, req=None): """ Service function for the aborting the task '[name]_abort'. handles a shutdown of the task but instead of calling signal_complete, this method calls `signal_aborted`, which lets the task server know that it can proceed with launching a mayday task (as opposed to queueing up another general task). Args: msg (std_msgs.msg.String): the message received through the subscriber channel. """ if not self.active: logwarn("Can't abort {} because task isn't active.".format(self.name)) return False self.instruct() self.prep_shutdown(did_fail=True) return True
def wait(self): """ Waits for and verifies trajectory execution result """ #create a timeout for our trajectory execution #total time trajectory expected for trajectory execution plus a buffer last_time = self._r_goal.trajectory.points[-1].time_from_start.to_sec() time_buffer = rospy.get_param(self._param_ns + 'goal_time', 0.0) + 1.5 timeout = rospy.Duration(self._slow_move_offset + last_time + time_buffer) l_finish = self._left_client.wait_for_result(timeout) r_finish = self._right_client.wait_for_result(timeout) l_result = (self._left_client.get_result().error_code == 0) r_result = (self._right_client.get_result().error_code == 0) #verify result if all([l_finish, r_finish, l_result, r_result]): return True else: msg = ("Trajectory action failed or did not finish before " "timeout/interrupt.") rospy.logwarn(msg) return False
def set_position(self, position): """Accept position from 0 (open) to 100 (closed).""" position_turns = self.parse_percent_to_turn(position) goal = kinova_msgs.msg.SetFingersPositionGoal() goal.fingers.finger1 = float(position_turns[0]) goal.fingers.finger2 = float(position_turns[1]) if len(position)==3: goal.fingers.finger3 = float(position_turns[2]) else: goal.fingers.finger3 = 0.0 self.client.send_goal(goal) if self.client.wait_for_result(rospy.Duration(5.0)): return self.client.get_result() else: self.client.cancel_all_goals() rospy.logwarn(' the gripper action timed-out') return None
def execute_iteration(self, task, method, iteration, trial, num_iterations): rospy.logwarn("Controller starts iteration {} {}/{} trial {}".format(method, iteration, num_iterations, trial)) rospy.wait_for_service('ergo/reset') # Ensures Ergo keeps working or wait till it reboots # After resuming, we keep the same iteration if self.perception.has_been_pressed('buttons/help'): rospy.sleep(1.5) # Wait for the robot to fully stop self.recorder.record(task, method, trial, iteration) self.perception.switch_led('button_leds/pause', True) recording = self.perception.record(human_demo=True, nb_points=self.params['nb_points']) self.torso.set_torque_max(self.torso_params['torques']['reset']) self.torso.reset(slow=True) return True else: trajectory = self.learning.produce(skill_to_demonstrate=self.demonstrate).torso_trajectory self.torso.set_torque_max(self.torso_params['torques']['motion']) self.recorder.record(task, method, trial, iteration) self.perception.switch_led('button_leds/pause', True) self.torso.execute_trajectory(trajectory) # TODO: blocking, non-blocking, action server? recording = self.perception.record(human_demo=False, nb_points=self.params['nb_points']) self.perception.switch_led('button_leds/pause', False) recording.demo.torso_demonstration = JointTrajectory() self.torso.set_torque_max(80) self.torso.reset(slow=False) return self.learning.perceive(recording.demo)
def cb_produce(self, request): with self.lock_iteration: # Check if we need a new learner self.produce_init_learner() focus = copy(self.focus) rospy.loginfo("Learning node is requesting the current state") state = self.get_state(GetSensorialStateRequest()).state demonstrate = request.skill_to_demonstrate if demonstrate == "": rospy.loginfo("Learning node is producing...") w = self.learning.produce(self.translator.get_context(state), focus) else: rospy.logwarn("Assessing {}...".format(demonstrate)) context = self.translator.get_context(state) w = self.learning.produce(context, goal=demonstrate) trajectory_matrix = self.translator.w_to_trajectory(w) trajectory_msg = self.translator.matrix_to_trajectory_msg(trajectory_matrix) self.ready_for_interaction = True response = ProduceResponse(torso_trajectory=trajectory_msg) return response
def publish_obstacles(timer_event): """Requests and publishes obstacles. Args: timer_event: ROS TimerEvent. """ try: moving_obstacles, stationary_obstacles = client.get_obstacles( frame, lifetime) except (ConnectionError, Timeout) as e: rospy.logwarn(e) return except (ValueError, HTTPError) as e: rospy.logerr(e) return except Exception as e: rospy.logfatal(e) return moving_pub.publish(moving_obstacles) stationary_pub.publish(stationary_obstacles)
def get_active_mission(req): """ Service to update mission information with current active mission. Args: req: Request of type Trigger. Returns: TriggerResponse with true, false for success, failure. """ with lock: global msgs try: msgs = client.get_active_mission(frame) except (ConnectionError, Timeout) as e: rospy.logwarn(e) return False, str(e) except (ValueError, HTTPError) as e: rospy.logerr(e) return False, str(e) rospy.loginfo("Using active mission") return True, "Success"
def _send_data(self, channel, data, string_pattern): str_pat = 'I' if string_pattern != 's': str_pat += string_pattern packer = struct.Struct(str_pat) sent_vect = [channel] + data packed_data = packer.pack(*sent_vect) else: packer = struct.Struct(str_pat) sent_vect = [channel] packed_data = packer.pack(*sent_vect) packed_data += data sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP try: sock.sendto(packed_data, (self.ip, self.port)) except socket.gaierror: rospy.logwarn("Host not connected")
def goToAngles(self,angles,speed=DEFAULT_SPEED,joint_tolerance_plan=DEFAULT_JOINT_TOLERANCE_PLAN,joint_tolerance=DEFAULT_JOINT_TOLERANCE,speed_tolerance=DEFAULT_SPEED_TOLERANCE,timeout=DEFAULT_TIMEOUT,path_tolerance=0,stop_condition=None): """ joint_tolerance_plan,speed_tolerance are ignored, """ with self.moving_lock: self._moving=True if type(angles) is not dict: # print "converting to dict" d=getDictFromAngles(angles) else: # print "not converting to dict",angles d=dict(angles) angles=getAnglesFromDict(angles) rospy.logdebug("SimpleLimb %s goToAngles %s speed %f joint_tolerance %f speed_tolerance %f timeout %f"%(self.side,getStrFromAngles(angles),speed,joint_tolerance,speed_tolerance,timeout)) self.setSpeed(speed) try: ret=self.move_to_joint_positions(d,joint_tolerance,speed_tolerance,timeout,stop_condition=stop_condition) except Exception,e: rospy.logwarn( "Timeout PID: "+str(e)) ret=False with self.moving_lock: self._moving=False diff=getAnglesDiff(angles,self.getAngles()) rospy.logdebug("SimpleLimb %s goToAngles distance to target: %s"%(self.side,str(diff))) return ret
def device_serial_nr_cb(self, data): snr = data.data if self.serial_nr is not None: if not snr == self.serial_nr: rospy.logwarn('Got new serial Nr but already have one. Restart to calibrate a new cameara.') return # if the serial nr is available, the width and height are too self.res_height = rospy.get_param('/duo_node/resolution_height') self.res_width = rospy.get_param('/duo_node/resolution_width') self.left_image_label.setFixedSize(QSize(self.res_width, self.res_height)) self.right_image_label.setFixedSize(QSize(self.res_width, self.res_height)) rospy.loginfo('Got new serial Nr: {}'.format(snr)) self.status_bar_update.emit('Device serial Nr.: {}'.format(snr)) self.serial_nr = snr
def parse_parameter(self, testblock_name, params): """ Method that returns the metric method with the given parameter. :param params: Parameter """ metrics = [] if type(params) is not list: rospy.logerr("metric config not a list") return False for metric in params: # check for optional parameters try: groundtruth = metric["groundtruth"] groundtruth_epsilon = metric["groundtruth_epsilon"] except (TypeError, KeyError): rospy.logwarn("No groundtruth parameters given, skipping groundtruth evaluation for metric 'path_length' in testblock '%s'", testblock_name) groundtruth = None groundtruth_epsilon = None metrics.append(CalculatePathLength(metric["root_frame"], metric["measured_frame"], groundtruth, groundtruth_epsilon)) return metrics
def parse_parameter(self, testblock_name, params): """ Method that returns the metric method with the given parameter. :param params: Parameter """ metrics = [] if type(params) is not list: rospy.logerr("metric config not a list") return False for metric in params: # check for optional parameters try: groundtruth = metric["groundtruth"] groundtruth_epsilon = metric["groundtruth_epsilon"] except (TypeError, KeyError): rospy.logwarn("No groundtruth parameters given, skipping groundtruth evaluation for metric 'publish_rate' in testblock '%s'", testblock_name) groundtruth = None groundtruth_epsilon = None metrics.append(CalculatePublishRate(metric["topic"], groundtruth, groundtruth_epsilon)) return metrics
def dashwarn(msg, obj, title='Warning'): """ Logs a message with ``rospy.logwarn`` and displays a ``QMessageBox`` to the user :param msg: Message to display. :type msg: str :param obj: Parent object for the ``QMessageBox`` :type obj: QObject :param title: An optional title for the `QMessageBox`` :type title: str """ rospy.logwarn(msg) box = QMessageBox() box.setText(msg) box.setWindowTitle(title) box.show() obj._message_box = box
def check_vitals(self, stat): try: status = roboclaw.ReadError(self.address)[1] except OSError as e: rospy.logwarn("Diagnostics OSError: %d", e.errno) rospy.logdebug(e) return state, message = self.ERRORS[status] stat.summary(state, message) try: stat.add("Main Batt V:", float(roboclaw.ReadMainBatteryVoltage(self.address)[1] / 10)) stat.add("Logic Batt V:", float(roboclaw.ReadLogicBatteryVoltage(self.address)[1] / 10)) stat.add("Temp1 C:", float(roboclaw.ReadTemp(self.address)[1] / 10)) stat.add("Temp2 C:", float(roboclaw.ReadTemp2(self.address)[1] / 10)) except OSError as e: rospy.logwarn("Diagnostics OSError: %d", e.errno) rospy.logdebug(e) return stat # TODO: need clean shutdown so motors stop even if new msgs are arriving
def callback_external(self, msg, **metadata): if self.debug: rospy.loginfo("Received external SBP msg.") if self.piksi_framer: self.piksi_framer(msg, **metadata) else: rospy.logwarn("Received external SBP msg, but Piksi not connected.")
def main(): import signal rospy.init_node('uwb_multi_range_node') u = UWBMultiRange() def sigint_handler(sig, _): if sig == signal.SIGINT: u.stop() signal.signal(signal.SIGINT, sigint_handler) try: u.exec_() except (rospy.ROSInterruptException, select.error): rospy.logwarn("Interrupted... Stopping.")
def handle_multi_range_message(self, multi_range_msg): """Handle a ROS multi-range message by updating and publishing the state. Args: multi_range_msg (uwb.msg.UWBMultiRangeWithOffsets): ROS multi-range message. """ # Update tracker position based on time-of-flight measurements new_estimate = self.update_estimate(multi_range_msg) if new_estimate is None: rospy.logwarn('Could not compute initial estimate: address={}, remote_address={}'.format( multi_range_msg.address, multi_range_msg.remote_address)) else: # Publish tracker message ros_msg = uwb.msg.UWBTracker() ros_msg.header.stamp = rospy.get_rostime() ros_msg.address = multi_range_msg.address ros_msg.remote_address = multi_range_msg.remote_address ros_msg.state = new_estimate.state ros_msg.covariance = np.ravel(new_estimate.covariance) self.uwb_pub.publish(ros_msg) # Publish target transform (rotation is identity) self.tf_broadcaster.sendTransform( (new_estimate.state[0], new_estimate.state[1], new_estimate.state[2]), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.get_rostime(), self.target_frame, self.tracker_frame )
def main(): import signal rospy.init_node('uwb_tracker_node') u = UWBTracker() def sigint_handler(sig, _): if sig == signal.SIGINT: u.stop() signal.signal(signal.SIGINT, sigint_handler) try: u.exec_() except (rospy.ROSInterruptException, select.error): rospy.logwarn("Interrupted... Stopping.")
def read_settings(self): # Declination self._declination = math.radians(rospy.get_param('~declination_deg', 0.0)) # Calibration offset self._calibration_offset = np.array(rospy.get_param('~calibration_offset', [0.0, 0.0, 0.0])) # Calibration compensation self._calibration_compensation = np.array(rospy.get_param('~calibration_compensation', [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0])) # create matrix from array self._calibration_compensation = self._calibration_compensation.reshape(3,3) # Constant bearing offset self._bearing_offset = math.radians(rospy.get_param('~bearing_constant_offset_deg', 0.0)) # Number of samples used to compute average self._number_samples_average = rospy.get_param('~number_samples_average', 10) # Verbose self._verbose = rospy.get_param('~verbose', "True") # Print some useful information rospy.logwarn(rospy.get_name() + " declination: " + str(math.degrees(self._declination)) + " (deg)") rospy.logwarn(rospy.get_name() + " constant bearing offset added to final bearing: " + str(math.degrees(self._bearing_offset)) + " (deg)") if self._verbose: rospy.loginfo(rospy.get_name() + " calibration offset: " + str(self._calibration_offset)) rospy.loginfo(rospy.get_name() + " calibration compensation: \n" + str(self._calibration_compensation)) rospy.loginfo(rospy.get_name() + " number of samples to average: " + str(self._number_samples_average))
def compute_calibration(self): """ Computes the calibration through the ViSP service and returns it. :rtype: easy_handeye.handeye_calibration.HandeyeCalibration """ if len(self.samples) < HandeyeCalibrator.MIN_SAMPLES: rospy.logwarn("{} more samples needed! Not computing the calibration".format( HandeyeCalibrator.MIN_SAMPLES - len(self.samples))) return # Update data hand_world_samples, camera_marker_samples = self.get_visp_samples() if len(hand_world_samples.transforms) != len(camera_marker_samples.transforms): rospy.logerr("Different numbers of hand-world and camera-marker samples!") raise AssertionError rospy.loginfo("Computing from %g poses..." % len(self.samples)) try: result = self.calibrate(camera_marker_samples, hand_world_samples) rospy.loginfo("Computed calibration: {}".format(str(result))) transl = result.effector_camera.translation rot = result.effector_camera.rotation result_tuple = ((transl.x, transl.y, transl.z), (rot.x, rot.y, rot.z, rot.w)) ret = HandeyeCalibration(self.eye_on_hand, self.robot_base_frame, self.robot_effector_frame, self.tracking_base_frame, result_tuple) return ret except rospy.ServiceException as ex: rospy.logerr("Calibration failed: " + str(ex)) return None
def compute_calibration(self, _): self.last_calibration = self.calibrator.compute_calibration() # TODO: avoid confusion class/msg, change class into HandeyeCalibrationConversions ret = hec.srv.ComputeCalibrationResponse() if self.last_calibration is None: rospy.logwarn('No valid calibration computed, returning null') return ret ret.calibration.eye_on_hand = self.last_calibration.eye_on_hand ret.calibration.transform = self.last_calibration.transformation return ret
def __init__(self, arm, lights=True): """ @type arm: str @param arm: arm of gripper to control @type lights: bool @param lights: if lights should activate on cuff grasp """ self._arm = arm # inputs self._cuff = Cuff(limb=arm) # connect callback fns to signals self._lights = None if lights: self._lights = Lights() self._cuff.register_callback(self._light_action, '{0}_cuff'.format(arm)) try: self._gripper = Gripper(arm) if not (self._gripper.is_calibrated() or self._gripper.calibrate() == True): rospy.logerr("({0}_gripper) calibration failed.".format( self._gripper.name)) raise self._cuff.register_callback(self._close_action, '{0}_button_upper'.format(arm)) self._cuff.register_callback(self._open_action, '{0}_button_lower'.format(arm)) rospy.loginfo("{0} Cuff Control initialized...".format( self._gripper.name)) except: self._gripper = None msg = ("{0} Gripper is not connected to the robot." " Running cuff-light connection only.").format(arm.capitalize()) rospy.logwarn(msg)
def _command_joints(self, joint_names, point, start_time, dimensions_dict): if (self._limb.has_collided() or not self.robot_is_enabled() or not self._alive or self._cuff.cuff_button()): rospy.logerr("{0}: Robot arm in Error state. Stopping execution.".format( self._action_name)) self._limb.exit_control_mode() self._result.error_code = self._result.PATH_TOLERANCE_VIOLATED self._server.set_aborted(self._result) return False elif self._server.is_preempt_requested(): rospy.logwarn("{0}: Trajectory execution Preempted. Stopping execution.".format( self._action_name)) self._limb.exit_control_mode() self._server.set_preempted() return False velocities = [] deltas = self._get_current_error(joint_names, point.positions) for delta in deltas: if ((math.fabs(delta[1]) >= self._path_thresh[delta[0]] and self._path_thresh[delta[0]] >= 0.0)): rospy.logerr("%s: Exceeded Error Threshold on %s: %s" % (self._action_name, delta[0], str(delta[1]),)) self._result.error_code = self._result.PATH_TOLERANCE_VIOLATED self._server.set_aborted(self._result) self._limb.exit_control_mode() return False if self._mode == 'velocity': velocities.append(self._pid[delta[0]].compute_output(delta[1])) if self._mode == 'velocity': self._limb.set_joint_velocities(dict(zip(joint_names, velocities))) if self._mode == 'position': self._limb.set_joint_positions(dict(zip(joint_names, point.positions))) if self._mode == 'position_w_id': self._limb.set_joint_trajectory(joint_names, point.positions, point.velocities, point.accelerations) return True
def version_check(self): """ Verifies the version of the software running on the robot is compatible with this local version of the Intera SDK. Currently uses the variables in intera_interface.settings and can be overridden for all default examples by setting CHECK_VERSION to False. @rtype: bool @return: Returns True if SDK version is compatible with robot Version, False otherwise """ param_name = "/manifest/robot_software/version/HLR_VERSION_STRING" sdk_version = settings.SDK_VERSION # get local lock for rosparam threading bug with self.__class__.param_lock: robot_version = rospy.get_param(param_name, None) if not robot_version: rospy.logwarn("RobotEnable: Failed to retrieve robot version " "from rosparam: %s\n" "Verify robot state and connectivity " "(i.e. ROS_MASTER_URI)", param_name) return False else: # parse out first 3 digits of robot version tag pattern = ("^([0-9]+)\.([0-9]+)\.([0-9]+)") match = re.search(pattern, robot_version) if not match: rospy.logwarn("RobotEnable: Invalid robot version: %s", robot_version) return False robot_version = match.string[match.start(1):match.end(3)] if robot_version not in settings.VERSIONS_SDK2ROBOT[sdk_version]: errstr_version = """RobotEnable: Software Version Mismatch. Robot Software version (%s) does not match local SDK version (%s). Please Update your Robot Software. \ See: http://sdk.rethinkrobotics.com/intera/Software_Update""" rospy.logerr(errstr_version, robot_version, sdk_version) return False return True
def publish_command(self, op, args, timeout=2.0): """ publish on the command topic return true if the command is acknowleged within the timeout """ cmd_time = rospy.Time.now() self.cmd_times.append(cmd_time) self.cmd_times = self.cmd_times[-100:] cmd_msg = IOComponentCommand( time=cmd_time, op=op, args=json.dumps(args)) rospy.logdebug("publish_command %s %s" % (cmd_msg.op, cmd_msg.args)) if timeout != None: timeout_time = rospy.Time.now() + rospy.Duration(timeout) while not rospy.is_shutdown(): self._command_pub.publish(cmd_msg) if self.is_state_valid(): if cmd_time in self.state.commands: rospy.logdebug("command %s acknowleged" % (cmd_msg.op,)) return True rospy.sleep(0.1) if timeout_time < rospy.Time.now(): rospy.logwarn("Timed out waiting for command acknowlegment...") break return False return True
def preempt_callback(self): rospy.logwarn('\033[94m[%s]\033[0m rendering preempted.'%rospy.get_name()) for k in self.is_rendering.keys(): if self.is_rendering[k]: self.render_client[k].cancel_all_goals()
def new_goal_callback(self, goal_pose): if not self.is_working: self.is_working = True new_goal = State.from_pose(goal_pose.pose) if self.map is not None and self.map.is_allowed(new_goal, self.robot): self.goal = new_goal rospy.loginfo("New goal was set") if self.ready_to_plan(): self.replan() else: rospy.logwarn("New goal is bad or no map available") self.is_working = False
def new_start_callback(self, start_pose): if not self.is_working: self.is_working = True new_start = State.from_pose(start_pose.pose.pose) if self.map is not None and self.map.is_allowed(new_start, self.robot): self.start = new_start rospy.loginfo("New start was set") if self.ready_to_plan(): self.replan() else: rospy.logwarn("New start is bad or no map available") self.is_working = False
def handle_process(self, proc, err): """ Takes a running subprocess.Popen object `proc`, rosdebugs everything it prints to stdout, roswarns everything it prints to stderr, and raises `err` if it fails """ poll = select.poll() poll.register(proc.stdout) poll.register(proc.stderr) while proc.poll() is None and not rospy.is_shutdown(): res = poll.poll(1) for fd, evt in res: if not (evt & select.POLLIN): continue if fd == proc.stdout.fileno(): line = proc.stdout.readline().strip() if line: rospy.logdebug(line) elif fd == proc.stderr.fileno(): line = proc.stderr.readline().strip() if line: rospy.logwarn(line) if proc.poll(): proc.terminate() proc.wait() raise RuntimeError("Process interrupted by ROS shutdown") if proc.returncode: raise err
def connect_serial(): timeout_s = 2 / serial_rate_hz # serial port timeout is 2x loop rate baud_rate = rospy.get_param("~baud_rate", 115200) # Initialize the serial connection path = "/dev/serial/by-id" port = None close_serial() while port is None: try: if not os.path.exists(path): raise Exception("No serial device found on system in {}".format(path)) ports = [port for port in os.listdir(path) if "arduino" in port.lower()] if len(ports) == 0: raise Exception("No arduino device found on system in {}".format(path)) port = ports[0] serial_connection = serial.Serial(os.path.join(path, port), baud_rate, dsrdtr = True, # Causes an Arduino soft reset timeout = timeout_s, writeTimeout = timeout_s) return serial_connection except Exception as e: rospy.logwarn(e) rospy.sleep(0.2) #seconds
def connect(self): if self.pseudo: rospy.loginfo('Connected to pseudo sensor') return try: self.serial = serial.Serial(self.serial_port, 19200, timeout=1) rospy.logdebug("self.serial.isOpen() = {}".format(self.serial.isOpen())) if not self.sensor_is_connected: self.sensor_is_connected = True rospy.loginfo('Connected to sensor') except: if self.sensor_is_connected: self.sensor_is_connected = False rospy.logwarn('Unable to connect to sensor')
def refresh(self): try: self.event_queue = pygame.event.get() self.screen.fill(self.black) self.blitSensorValues() self.blitDesiredUI() pygame.display.update() except Exception as e: rospy.logwarn("Refresh crashed. Error: {}".format(e))
def connect(self): try: if self.device_id is None: # Try to find pH device id automatically devices = list_devices.get_ftdi_device_list() # Check if any atlas devices are connected to device if len(devices) == 0: raise IOError("No atlas device found on system") for device in devices: # Extract id device_id = (device.split("FTDI:FT230X Basic UART:"))[1] # Check if device is ph sensor with AtlasDevice(device_id) as atlas_device: atlas_device.send_cmd("i") # get device information time.sleep(1) lines = atlas_device.read_lines() if len(lines) == 0: continue if "EC" in lines[0]: self.device_id = device_id rospy.loginfo("Automatically found device id: {}".format(device_id)) self.device = AtlasDevice(self.device_id) self.device.send_cmd("C,0") # turn off continuous mode time.sleep(1) self.device.flush() rospy.loginfo("Connected to AtlasEc") except Exception as e: rospy.logwarn("Failed to connect to AtlasEc. Error: {}".format(e)) pass
