我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用rospy.logerr()。
def ik_solve(limb, pos, orient): ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') poses = { str(limb): PoseStamped(header=hdr, pose=Pose(position=pos, orientation=orient))} ikreq.pose_stamp.append(poses[limb]) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) return 1 if resp.isValid[0]: limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position)) return limb_joints else: return Errors.RaiseNotReachable(pos)
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 run(self): self.topic_type = wait_topic_ready(self.topic_name, self.url) #print str(self.topic_type)+" self.topic_type" if not self.topic_type: rospy.logerr('Type of topic %s are not equal in the remote and local sides', self.topic_name) return topic_type_module, topic_type_name = tuple(self.topic_type.split('/')) try: roslib.load_manifest(topic_type_module) msg_module = import_module(topic_type_module + '.msg') self.rostype = getattr(msg_module, topic_type_name) if self.test: self.publisher = rospy.Publisher(self.topic_name + '_rb', self.rostype, queue_size = self.queue_size) else: self.publisher = rospy.Publisher(self.topic_name, self.rostype, queue_size = self.queue_size) self.ws = websocket.WebSocketApp(self.url, on_message = self.on_message, on_error = self.on_error, on_close = self.on_close, on_open = self.on_open) rospy.loginfo('Create connection to Rosbridge server %s for subscribed topic %s successfully', self.url, self.topic_name) self.ws.run_forever() except ResourceNotFound, e: rospy.logerr('Proxy for subscribed topic %s init falied. Reason: Could not find the required resource: %s', self.topic_name, str(e)) except Exception, e: rospy.logerr('Proxy for subscribed topic %s init falied. Reason: %s', self.topic_name, str(e))
def on_message(self, ws, message): data = json.loads(message) ts = get_message_timestamp(data['msg']) if not comp_and_replace_value(data['topic'], self.topic_type, ts): return rosmsg = self.rostype() if not data or data['op'] != 'publish' or data['topic'] != self.topic_name: rospy.logerr('Failed to handle message on subscribed topic %s [%s]', self.topic_name, data) return data.pop('_format', None) try: msgconv.populate_instance(data['msg'], rosmsg) self.publisher.publish(rosmsg) except Exception, e: rospy.logerr('Failed to publish message on topic %s. Reason: %s', self.topic_name, str(e))
def run(self): self.service_type, self.service_args = wait_service_ready(self.service_name, self.url) if not self.service_type: rospy.logerr('Type of service %s are not equal in the remote and local sides', self.service_type) return service_type_module, service_type_name = tuple(self.service_type.split('/')) try: roslib.load_manifest(service_type_module) msg_module = import_module(service_type_module + '.srv') self.srvtype = getattr(msg_module, service_type_name) if self.test: self.caller = rospy.Service(self.service_name + '_rb', self.srvtype, self.callback)#, self.queue_size) else: self.caller = rospy.Service(self.service_name, self.srvtype, self.callback)#, self.queue_size) self.ws = websocket.WebSocketApp(self.url, on_message = self.on_message, on_error = self.on_error, on_close = self.on_close, on_open = self.on_open) rospy.loginfo('Create connection to Rosbridge server %s for calling service %s successfully', self.url, self.service_name) self.ws.run_forever() except ResourceNotFound, e: rospy.logerr('Proxy for service %s init falied. Reason: Could not find the required resource: %s', self.service_name, str(e)) except Exception, e: rospy.logerr('Proxy for service %s init falied. Reason: %s', self.service_name, str(e))
def on_message(self, ws, message): data = json.loads(message) if not data or data['op'] != 'service_response' or data['service'] != self.service_name: rospy.logerr('Failed to handle message on service type %s [%s]', self.service, data) return try: rosmsg = self.srvtype()._response_class() msgconv.populate_instance(data['values'], rosmsg) # need lock to protect call_id = data.get('id').encode('ascii') with self.lock: self.event_queue[call_id]['result'] = rosmsg self.event_queue[call_id]['event'].set() except Exception, e: rospy.logerr('Failed to call service on %s. Reason: %s', self.service_name, str(e))
def run(self): self.topic_type = wait_topic_ready(self.topic_name, self.url) if not self.topic_type: rospy.logerr('Type of topic %s are not equal in the remote and local sides', self.topic_name) return topic_type_module, topic_type_name = tuple(self.topic_type.split('/')) try: roslib.load_manifest(topic_type_module) msg_module = import_module(topic_type_module + '.msg') self.rostype = getattr(msg_module, topic_type_name) self.subscriber = rospy.Subscriber(self.topic_name, self.rostype, self.callback) self.ws = websocket.WebSocketApp(self.url, on_message = self.on_message, on_error = self.on_error, on_close = self.on_close, on_open = self.on_open) rospy.loginfo('Create connection to Rosbridge server for published topic %s successfully', self.topic_name) self.ws.run_forever() except ResourceNotFound, e: rospy.logerr('Could not find the required resource %s', str(e)) except Exception, e: rospy.logerr('Proxy for published topic %s init falied. Reason: %s', self.topic_name, str(e))
def callback(self, rosmsg): if not self.advertised: return data = msgconv.extract_values(rosmsg) ts = get_message_timestamp(data) if not comp_and_replace_value(self.rb_topic_name, self.topic_type, ts): return try: self.ws.send(json.dumps({ 'op': 'publish', 'topic': self.rb_topic_name, 'msg': data, })) except Exception, e: rospy.logerr('Failed to publish message on topic %s with %s. Reason: %s', self.topic_name, data, str(e))
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 main(): """RSDK Inverse Kinematics Example A simple example of using the Rethink Inverse Kinematics Service which returns the joint angles and validity for a requested Cartesian Pose. Run this example, the example will use the default limb and call the Service with a sample Cartesian Pose, pre-defined in the example code, printing the response of whether a valid joint solution was found, and if so, the corresponding joint angles. """ rospy.init_node("rsdk_ik_service_client") if ik_service_client(): rospy.loginfo("Simple IK call passed!") else: rospy.logerr("Simple IK call FAILED") if ik_service_client(use_advanced_options=True): rospy.loginfo("Advanced IK call passed!") else: rospy.logerr("Advanced IK call FAILED")
def attach_springs(self): """ Switches to joint torque mode and attached joint springs to current joint positions. """ # record initial joint angles self._start_angles = self._limb.joint_angles() # set control rate control_rate = rospy.Rate(self._rate) # for safety purposes, set the control rate command timeout. # if the specified number of command cycles are missed, the robot # will timeout and return to Position Control Mode self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds) # loop at specified rate commanding new joint torques while not rospy.is_shutdown(): if not self._rs.state().enabled: rospy.logerr("Joint torque example failed to meet " "specified control rate timeout.") break self._update_forces() control_rate.sleep()
def main(): """RSDK Forward Kinematics Example A simple example of using the Rethink Forward Kinematics Service which returns the Cartesian Pose based on the input joint angles. Run this example, the example will use the default limb and call the Service with a sample Joint Position, pre-defined in the example code, printing the response of whether a valid Cartesian solution was found, and if so, the corresponding Cartesian pose. """ rospy.init_node("rsdk_fk_service_client") if fk_service_client(): rospy.loginfo("Simple FK call passed!") else: rospy.logerr("Simple FK call FAILED")
def __init__(self, limb="right"): """ @param limb: Limb to run CalibrateArm on arm side. """ self._limb=limb self._client = actionlib.SimpleActionClient('/calibration_command', CalibrationCommandAction) # Waits until the action server has started up and started # listening for goals. server_up = self._client.wait_for_server(rospy.Duration(10.0)) if not server_up: rospy.logerr("Timed out waiting for Calibration" " Server to connect. Check your ROS networking" " and/or reboot the robot.") rospy.signal_shutdown("Timed out waiting for Action Server") sys.exit(1)
def get_robot_assemblies(self): """ Return the names of the robot's assemblies from ROS parameter. @rtype: list [str] @return: ordered list of assembly names (e.g. right, left, torso, head). on networked robot """ assemblies = list() try: assemblies = rospy.get_param("/robot_config/assembly_names") except KeyError: rospy.logerr("RobotParam:get_robot_assemblies cannot detect assembly names" " under param /robot_config/assembly_names") except (socket.error, socket.gaierror): self._log_networking_error() return assemblies
def get_joint_names(self, limb_name): """ Return the names of the joints for the specified limb from ROS parameter. @type limb_name: str @param limb_name: name of the limb for which to retrieve joint names @rtype: list [str] @return: ordered list of joint names from proximal to distal (i.e. shoulder to wrist). joint names for limb """ joint_names = list() try: joint_names = rospy.get_param( "robot_config/{0}_config/joint_names".format(limb_name)) except KeyError: rospy.logerr(("RobotParam:get_joint_names cannot detect joint_names for" " arm \"{0}\"").format(limb_name)) except (socket.error, socket.gaierror): self._log_networking_error() return joint_names
def get_robot_name(self): """ Return the name of class of robot from ROS parameter. @rtype: str @return: name of the class of robot (eg. "sawyer", "baxter", etc.) """ robot_name = None try: robot_name = rospy.get_param("/manifest/robot_class") except KeyError: rospy.logerr("RobotParam:get_robot_name cannot detect robot name" " under param /manifest/robot_class") except (socket.error, socket.gaierror): self._log_networking_error() return robot_name
def move_to_neutral(self, timeout=15.0, speed=0.3): """ Command the Limb joints to a predefined set of "neutral" joint angles. From rosparam named_poses/<limb>/poses/neutral. @type timeout: float @param timeout: seconds to wait for move to finish [15] @type speed: float @param speed: ratio of maximum joint speed for execution default= 0.3; range= [0.0-1.0] """ try: neutral_pose = rospy.get_param("named_poses/{0}/poses/neutral".format(self.name)) except KeyError: rospy.logerr(("Get neutral pose failed, arm: \"{0}\".").format(self.name)) return angles = dict(zip(self.joint_names(), neutral_pose)) self.set_joint_position_speed(speed) return self.move_to_joint_positions(angles, timeout)
def __init__(self, limb="right"): """ Constructor. @type limb: str @param limb: limb side to interface """ params = RobotParams() limb_names = params.get_limb_names() if limb not in limb_names: rospy.logerr("Cannot detect Cuff's limb {0} on this robot." " Valid limbs are {1}. Exiting Cuff.init().".format( limb, limb_names)) return self.limb = limb self.device = None self.name = "cuff" self.cuff_config_sub = rospy.Subscriber('/io/robot/cuff/config', IODeviceConfiguration, self._config_callback) # Wait for the cuff status to be true intera_dataflow.wait_for( lambda: not self.device is None, timeout=5.0, timeout_msg=("Failed find cuff on limb '{0}'.".format(limb)) ) self._cuff_io = IODeviceInterface("robot", self.name)
def _setup_image(self, image_path): """ Load the image located at the specified path @type image_path: str @param image_path: the relative or absolute file path to the image file @rtype: sensor_msgs/Image or None @param: Returns sensor_msgs/Image if image convertable and None otherwise """ if not os.access(image_path, os.R_OK): rospy.logerr("Cannot read file at '{0}'".format(image_path)) return None img = cv2.imread(image_path) # Return msg return cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
def __init__(self): """ Constructor. """ camera_param_dict = RobotParams().get_camera_details() camera_list = camera_param_dict.keys() # check to make sure cameras is not an empty list if not camera_list: rospy.logerr(' '.join(["camera list is empty: ", ' , '.join(camera_list)])) return camera_color_dict = {"mono":['cognex'], "color":['ienso_ethernet']} self.cameras_io = dict() for camera in camera_list: if camera_param_dict[camera]['cameraType'] in camera_color_dict['' 'color']: is_color = True else: is_color = False self.cameras_io[camera] = {'interface': IODeviceInterface("internal" "_camera", camera), 'is_color': is_color}
def stop_streaming(self, camera_name): """ Stop camera streaming by given the camera name. @type camera_name: str @param camera_name: camera name @rtype: bool @return: False if camera not exists in camera name list or not able to stop streaming camera. True if the camera not is streaming mode or the camera successfully stop streaming. """ if not self.verify_camera_exists(camera_name): return False elif self._camera_streaming_status(camera_name): self.cameras_io[camera_name]['interface'].set_signal_value( "camera_streaming", False) if self._camera_streaming_status(camera_name): rospy.logerr(' '.join(["Failed to stop", camera_name, " from streaming on this robot."])) return False else: return True else: # Camera not in streaming mode return True
def set_signal_value(self, signal_name, signal_value, signal_type=None, timeout=5.0): """ set the value for the given signal return True if the signal value is set, False if the requested signal is invalid """ if signal_name not in self.list_signal_names(): rospy.logerr("Cannot find signal '{0}' in this IO Device.".format(signal_name)) return if signal_type == None: s_type = self.get_signal_type(signal_name) if s_type == None: rospy.logerr("Failed to get 'type' for signal '{0}'.".format(signal_name)) return else: s_type = signal_type set_command = SetCommand().set_signal(signal_name, s_type, signal_value) self.publish_command(set_command.op, set_command.args, timeout=timeout) # make sure both state and config are valid: self.revalidate(timeout, invalidate_state=False, invalidate_config=False)
def set_port_value(self, port_name, port_value, port_type=None, timeout=5.0): """ set the value for the given port return True if the port value is set, False if the requested port is invalid """ if port_name not in list_port_names(): rospy.logerr("Cannot find port '{0}' in this IO Device.".format(port_name)) return if port_type == None: p_type = self.get_port_type(port_name) if p_type == None: rospy.logerr("Failed to get 'type' for port '{0}'.".format(port_name)) return else: p_type = port_type set_command = SetCommand().set_port(port_name, p_type, port_value) self.publish_command(set_command.op, set_command.args, timeout=timeout) # make sure both state and config are valid: self.revalidate(timeout, invalidate_state=False, invalidate_config=False)
def clean_cache(self): """ This method completely cleans the cache directory of this ``SpeechActionServer`` instance. Use with care. """ rospy.loginfo('Purging speech cache...') for f in os.listdir(self._cache_dir): if f == '.gitkeep': continue try: p = os.path.join(self._cache_dir, f) if os.path.isfile(p): os.unlink(p) except Exception as e: rospy.logerr(e) rospy.loginfo('Speech cache has been purged.') self.cleaned_pub.publish()
def get_proxy(self, name, warn=True): """ Returns a proxy to a specific module. If it has not been created yet, it is created :param name: the name of the module to create a proxy for :return: a proxy to the corresponding module """ if name in self.__proxies and self.__proxies[name] is not None: return self.__proxies[name] proxy = None try: proxy = ALProxy(name,self.pip,self.pport) except RuntimeError,e: if warn: rospy.logerr("Could not create Proxy to \"%s\". \nException message:\n%s",name, e) self.__proxies[name] = proxy return proxy
def connectNaoQi(self): rospy.loginfo("Exploration Node Connecting to NaoQi at %s:%d", self.pip, self.pport) self.navigation = self.get_proxy("ALNavigation") self.motion = self.get_proxy("ALMotion") if self.navigation is None or self.motion is None: rospy.logerr("Unable to reach ALMotion and ALNavigation.") exit(0) version_array = self.get_proxy("ALSystem").systemVersion().split('.') if len(version_array) < 3: rospy.logerr("Unable to deduce the system version.") exit(0) version_tuple = (int(version_array[0]), int(version_array[1]), int(version_array[2])) min_version = (2, 5, 2) if version_tuple < min_version: rospy.logerr("Naoqi version " + str(min_version) + " required for localization. Yours is " + str(version_tuple)) exit(0)
def connectNaoQi(self): rospy.loginfo("Node Loading map at %s:%d", self.pip, self.pport) self.navigation = self.get_proxy("ALNavigation") if self.navigation is None: rospy.logerr("Unable to reach ALNavigation.") exit(0) version_array = self.get_proxy("ALSystem").systemVersion().split('.') if len(version_array) < 3: rospy.logerr("Unable to deduce the system version.") exit(0) version_tuple = (int(version_array[0]), int(version_array[1]), int(version_array[2])) min_version = (2, 5, 2) if version_tuple < min_version: rospy.logerr("Naoqi version " + str(min_version) + " required for localization. Yours is " + str(version_tuple)) exit(0)
def publish(topic, instructions=None): if type(topic) not in [str, nb_channels.Messages]: rospy.logerr("** Topic must be of type string or Messages in order to publish to cable **") return False if instructions and type(instructions) is not dict: rospy.logerr("** Instructions must be of type dictionary in order to publish to cable **") return False # Properly format topic string topic_val = topic if type(topic) is str else topic.value try: rospy.wait_for_service('ui_send', 0.1) client = UIClient() return client.send( topic_val, json.dumps(instructions) ) except rospy.ROSException: return UIMessageResponse(success=False)
def set_arm_configuration(self, sidename, joints, movetime = None): if sidename == 'left': side = ArmTrajectoryRosMessage.LEFT elif sidename == 'right': side = ArmTrajectoryRosMessage.RIGHT else: rospy.logerr("Unknown side {}".format(sidename)) return if movetime is None: movetime = self.ARM_MOVE_TIME for i in range(len(joints)): if math.isnan(joints[i]): joints[i] = self.last_arms[side][i] msg = self.make_arm_trajectory(side, ArmTrajectoryRosMessage.OVERRIDE, movetime, joints) self.last_arms[side] = deepcopy(joints) log('Setting {} arm to {}'.format(sidename, joints)) self.arm_trajectory_publisher.publish(msg) rospy.sleep(movetime)
def __init__(self, name): rospy.init_node(name) self.start_rotation = None self.WIDTH = 320 self.HEIGHT = 240 self.set_x_range(-30.0, 30.0) self.set_y_range(-30.0, 30.0) self.set_z_range(-30.0, 30.0) self.completed_image = None if not rospy.has_param('/ihmc_ros/robot_name'): rospy.logerr("Cannot run team_zarj_main.py, missing parameters!") rospy.logerr("Missing parameter '/ihmc_ros/robot_name'") else: self.zarj = ZarjOS()
def markerCB(self, msg): try: self.marker_lock.acquire() if not self.initialize_poses: return self.initial_poses = {} for marker in msg.markers: if marker.name.startswith("EE:goal_"): # resolve tf if marker.header.frame_id != self.frame_id: ps = PoseStamped(header=marker.header, pose=marker.pose) try: transformed_pose = self.tf_listener.transformPose(self.frame_id, ps) self.initial_poses[marker.name[3:]] = transformed_pose.pose except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, e): rospy.logerr("tf error when resolving tf: %s" % e) else: self.initial_poses[marker.name[3:]] = marker.pose #tf should be resolved finally: self.marker_lock.release()
def init_traj(self): try: # self.recorder.init_traj(itr) if self.use_goalimage: goal_img_main, goal_state = self.load_goalimage() goal_img_aux1 = np.zeros([64, 64, 3]) else: goal_img_main = np.zeros([64, 64, 3]) goal_img_aux1 = np.zeros([64, 64, 3]) goal_img_main = self.bridge.cv2_to_imgmsg(goal_img_main) goal_img_aux1 = self.bridge.cv2_to_imgmsg(goal_img_aux1) rospy.wait_for_service('init_traj_visualmpc', timeout=1) self.init_traj_visual_func(0, 0, goal_img_main, goal_img_aux1, self.save_subdir) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) raise ValueError('get_kinectdata service failed')
def get_interpolated_joint_angles(self): int_des_pos = self.calc_interpolation(self.previous_des_pos, self.des_pos, self.t_prev, self.t_next) # print 'interpolated: ', int_des_pos desired_pose = inverse_kinematics.get_pose_stamped(int_des_pos[0], int_des_pos[1], int_des_pos[2], inverse_kinematics.EXAMPLE_O) start_joints = self.ctrl.limb.joint_angles() try: des_joint_angles = inverse_kinematics.get_joint_angles(desired_pose, seed_cmd=start_joints, use_advanced_options=True) except ValueError: rospy.logerr('no inverse kinematics solution found, ' 'going to reset robot...') current_joints = self.ctrl.limb.joint_angles() self.ctrl.limb.set_joint_positions(current_joints) raise Traj_aborted_except('raising Traj_aborted_except') return des_joint_angles
def attach_springs(self): """ Switches to joint torque mode and attached joint springs to current joint positions. """ # record initial joint angles self._des_angles = self._limb.joint_angles() # set control rate control_rate = rospy.Rate(self._rate) # for safety purposes, set the control rate command timeout. # if the specified number of command cycles are missed, the robot # will timeout and disable self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds) # loop at specified rate commanding new joint torques while not rospy.is_shutdown(): if not self._rs.state().enabled: rospy.logerr("Joint torque example failed to meet " "specified control rate timeout.") break self._update_forces() control_rate.sleep()
def save(self, i_save, action, endeffector_pose): self.t_savereq = rospy.get_time() assert self.instance_type == 'main' if self.use_aux: # request save at auxiliary recorders try: rospy.wait_for_service('get_kinectdata', 0.1) resp1 = self.save_kinectdata_func(i_save) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) raise ValueError('get_kinectdata service failed') if self.save_images: self._save_img_local(i_save) if self.save_actions: self._save_state_actions(i_save, action, endeffector_pose) if self.save_gif: highres = cv2.cvtColor(self.ltob.img_cv2, cv2.COLOR_BGR2RGB) print 'highres dim',highres.shape self.highres_imglist.append(highres)
def ik_quaternion(self, quaternion_pose): node = ("ExternalTools/{}/PositionKinematicsNode/" "IKService".format(self.limb_name)) ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_request = SolvePositionIKRequest() ik_request.pose_stamp.append(quaternion_pose) try: rospy.loginfo('ik: waiting for IK service...') rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_request) except (rospy.ServiceException, rospy.ROSException) as err: rospy.logerr("ik_move: service request failed: %r" % err) else: if ik_response.isValid[0]: rospy.loginfo("ik_move: valid joint configuration found") limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position)) return limb_joints else: rospy.logerr('ik_move: no valid configuration found') return None
def attach_springs(self): self._start_angles = self._get_cmd_positions() control_rate = rospy.Rate(self._rate) # for safety purposes, set the control rate command timeout. # if the specified number of command cycles are missed, the robot # will timeout and disable self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds) error = [self._limb.joint_angles()[key] - self._start_angles[key] for key in self._limb.joint_angles().keys()] self.sum_sqr_error = sum([error[i]**2 for i in range(len(error))]) print ('-------new set of joint position---------') print (self.sum_sqr_error) tolerance = 0.020 # loop at specified rate commanding new joint torques while self.sum_sqr_error>tolerance and not rospy.is_shutdown(): if not self._rs.state().enabled: rospy.logerr("Joint torque example failed to meet " "specified control rate timeout.") break self._update_forces() control_rate.sleep()
def __init__(self): token = rospy.get_param('/telegram/token', None) if token is None: rospy.logerr("No token found in /telegram/token param server.") exit(0) else: rospy.loginfo("Got telegram bot token from param server.") # Set CvBridge self.bridge = CvBridge() # Create the EventHandler and pass it your bot's token. updater = Updater(token) # Get the dispatcher to register handlers dp = updater.dispatcher # on noncommand i.e message - echo the message on Telegram dp.add_handler(MessageHandler(Filters.text, self.pub_received)) # log all errors dp.add_error_handler(self.error) # Start the Bot updater.start_polling()
def __init__(self): self.joy_pub = rospy.Publisher('sensors/joystick/1', Joy, queue_size=1) self.joy_pub2 = rospy.Publisher('sensors/joystick/2', Joy, queue_size=1) self.rospack = RosPack() self.rate = rospy.Rate(20) count = len(devices.gamepads) if count < 2: rospy.logerr("Sensors: Expecting 2 joysticks but found only {}, exiting".format(count)) sys.exit(-1) gamepads = sorted(devices.gamepads, key=lambda pad: int(str(pad.name)[-1])) rospy.loginfo(gamepads) self.joysticks = [JoystickThread(pad) for pad in gamepads] [joystick.start() for joystick in self.joysticks]
def _read(self, max_attempts=600): got_image = False if self._camera is not None and self._camera.isOpened(): got_image, image = self._camera.read() if not got_image: if not self._error: if max_attempts > 0: rospy.sleep(0.1) self._open() return self._read(max_attempts-1) rospy.logerr("Reached maximum camera reconnection attempts, abandoning!") self._error = True return False, None return False, None return True, image
def _init_pubsub(self): self.joint_states_pub = rospy.Publisher('joint_states', JointState) self.odom_pub = rospy.Publisher('odom', Odometry) self.sensor_state_pub = rospy.Publisher('~sensor_state', TurtlebotSensorState) self.operating_mode_srv = rospy.Service('~set_operation_mode', SetTurtlebotMode, self.set_operation_mode) self.digital_output_srv = rospy.Service('~set_digital_outputs', SetDigitalOutputs, self.set_digital_outputs) if self.drive_mode == 'twist': self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Twist, self.cmd_vel) self.drive_cmd = self.robot.direct_drive elif self.drive_mode == 'drive': self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Drive, self.cmd_vel) self.drive_cmd = self.robot.drive elif self.drive_mode == 'turtle': self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Turtle, self.cmd_vel) self.drive_cmd = self.robot.direct_drive else: rospy.logerr("unknown drive mode :%s"%(self.drive_mode)) self.transform_broadcaster = None if self.publish_tf: self.transform_broadcaster = tf.TransformBroadcaster()
def set_operation_mode(self,req): if not self.robot.sci: raise Exception("Robot not connected, SCI not available") self.operate_mode = req.mode if req.mode == 1: #passive self._robot_run_passive() elif req.mode == 2: #safe self._robot_run_safe() elif req.mode == 3: #full self._robot_run_full() else: rospy.logerr("Requested an invalid mode.") return SetTurtlebotModeResponse(False) return SetTurtlebotModeResponse(True)
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 delete_object(self, req): """Handles DeleteObject service requests. Args: req: DeleteObjectRequest message. Returns: DeleteObjectResponse. """ response = interop.srv.DeleteObjectResponse() try: self.objects_dir.delete_object(req.id) except (KeyError, OSError) as e: rospy.logerr("Could not delete object: {}".format(e)) response.success = False except Exception as e: rospy.logfatal(e) response.success = False else: response.success = True return response
def delete_object_image(self, req): """Handles DeleteObjectImage service requests. Args: req: DeleteObjectImageRequest message. Returns: DeleteObjectImageResponse. """ response = interop.srv.DeleteObjectImageResponse() try: self.objects_dir.delete_object_image(req.id) except (KeyError, IOError) as e: rospy.logerr("Could not delete object image: {}".format(e)) response.success = False except Exception as e: rospy.logfatal(e) response.success = False else: response.success = True return response
def symlink_objects_path_to_latest(objects_path): """Symlinks 'latest' to the given directory. Args: objects_path: Path to objects directory. """ objects_root = os.path.dirname(objects_path) path_to_symlink = os.path.join(objects_root, "latest") try: os.symlink(objects_path, path_to_symlink) except OSError as e: # Replace the old symlink if an old symlink with the same # name exists. if e.errno == errno.EEXIST: os.remove(path_to_symlink) os.symlink(objects_path, path_to_symlink) else: rospy.logerr( "Could not create symlink to the latest objects directory")
def callback_cmd_vel(message): lfile = '/dev/rtmotor_raw_l0' rfile = '/dev/rtmotor_raw_r0' #for forwarding forward_hz = 80000.0*message.linear.x/(9*math.pi) #for rotation rot_hz = 400.0*message.angular.z/math.pi try: lf = open(lfile,'w') rf = open(rfile,'w') lf.write(str(int(round(forward_hz - rot_hz))) + '\n') rf.write(str(int(round(forward_hz + rot_hz))) + '\n') except: rospy.logerr("cannot write to rtmotor_raw_*") lf.close() rf.close()
def __init__(self, limb): ns = 'robot/limb/' + limb + '/' self._client = actionlib.SimpleActionClient( ns + "follow_joint_trajectory", FollowJointTrajectoryAction, ) self._goal = FollowJointTrajectoryGoal() self._goal_time_tolerance = rospy.Time(0.1) self._goal.goal_time_tolerance = self._goal_time_tolerance server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0)) if not server_up: rospy.logerr("Timed out waiting for Joint Trajectory" " Action Server to connect. Start the action server" " before running example.") rospy.signal_shutdown("Timed out waiting for Action Server") sys.exit(1) self.clear(limb)
