我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用rospy.logdebug()。
def test_publish_to_topics(self): topic_ending = "desired" rospy.logdebug("Sleeping for 5 seconds to let ROS spin up...") rospy.sleep(5) for variable, value in self.variables: # Publish to each variable/desired topic to see if all of the # actuators come on as expected. topic_string = variable + "/" + topic_ending rospy.logdebug("Testing Publishing to " + topic_string) pub_desired = rospy.Publisher(topic_string, Float64, queue_size=10) sub_desired = rospy.Subscriber(topic_string, Float64, self.callback) rospy.sleep(2) print(self.namespace + "/" + topic_string) for _ in range(NUM_TIMES_TO_PUBLISH): pub_desired.publish(value) rospy.sleep(1) rospy.sleep(2) pub_desired.publish(0)
def handleTargetPose(self, data, post=True): """handles cmd_pose requests, walks to (x,y,theta) in robot coordinate system""" rospy.logdebug("Walk target_pose: %f %f %f", data.x, data.y, data.theta) self.gotoStartWalkPose() try: if post: self.motionProxy.post.moveTo(data.x, data.y, data.theta, self.footGaitConfig) else: self.motionProxy.moveTo(data.x, data.y, data.theta, self.footGaitConfig) return True except RuntimeError,e: rospy.logerr("Exception caught in handleTargetPose:\n%s", e) return False
def handleStep(self, data): rospy.logdebug("Step leg: %d; target: %f %f %f", data.leg, data.pose.x, data.pose.y, data.pose.theta) try: if data.leg == StepTarget.right: leg = "RLeg" elif data.leg == StepTarget.left: leg = "LLeg" else: rospy.logerr("Received a wrong leg constant: %d, ignoring step command", data.leg) return self.motionProxy.stepTo(leg, data.pose.x, data.pose.y, data.pose.theta) return True except RuntimeError, e: rospy.logerr("Exception caught in handleStep:\n%s", e) return False
def update_with_categorical_distribution(self, recognition): """ Update the recognition with a categorical distribution of the trained faces :param recognition: Input recognition :return: Output recognition with an updated categorical distribution """ if self._trained_faces: # Try to get a representation of the detected face recognition_representation = None try: recognition_representation = self._get_representation(recognition.image) except Exception as e: print "Could not get representation of face image but detector found one: %s" % str(e) rospy.logdebug('recognition_representation: %s', recognition_representation) # If we have a representation, update with use of the l2 distance w.r.t. the face dict if recognition_representation is not None: recognition.l2_distances = [L2Distance(_get_min_l2_distance( face.representations, recognition_representation), face.label) for face in self._trained_faces] # Sort l2 distances probabilities, lowest on index 0 recognition.l2_distances = sorted(recognition.l2_distances, key=lambda x: x.distance) return recognition
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 waitForInitialPose(self, next_topic, timeout=None): counter = 0 while not rospy.is_shutdown(): counter = counter + 1 if timeout and counter >= timeout: return False try: self.marker_lock.acquire() self.initialize_poses = True topic_suffix = next_topic.split("/")[-1] if self.initial_poses.has_key(topic_suffix): self.pre_pose = PoseStamped(pose=self.initial_poses[topic_suffix]) self.initialize_poses = False return True else: rospy.logdebug(self.initial_poses.keys()) rospy.loginfo("Waiting for pose topic of '%s' to be initialized", topic_suffix) rospy.sleep(1) finally: self.marker_lock.release()
def pick(self, pose, direction=(0, 0, 1), distance=0.1, controller=None): """Go to pose + pick_direction * pick_distance, open, go to pose, close, go to pose + pick_direction * pick_distance. """ rospy.logdebug("pick pose: %s" % pose) pregrasp_pose = self.translate(pose, direction, distance) rospy.logdebug("pregrasp_pose: %s" % pregrasp_pose) rospy.sleep(1) self.move_ik(pregrasp_pose) # We want to block end effector opening so that the next # movement happens with the gripper fully opened. self.gripper.open(block=True) self.move_ik(pose) if controller is not None: print ('controller ON!!') controller.enable() rospy.sleep(5) controller.disable() self.gripper.close(block=True) self.move_ik(pregrasp_pose)
def alvarcb(self, markers): rospy.logdebug("Detected markers!") # can we find the correct marker? for m in markers.markers: if m.id == self.marker_id: odom_meas = Odometry() odom_meas.header.frame_id = self.frame_id m.pose.header.frame_id = self.camera_frame_id odom_meas.child_frame_id = self.base_frame_id odom_meas.header.stamp = m.header.stamp m.pose.header.stamp = m.header.stamp # now we need to transform this pose measurement from the camera # frame into the frame that we are reporting measure odometry in pose_transformed = self.transform_pose(m.pose) if pose_transformed is not None: odom_meas.pose.pose = pose_transformed.pose # Now let's add our offsets: odom_meas = odom_conversions.odom_add_offset(odom_meas, self.odom_offset) self.meas_pub.publish(odom_meas) self.send_transforms(odom_meas) self.publish_path(m.pose) return
def login(self): """Authenticates with the server. Raises: Timeout: On timeout. HTTPError: On request failure. ConnectionError: On connection failure. """ method = "POST" uri = "/api/login" response = self.session.request( method=method, url=self.url + uri, timeout=self.timeout, verify=self.verify, data=self.__credentials) message = self._get_response_log_message(method, uri, response) try: response.raise_for_status() rospy.logdebug(message) except requests.HTTPError: # For more human-readable error messages. raise requests.HTTPError(message, response=response)
def _joy_cb(self, msg): """ The joy/game-pad message callback. :type msg: Joy :param msg: The incoming joy message. """ if msg.buttons[self._head_button] == 1: angle_deg = ((msg.axes[self._head_axes] + 1.0) / 2.0) * SUM_HEAD_ANGLE + MIN_HEAD_ANGLE rospy.logdebug('Send head angle: {} degrees.'.format(angle_deg)) self._head_pub.publish(data=angle_deg) if msg.buttons[self._lift_button] == 1: lift_mm = abs(msg.axes[self._lift_axes]) * SUM_LIFT_HEIGHT + MIN_LIFT_HEIGHT rospy.logdebug('Send lift height: {} mm.'.format(lift_mm)) self._lift_pub.publish(data=abs(msg.axes[self._lift_axes]))
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 get_current_volume(self): cmd = ShellCmd(self.base_get_cmd) while not cmd.is_done() and not rospy.is_shutdown(): rospy.sleep(0.1) output = cmd.get_stdout() rospy.logdebug("self.base_get_cmd output: " + str(output)) # Output looks like: # Simple mixer control 'Master',0 # Capabilities: pvolume pvolume-joined pswitch pswitch-joined # Playback channels: Mono # Limits: Playback 0 - 87 # Mono: Playback 44 [51%] [-32.25dB] [on] pct_idx = output.index('%') # At most 3 characters of 100% pct_text = output[pct_idx - 3:pct_idx] # Remove [ if it was less than 3 chars pct_text = pct_text.replace('[', '') # Remove spaces if it was less than 2 char pct_text = pct_text.strip() curr_vol = int(pct_text) return curr_vol
def purge(self): rospy.logdebug("### ENTRY purge ###") # wait for old transition to finish before processing new one self._wait_for_transition_is_done() if not self.atf_started: raise ATFTestblockError("Calling purge for testblock '%s' before ATF has been started." % self.name) if self.get_state() in self.m.endStates: raise ATFTestblockError("Calling purge for testblock '%s' while testblock already stopped." % self.name) # set new transition trigger t = TestblockTrigger() t.stamp = rospy.Time.now() t.name = self.name t.trigger = TestblockTrigger.PURGE self.trigger = t rospy.logdebug("### EXIT purge ###")
def start(self): rospy.logdebug("### ENTRY start ###") # wait for old transition to finish before processing new one self._wait_for_transition_is_done() if not self.atf_started: raise ATFTestblockError("Calling start for testblock '%s' before ATF has been started." % self.name) if self.get_state() in self.m.endStates: raise ATFTestblockError("Calling start for testblock '%s' while testblock already stopped." % self.name) # set new transition trigger t = TestblockTrigger() t.stamp = rospy.Time.now() t.name = self.name t.trigger = TestblockTrigger.START self.trigger = t rospy.logdebug(" start call with trigger : '%s'", self.trigger.trigger) rospy.logdebug("### EXIT start ###")
def pause(self): rospy.logdebug("### ENTRY pause ###") # wait for old transition to finish before processing new one self._wait_for_transition_is_done() if not self.atf_started: raise ATFTestblockError("Calling pause for testblock '%s' before ATF has been started." % self.name) if self.get_state() in self.m.endStates: raise ATFTestblockError("Calling pause for testblock '%s' while testblock already stopped." % self.name) # set new transition trigger t = TestblockTrigger() t.stamp = rospy.Time.now() t.name = self.name t.trigger = TestblockTrigger.PAUSE self.trigger = t rospy.logdebug("### EXIT pause ###")
def _purged_state(self): rospy.logdebug("*** ENTRY _purged_state ***") self._wait_while_transition_is_active() #self.recorder_handle.record_trigger(self.trigger) if self.trigger.trigger == TestblockTrigger.START: self._start() new_state = TestblockState.ACTIVE elif self.trigger.trigger == TestblockTrigger.STOP: rospy.logdebug("Stopping testblock is called from _purged_state") self._stop() new_state = TestblockState.SUCCEEDED else: message = "testblock '%s': invalid transition '%s' from state '%s'" % (self.trigger.name, str(self.trigger.trigger), self.get_state()) rospy.logerr(message) new_state = TestblockState.ERROR self.exception = message raise ATFTestblockError(message) rospy.logdebug(" _purged_state trigger : '%s'", self.trigger.trigger) self.trigger = None rospy.logdebug(" _purged_state after trigger = None : '%s'", self.trigger) rospy.logdebug("*** EXIT _purged_state ***") return new_state
def _active_state(self): rospy.logdebug("*** ENTRY _active_state ***") self._wait_while_transition_is_active() #self.recorder_handle.record_trigger(self.trigger) if self.trigger.trigger == TestblockTrigger.PURGE: self._purge() new_state = TestblockState.PURGED elif self.trigger.trigger == TestblockTrigger.PAUSE: self._pause() new_state = TestblockState.PAUSED elif self.trigger.trigger == TestblockTrigger.STOP: rospy.logdebug("Stopping testblock is called from _active_state") self._stop() new_state = TestblockState.SUCCEEDED else: message = "testblock '%s': invalid transition '%s' from state '%s'" % (self.trigger.name, str(self.trigger.trigger), self.get_state()) rospy.logerr(message) new_state = TestblockState.ACTIVE self.exception = message raise ATFTestblockError(message) rospy.logdebug(" _active_state trigger - _active_state : '%s'", self.trigger.trigger) self.trigger = None rospy.logdebug(" _active_state after trigger = None : '%s'", self.trigger) rospy.logdebug("*** EXIT _active_state ***") return new_state
def _paused_state(self): rospy.logdebug("*** ENTRY _paused_state ***") self._wait_while_transition_is_active() #self.recorder_handle.record_trigger(self.trigger) if self.trigger.trigger == TestblockTrigger.PURGE: self._purge() new_state = TestblockState.PURGED elif self.trigger.trigger == TestblockTrigger.START: self._start() new_state = TestblockState.ACTIVE elif self.trigger.trigger == TestblockTrigger.STOP: rospy.logdebug("Stopping testblock is called from _paused_state") self._stop() new_state = TestblockState.SUCCEEDED else: message = "testblock '%s': invalid transition '%s' from state '%s'" % (self.trigger.name, str(self.trigger.trigger), self.get_state()) rospy.logerr(message) new_state = TestblockState.ERROR self.exception = message raise ATFTestblockError(message) rospy.logdebug(" _paused_state trigger - _active_state : '%s'", self.trigger.trigger) self.trigger = None rospy.logdebug(" _paused_state after trigger = None : '%s'", self.trigger) rospy.logdebug("*** EXIT _paused_state ***") return new_state
def shutdown(self): """ This needs to be called whenever this class terminates. This closes all the instances on all trees. Also unregisters ROS' subscriber, stops timer. """ rospy.logdebug('RobotMonitorWidget in shutdown') names = self._inspectors.keys() for name in names: self._inspectors[name].close() if self._timeline: self._timeline.shutdown() self._timer.stop() del self._timer
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 update_filter(self, timestep, estimate, ranges): """Update position filter. Args: timestep (float): Time elapsed since last update. estimate (StateEstimate): Position estimate to update. ranges (list of floats): Range measurements. Returns: new_estimate (StateEstimate): Updated position estimate. outlier_flag (bool): Flag indicating whether the measurement was rejected as an outlier. """ num_of_units = len(ranges) x = estimate.state P = estimate.covariance # Compute process matrix and covariance matrices F, Q, R = self._compute_process_and_covariance_matrices(timestep) # rospy.logdebug('F: {}'.format(F)) # rospy.logdebug('Q: {}'.format(Q)) # rospy.logdebug('R: {}'.format(R)) # Prediction x = np.dot(F, x) P = np.dot(F, np.dot(P, F.T)) + Q # Update n = np.copy(x) H = np.zeros((num_of_units, x.size)) z = np.zeros((num_of_units, 1)) h = np.zeros((num_of_units, 1)) for i in xrange(self.ikf_iterations): n, K, outlier_flag = self._ikf_iteration(x, n, ranges, h, H, z, estimate, R) if outlier_flag: new_estimate = estimate else: new_state = n new_covariance = np.dot((np.eye(6) - np.dot(K, H)), P) new_estimate = UWBTracker.StateEstimate(new_state, new_covariance) return new_estimate, outlier_flag
def Start(self): rospy.logdebug("Starting") self._SerialDataGateway.Start() #######################################################################################################################
def Stop(self): rospy.logdebug("Stopping") self._SerialDataGateway.Stop() #######################################################################################################################
def handleCmdVel(self, data): rospy.logdebug("Walk cmd_vel: %f %f %f, frequency %f", data.linear.x, data.linear.y, data.angular.z, self.stepFrequency) if data.linear.x != 0 or data.linear.y != 0 or data.angular.z != 0: self.gotoStartWalkPose() try: eps = 1e-3 # maybe 0,0,0 is a special command in motionProxy... if abs(data.linear.x)<eps and abs(data.linear.y)<eps and abs(data.angular.z)<eps: self.motionProxy.moveToward(0,0,0,[["Frequency",0.5]]) else: if data.linear.x>=eps and abs(data.linear.y)<eps and abs(data.angular.z)<eps: self.motionProxy.angleInterpolationWithSpeed("HeadPitch", self.headPitchAngle, 0.5) self.motionProxy.moveToward(data.linear.x, data.linear.y, data.angular.z, [["Frequency", self.stepFrequency]] + self.footGaitConfig) rospy.loginfo('case forward') elif data.linear.x<=-eps and abs(data.linear.y)<eps and abs(data.angular.z)<eps: self.motionProxy.angleInterpolationWithSpeed("HeadPitch", self.headPitchAngleBack, 0.5) self.motionProxy.moveToward(data.linear.x, data.linear.y, data.angular.z, [["Frequency", self.stepFrequencyBack]] + self.footGaitConfigBack) rospy.loginfo('case backward') elif abs(data.linear.x)<eps and abs(data.linear.y)<eps and data.angular.z>=-eps: self.motionProxy.angleInterpolationWithSpeed("HeadPitch", self.headPitchAngleLeft, 0.5) self.motionProxy.moveToward(data.linear.x, data.linear.y, data.angular.z, [["Frequency", self.stepFrequencyLeft]] + self.footGaitConfigLeft) rospy.loginfo('case left') elif abs(data.linear.x)<eps and abs(data.linear.y)<eps and data.angular.z<=eps: self.motionProxy.angleInterpolationWithSpeed("HeadPitch", self.headPitchAngleRight, 0.5) self.motionProxy.moveToward(data.linear.x, data.linear.y, data.angular.z, [["Frequency", self.stepFrequencyRight]] + self.footGaitConfigRight) rospy.loginfo('case right') else: self.motionProxy.angleInterpolationWithSpeed("HeadPitch", self.headPitchAngle, 0.5) self.motionProxy.moveToward(data.linear.x, data.linear.y, data.angular.z, [["Frequency", self.stepFrequency]] + self.footGaitConfig) rospy.loginfo('case else') except RuntimeError,e: # We have to assume there's no NaoQI running anymore => exit! rospy.logerr("Exception caught in handleCmdVel:\n%s", e) rospy.signal_shutdown("No NaoQI available anymore")
def _open_action(self, value): if value and self._gripper.is_ready(): rospy.logdebug("gripper open triggered") self._gripper.open() if self._lights: self._set_lights('red', False) self._set_lights('green', True)
def _close_action(self, value): if value and self._gripper.is_ready(): rospy.logdebug("gripper close triggered") self._gripper.close() if self._lights: self._set_lights('green', False) self._set_lights('red', True)
def _light_action(self, value): if value: rospy.logdebug("cuff grasp triggered") else: rospy.logdebug("cuff release triggered") if self._lights: self._set_lights('red', False) self._set_lights('green', False) self._set_lights('blue', value)
def display_image(self, image_path, display_in_loop=False, display_rate=1.0): """ Displays image(s) to robot's head @type image_path: list @param image_path: the relative or absolute file path to the image file(s) @type display_in_loop: bool @param display_in_loop: Set True to loop through all image files infinitely @type display_rate: float @param display_rate: the rate to publish image into head """ rospy.logdebug("Display images in loop:'{0}', frequency: '{1}'".format(display_in_loop, display_rate)) image_msg = [] image_list = image_path if isinstance(image_path, list) else [image_path] for one_path in image_list: cv_img = self._setup_image(one_path) if cv_img: image_msg.append(cv_img) if not image_msg: rospy.logerr("Image message list is empty") else: r = rospy.Rate(display_rate) while not rospy.is_shutdown(): for one_msg in image_msg: self._image_pub.publish(one_msg) r.sleep() if not display_in_loop: break
def __init__(self, path_root, config_msg_type, status_msg_type): self._path = path_root self.config_mutex = Lock() self.state_mutex = Lock() self.cmd_times = [] self.ports = dict() self.signals = dict() self.state = None self.config = None self.state_changed = intera_dataflow.Signal() self.config_changed = intera_dataflow.Signal() self._config_sub = rospy.Subscriber(self._path + "/config", config_msg_type, self.handle_config) self._state_sub = rospy.Subscriber(self._path + "/state", status_msg_type, self.handle_state) self._command_pub = rospy.Publisher(self._path + "/command", IOComponentCommand, queue_size=10) # Wait for the state to be populated intera_dataflow.wait_for( lambda: not self.state is None, timeout=5.0, timeout_msg=("Failed to get state.") ) rospy.logdebug("Making new IOInterface on %s" % (self._path,))
def is_oneiric(): if path.exists('/etc/issue'): rospy.logdebug('/etc/issue exists') with open('/etc/issue') as f: contents = f.readline() rospy.logdebug('contents are {0}'.format(contents)) return '11.10' in contents else: rospy.logdebug('/etc/issue does not exist')
def is_lucid_or_maverick(): if path.exists('/etc/issue'): rospy.logdebug('/etc/issue exists') with open('/etc/issue') as f: contents = f.readline() rospy.logdebug('contents are {0}'.format(contents)) return '10.04' in contents or '10.10' in contents else: rospy.logdebug('/etc/issue does not exist')
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 setUp(self): self.namespace = rospy.get_namespace() rospy.logdebug("Initializing test_publish_to_topics in namespace:" + self.namespace) self.variables = [("air_flush_on", 1), ("air_temperature", 23), ("light_intensity_blue", 1), ("light_intensity_red", 1), ("light_intensity_white", 1), ("nutrient_flora_duo_a", 5), ("nutrient_flora_duo_b", 5), ("water_potential_hydrogen", 6) ] # self.topic_ending = ["raw", "measured", "commanded", "desired"] rospy.init_node(NAME, log_level=rospy.DEBUG)
def callback(self, data): print(data) rospy.logdebug("Received message: {}".format(data))
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 trace(msg, *args): if TRACE: msg = '\nTRACE> ' + msg rospy.logdebug(msg, *args)
def handleJointAngles(self, msg): rospy.logdebug("Received a joint angle target") try: # Note: changeAngles() and setAngles() are non-blocking functions. if (msg.relative==0): self.motionProxy.setAngles(list(msg.joint_names), list(msg.joint_angles), msg.speed) else: self.motionProxy.changeAngles(list(msg.joint_names), list(msg.joint_angles), msg.speed) except RuntimeError,e: rospy.logerr("Exception caught:\n%s", e)
def handleJointStiffness(self, msg): rospy.logdebug("Received a joint angle stiffness") try: self.motionProxy.setStiffnesses(list(msg.name), list(msg.effort)) except RuntimeError,e: rospy.logerr("Exception caught:\n%s", e)
def executeJointTrajectoryAction(self, goal): rospy.loginfo("JointTrajectory action executing"); names, angles, times = self.jointTrajectoryGoalMsgToAL(goal) rospy.logdebug("Received trajectory for joints: %s times: %s", str(names), str(times)) rospy.logdebug("Trajectory angles: %s", str(angles)) task_id = None running = True #Wait for task to complete while running and not self.jointTrajectoryServer.is_preempt_requested() and not rospy.is_shutdown(): #If we haven't started the task already... if task_id is None: # ...Start it in another thread (thanks to motionProxy.post) task_id = self.motionProxy.post.angleInterpolation(names, angles, times, (goal.relative==0)) #Wait for a bit to complete, otherwise check we can keep running running = self.motionProxy.wait(task_id, self.poll_rate) # If still running at this point, stop the task if running and task_id: self.motionProxy.stop( task_id ) jointTrajectoryResult = JointTrajectoryResult() jointTrajectoryResult.goal_position.header.stamp = rospy.Time.now() jointTrajectoryResult.goal_position.position = self.motionProxy.getAngles(names, True) jointTrajectoryResult.goal_position.name = names if not self.checkJointsLen(jointTrajectoryResult.goal_position): self.jointTrajectoryServer.set_aborted(jointTrajectoryResult) rospy.logerr("JointTrajectory action error in result: sizes mismatch") elif running: self.jointTrajectoryServer.set_preempted(jointTrajectoryResult) rospy.logdebug("JointTrajectory preempted") else: self.jointTrajectoryServer.set_succeeded(jointTrajectoryResult) rospy.loginfo("JointTrajectory action done")
def executeJointStiffnessAction(self, goal): rospy.loginfo("JointStiffness action executing"); names, angles, times = self.jointTrajectoryGoalMsgToAL(goal) rospy.logdebug("Received stiffness trajectory for joints: %s times: %s", str(names), str(times)) rospy.logdebug("stiffness values: %s", str(angles)) task_id = None running = True #Wait for task to complete while running and not self.jointStiffnessServer.is_preempt_requested() and not rospy.is_shutdown(): #If we haven't started the task already... if task_id is None: # ...Start it in another thread (thanks to motionProxy.post) task_id = self.motionProxy.post.stiffnessInterpolation(names, angles, times) #Wait for a bit to complete, otherwise check we can keep running running = self.motionProxy.wait(task_id, self.poll_rate) # If still running at this point, stop the task if running and task_id: self.motionProxy.stop( task_id ) jointStiffnessResult = JointTrajectoryResult() jointStiffnessResult.goal_position.header.stamp = rospy.Time.now() jointStiffnessResult.goal_position.position = self.motionProxy.getStiffnesses(names) jointStiffnessResult.goal_position.name = names if not self.checkJointsLen(jointStiffnessResult.goal_position): self.jointStiffnessServer.set_aborted(jointStiffnessResult) rospy.logerr("JointStiffness action error in result: sizes mismatch") elif running: self.jointStiffnessServer.set_preempted(jointStiffnessResult) rospy.logdebug("JointStiffness preempted") else: self.jointStiffnessServer.set_succeeded(jointStiffnessResult) rospy.loginfo("JointStiffness action done")
def onTactileChanged(self, strVarName, value, strMessage): "Called when tactile touch status changes in ALMemory" if strVarName == "FrontTactilTouched": self.tactile.button = self.tactileTouchFrontButton elif strVarName == "MiddleTactilTouched": self.tactile.button = self.tactileTouchMiddleButton elif strVarName == "RearTactilTouched": self.tactile.button = self.tactileTouchRearButton self.tactile.state = int(value); self.tactilePub.publish(self.tactile) rospy.logdebug("tactile touched: name=%s, value=%d, message=%s.", strVarName, value, strMessage);
def onBumperChanged(self, strVarName, value, strMessage): "Called when bumper status changes in ALMemory" if strVarName == "RightBumperPressed": self.bumper.bumper = self.bumperRightButton elif strVarName == "LeftBumperPressed": self.bumper.bumper = self.bumperLeftButton self.bumper.state = int(value); self.bumperPub.publish(self.bumper) rospy.logdebug("bumper pressed: name=%s, value=%d, message=%s.", strVarName, value, strMessage);
def _image_cb(self, img): self._image_counter += 1 if self._image_counter > self._THROTTLE: rospy.logdebug('publish image') self._image_pub.publish(img) self._image_counter = 0
def _depth_cb(self, img): self._depth_counter += 1 if self._depth_counter > self._THROTTLE: rospy.logdebug('publish depth') self._depth_pub.publish(img) self._depth_counter = 0
def _depth_info_cb(self, info): rospy.logdebug('publish depth info') self._depth_info_pub.publish(info)
def __init__(self): if not hasattr(self, 'writer'): rospy.logdebug("initializing summary writer.") now = datetime.datetime.now() self.directory = self.get_output_folder('summaries') + now.strftime("/%Y-%m-%d-%s") self.writer = tf.train.SummaryWriter(self.directory)
def __init__(self, batch_size=1, output_size=[28, 28], input=""): self.name = 'inputlayer-%08x' % random.getrandbits(32) self.output_size = output_size self.input = input self.batch_size = batch_size self.batch = [] with tf.name_scope(self.name) as n_scope: self.name_scope = n_scope self.input_placeholder = tf.placeholder(dtype=tf.float32, shape=(self.batch_size, output_size[0], output_size[1], 1), name='input') rospy.logdebug("?? Input Layer initalized")
def _robot_reboot(self): """ Perform a soft-reset of the Create """ msg ="Soft-rebooting turtlebot to passive mode." rospy.logdebug(msg) self._diagnostics.node_status(msg,"warn") self._set_digital_outputs([0, 0, 0]) self.robot.soft_reset() time.sleep(2.0)
