def execute(self): if self.runs_ == -1: # run forever self.child_status_ = self.children_[0].execute() if self.child_status_ == 'SUCCESS': rospy.logwarn('REPEAT DECORATOR ['+self.name_+']: SUCCEEDED, RESET') self.children_[0].reset() return self.set_status('RUNNING') elif self.child_status_ == 'RUNNING': return self.set_status('RUNNING') elif self.child_status_ == 'FAILURE': rospy.logwarn('REPEAT DECORATOR ['+self.name_+']: FAILED, RESET') self.children_[0].reset() return self.set_status('RUNNING') else: ### FIX ME ### if self.num_runs_ < self.runs_: self.child_status_ = self.children_[0].execute() if self.child_status_ == 'SUCCESS': self.num_runs_ += 1 elif self.child_status_ == 'RUNNING': return self.set_status('RUNNING') elif self.child_status_ == 'FAILURE': return self.set_status('FAILURE') else: return self.set_status('SUCCESS')
def open_alarm(self, event): '''React when an alarm has been clicked 1. Use event to determine which cell has been clicked 2. Select that cell's whole row 3. Use the cached json_descriptions of each alarm to get that row's json data When an alarm has been clicked, we'll display its json parameters in the box on the right ''' items_selected = self.alarm_table.selectedItems() if len(items_selected) == 0: return row_selected = items_selected[0].row() self.alarm_table.selectRow(row_selected) key_item = self.alarm_table.itemAt(row_selected, 0) try: json_parameters = json.loads(self.alarm_parameter_cache[key_item]) except ValueError: rospy.logwarn("Could not decode alarm message") return self.alarm_tree.clear() try: build_tree_from_json(self.alarm_tree, json_parameters) except AssertionError: rospy.logwarn("Could not draw json tree for alarm (Is it a dictionary?)")
def attach(self, object_name, actuate = True): if actuate: # attach the collision object to the gripper self.gripper_close.call() # get the actual collision obj from planning scene remove_object = None rospy.logwarn('Attaching object: %s'% object_name) res = self.get_planning_scene(components= PlanningSceneComponents(components=PlanningSceneComponents.WORLD_OBJECT_GEOMETRY)) all_objects = res.scene.world.collision_objects for col_obj in all_objects: if col_obj.id == object_name: remove_object = col_obj break # self.planning_group.attachObject(object_name, self.end_link) if remove_object is not None: self.planning_scene_publisher = rospy.Publisher('planning_scene', PlanningScene, queue_size = 10) planning_scene_diff = PlanningScene(is_diff=True) remove_object.operation = CollisionObject.REMOVE del planning_scene_diff.world.collision_objects[:]; planning_scene_diff.world.collision_objects.append(copy.deepcopy(remove_object)); attached_object = AttachedCollisionObject() attached_object.object = remove_object attached_object.link_name = self.end_link attached_object.object.header.frame_id = self.joint_names[-1] attached_object.object.operation = CollisionObject.ADD del planning_scene_diff.robot_state.attached_collision_objects[:]; planning_scene_diff.robot_state.attached_collision_objects.append(attached_object); self.planning_scene_publisher.publish(planning_scene_diff)
def imu_remap(imu_msg): global namespace # global noisy # remap imu message to base_link_self and publish the new message remapped_msg = imu_msg remapped_msg.header.frame_id = namespace + 'base_link_filter' # if noisy: # # We will add noise to the IMU data # noise = randn(7, 1) # # orientation = remapped_msg.orientation # noisy_yaw = convert_quaternion_to_yaw(orientation) + noise[0] # orientation = convert_yaw_to_quaternion(noisy_yaw) # # angular_velocity = remapped_msg.angular_velocity # angular_velocity.x += noise[1] # angular_velocity.y += noise[2] # angular_velocity.z += noise[3] # # linear_acceleration = remapped_msg.linear_acceleration # linear_acceleration.x += noise[4] # linear_acceleration.y += noise[5] # linear_acceleration.z += noise[6] try: imu_publisher.publish(remapped_msg) except rospy.ROSException as e: rospy.logwarn(e.message)
def update_subs(self, topics): for topic in topics: # Different elements publish different topic types topic_type = String cb = self.string_cb name,kind = topic if kind == GUIElement.BUTTON_ID: topic_type = Int32 cb = self.int_cb elif kind == GUIElement.BUTTONGROUP_ID: topic_type = String cb = self.string_cb elif kind == GUIElement.STOPWATCH_ID: topic_type = Duration cb = self.dur_cb elif kind == GUIElement.INTSLIDER_ID: topic_type = Int32 cb = self.int_cb else: rospy.logwarn("Unsupported element type.") continue if name.strip('/') not in self.subs: print "Subscribing to topic: " + name rospy.Subscriber(name, topic_type, cb) # reset info on the topic, even if already subscribed self.subs[name.strip('/')] = {"type_id":kind, "pressed":False, "last_press": ""}
def smart_move_cb(self,req): ''' Find any valid object that meets the requirements. Find a cartesian path or possibly longer path through joint space. ''' if not self.driver_status == 'SERVO': msg = 'FAILURE -- not in servo mode' rospy.logerr('DRIVER -- Not in servo mode!') return msg # Check acceleration and velocity limits (acceleration, velocity) = self.check_req_speed_params(req) possible_goals = self.query(req) stamp = self.acquire() if len(possible_goals) == 0: return 'FAILURE -- no valid poses found. see costar_arm.py' for (dist,T,obj,name) in possible_goals: rospy.logwarn("Trying to move to frame at distance %f"%(dist)) # plan to T if not self.valid_verify(stamp): msg = 'FAILURE - Stopping smart move action because robot has been preempted by another process. see costar_arm.py' rospy.logwarn(msg) return msg (code,res) = self.planner.getPlan(T,self.q0,obj=obj) msg = self.send_and_publish_planning_result(res,stamp,acceleration,velocity) if msg[0:7] == 'SUCCESS': break return msg
def servo_to_joints_cb(self,req): if self.driver_status == 'SERVO': # Check acceleration and velocity limits (acceleration, velocity) = self.check_req_speed_params(req) stamp = self.acquire() # inverse kinematics traj = self.planner.getJointMove(req.target.position, self.q0, self.base_steps, self.steps_per_meter, self.steps_per_radians, time_multiplier = (1./velocity), percent_acc = acceleration, use_joint_move = True, table_frame = self.table_pose) # Send command if len(traj.points) > 0: if stamp is not None: rospy.logwarn("Robot moving to " + str(traj.points[-1].positions)) res = self.send_trajectory(traj,stamp,acceleration,velocity,cartesian=False) self.release() else: res = 'FAILURE -- could not preempt current arm control.' return res else: rospy.logerr('SIMPLE DRIVER -- IK failed') return 'FAILURE -- no trajectory points' else: rospy.logerr('SIMPLE DRIVER -- Not in servo mode') return 'FAILURE -- not in servo mode'
def executeBodyPose(self, goal): if not goal.pose_name in self.poseLibrary: rospy.loginfo("Pose %s not in library, reload library from parameters..." % goal.pose_name) self.readInPoses() if goal.pose_name in self.poseLibrary: rospy.loginfo("Executing body pose %s...", goal.pose_name); if not self.stopWalkSrv is None: self.stopWalkSrv() trajGoal = JointTrajectoryGoal() # time out one sec. after trajectory ended: trajGoal.trajectory = self.poseLibrary[goal.pose_name] timeout = trajGoal.trajectory.points[-1].time_from_start + Duration(1.0) trajGoal.trajectory.header.stamp = rospy.Time.now() # TODO: cancel goal after timeout is not working yet in nao_controller self.trajectoryClient.send_goal_and_wait(trajGoal, timeout) if self.trajectoryClient.get_state() != GoalStatus.SUCCEEDED: self.poseServer.set_aborted(text="JointTrajectory action did not succeed (timeout?)"); self.poseServer.set_succeeded() rospy.loginfo("Done."); else: msg = "pose \""+goal.pose_name+"\" not in pose_manager's library"; rospy.logwarn("%s", msg) self.poseServer.set_aborted(text=str(msg));
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 Loop(self): rospy.loginfo("Arlobot Calibration Node entering loop") while not rospy.is_shutdown(): self._loop_rate.sleep() else: rospy.logwarn("Arlobot Base Node: shutdown invoked, exiting")
def checkVer(self, input): data = [] gotHeaderStart = False verInfo = VersionResponds() input.setTimeout(1) countUntilTimeout = 0 while not rospy.is_shutdown() and countUntilTimeout < 3: try: if gotHeaderStart: for i in range(1, 11): data.append(input.read()) verInfo.buildRequest(data) if verInfo.checkPackage() and abs(verInfo.getVersion() - VERSION) < 1: if verInfo.getVersion() < VERSION: rospy.logwarn( "RiCBord has a firmware %.2f please update the firmware for better performers" % ( verInfo.getVersion())) elif verInfo.getVersion() > VERSION: rospy.logwarn( "RiCBord has a firmware %.2f please update your package for better performers" % ( verInfo.getVersion())) return True else: return False elif ord(input.read()) == HEADER_START: gotHeaderStart = True except TypeError: pass finally: countUntilTimeout += 1 return False
def parse(node, verbose=True): joint = Joint(node.getAttribute('name'), None, None, node.getAttribute('type')) for child in children(node): if child.localName == 'parent': joint.parent = child.getAttribute('link') elif child.localName == 'child': joint.child = child.getAttribute('link') elif child.localName == 'axis': joint.axis = child.getAttribute('xyz') elif child.localName == 'origin': joint.origin = Pose.parse(child) elif child.localName == 'limit': joint.limits = JointLimit.parse(child) elif child.localName == 'dynamics': joint.dynamics = Dynamics.parse(child) elif child.localName == 'safety_controller': joint.safety = SafetyController.parse(child) elif child.localName == 'calibration': joint.calibration = JointCalibration.parse(child) elif child.localName == 'mimic': joint.mimic = JointMimic.parse(child) else: if verbose: rospy.logwarn("Unknown joint element '%s'"%child.localName) return joint
def execute(self, userdata): rospy.loginfo(OKGREEN+"i'm looking what tags are"+ENDC) rospy.loginfo(OKGREEN+str(userdata.asr_userSaid)+ENDC) #rospy.loginfo(OKGREEN+"TAGS: "+str(self.tags)+ENDC) #rospy.loginfo(OKGREEN+str(userdata.asr_userSaid_tags)+ENDC) #userdata.object=userdata.asr_userSaid # it means that in this place it have a coke if userdata.asr_userSaid=="finish" : rospy.logwarn("-------------------------------------i'm have a finish order") return 'finish' listTags = userdata.asr_userSaid_tags #Process tags userdata.object_array = [] locationValue = [tag for tag in listTags if tag.key == 'direction'] objectValue = [tag for tag in listTags if tag.key == 'class'] if objectValue and locationValue: userdata.objectOrientation = locationValue[0].value userdata.objectName = objectValue[0].value return 'new_position' else: rospy.logerr("Object or Location not set") return 'aborted'
def execute(self, userdata): rospy.logwarn("i'm looking for::::::::::::::. "+str (userdata.name)) # first of all i look if it's some faces if userdata.faces.faces: # i look in what option we are, if we are looking for a name o no if userdata.name!="": userdata.face=[face for face in userdata.faces.faces if face.name==userdata.name] if userdata.face: userdata.face=userdata.face.pop() userdata.standard_error="Recognize_face_Name OK"+userdata.standard_error userdata.face_frame = userdata.faces.header.frame_id rospy.loginfo('-- FRAME ID:: ' + userdata.faces.header.frame_id) self.request_preempt() return 'succeeded' else : userdata.standard_error="Recognize:= Any face whit that name"+userdata.standard_error return 'aborted' # if we are no looking for a face we will organize else: # i want to take the best face confidence userdata.faces.faces.sort(cmp=None, key=attrgetter('confidence'), reverse=True) userdata.face=userdata.faces.faces[0] userdata.face_frame = userdata.faces.header.frame_id rospy.loginfo('-- FRAME ID:: ' + userdata.faces.header.frame_id) userdata.standard_error="Recognize_face_Normal OK"+userdata.standard_error self.request_preempt() return 'succeeded' else: userdata.standard_error="no faces available"+userdata.standard_error userdata.face=None return 'aborted'
def obtain_masters(self): ''' This method use the service 'list_masters' of the master_discoverer to get the list of discovered ROS master. Based on this list the L{SyncThread} for synchronization will be created. @see: U{master_discovery_fkie.interface_finder.get_listmaster_service() <http://docs.ros.org/api/master_discovery_fkie/html/modules.html#interface-finder-module>} ''' if not rospy.is_shutdown(): service_names = interface_finder.get_listmaster_service(masteruri_from_master(), False) for service_name in service_names: try: with self.__lock: try: socket.setdefaulttimeout(5) discoverMasters = rospy.ServiceProxy(service_name, DiscoverMasters) resp = discoverMasters() masters = [] master_names = [m.name for m in resp.masters] rospy.loginfo("ROS masters obtained from '%s': %s", service_name, master_names) for m in resp.masters: if self._can_sync(m.name): # do not sync to the master, if it is in ignore list or not in filled sync list masters.append(m.name) self.update_master(m.name, m.uri, m.timestamp, m.timestamp_local, m.discoverer_name, m.monitoruri, m.online) for key in set(self.masters.keys()) - set(masters): self.remove_master(self.masters[key].name) except rospy.ServiceException, e: rospy.logwarn("ERROR Service call 'list_masters' failed: %s", str(e)) except: import traceback rospy.logwarn("ERROR while initial list masters: %s", traceback.format_exc()) finally: socket.setdefaulttimeout(None) self.update_timer = threading.Timer(self.UPDATE_INTERVALL, self.obtain_masters) self.update_timer.start()
def fit_joints_to_range(self, qvec): self.qvecIK = qvec fits = True does_fit = False for i in range(6): #print "qvec", qvec #q = qvec[i] #treat d3 differently if (i!=2): does_fit = self.fit_q_to_range(q_lower_limits[i],q_upper_limits[i],i) #special case for d3...although generic formula also works in this case else: does_fit = True if (self.qvecIK[i] < q_lower_limits[i]): does_fit = False if (self.qvecIK[i] > q_upper_limits[i]): does_fit = False if (not does_fit): if(debug_needle_print): rospy.logwarn("IK err: jnt %d lower lim: %f upper lim: %f desired val = %f"%(i,q_lower_limits[i],q_upper_limits[i],self.qvecIK)) #qvec[i] = q fits = fits and does_fit if (fits): return True else: return False
def __init__(self, config_path, bus_layout): '''Thruster driver, an object for commanding all of the sub's thrusters - Gather configuration data and make it available to other nodes - Instantiate ThrusterPorts, (Either simulated or real), for communicating with thrusters - Track a thrust_dict, which maps thruster names to the appropriate port - Given a command message, route that command to the appropriate port/thruster - Send a thruster status message describing the status of the particular thruster ''' self.alarm_broadcaster = AlarmBroadcaster() self.thruster_out_alarm = self.alarm_broadcaster.add_alarm( name='thruster_out', action_required=True, severity=3 ) self.failed_thrusters = [] self.make_fake = rospy.get_param('simulate', False) if self.make_fake: rospy.logwarn("Running fake thrusters for simulation, based on parameter '/simulate'") # Individual thruster configuration data newtons, thruster_input = self.load_config(config_path) self.interpolate = scipy.interpolate.interp1d(newtons, thruster_input) # Bus configuration self.port_dict = self.load_bus_layout(bus_layout) thrust_service = rospy.Service('thrusters/thruster_range', ThrusterInfo, self.get_thruster_info) self.thrust_sub = rospy.Subscriber('thrusters/thrust', Thrust, self.thrust_cb, queue_size=1) self.status_pub = rospy.Publisher('thrusters/thruster_status', ThrusterStatus, queue_size=8) # This is essentially only for testing self.fail_thruster_server = rospy.Service('fail_thruster', FailThruster, self.fail_thruster)
def reset_iris_neutral_value(self): if self.dic_sequence_services['checked_sequence_of_missions'] == True: new_service = {} trigger_instant = self._widget.TriggerInstantNeutral.value() new_service['trigger_instant'] = trigger_instant self.dic_sequence_services['last_trigger_time'] = trigger_instant new_service['service_name'] = 'IrisPlusResetNeutral' new_service['inputs_service'] = {} self.dic_sequence_services['list_sequence_services'].append(new_service) else: try: # time out of one second for waiting for service rospy.wait_for_service(self.namespace+'IrisPlusResetNeutral',1.0) try: # request reseting neutral value for iris+ (implicit, with keywords) ResetingNeutral = rospy.ServiceProxy(self.namespace+'IrisPlusResetNeutral', IrisPlusResetNeutral) reply = ResetingNeutral() if reply.received == True: self._widget.ThrottleNeutralValue.setValue(reply.k_trottle_neutral) rospy.logwarn('Service for reseting neutral value succeded') except rospy.ServiceException as exc: rospy.logwarn("Service did not process request: " + str(exc)) rospy.logwarn('Service for reseting neutral value failed') pass except rospy.ServiceException as exc: rospy.logwarn("Service did not process request: " + str(exc)) rospy.logwarn('Service for reseting neutral value failed') pass
def tts_speak(text, wait_before_speaking=0): """ Lets the robot say the given text out aloud. This is a blocking call. """ from text_to_speech.msg import SoundAction, SoundGoal global __tts_client if __tts_client is None: __tts_client = actionlib.SimpleActionClient(TTS_ACTION_NAME, SoundAction) rospy.logdebug('Waiting for "%s"...' % TTS_ACTION_NAME) if not __tts_client.wait_for_server(rospy.Duration(1.0)): rospy.logwarn('Couldn\'t connect to "%s" server.' % TTS_ACTION_NAME) return False tts_goal = SoundGoal() tts_goal.wait_before_speaking = rospy.Duration(wait_before_speaking) tts_goal.text = text rospy.logdebug('Sent speak command with "%s" (wait: %.3f seconds).' % ( text, wait_before_speaking)) __tts_client.send_goal(tts_goal) __tts_client.wait_for_result() result = __tts_client.get_state() rospy.logdebug('Result for last speech command: %s' % result) return result == GoalStatus.SUCCEEDED
def __init__(self): # Start node rospy.init_node("recognizer_en") self._device_name_param = "~mic_name" # Find the name of your microphone by typing pacmd list-sources in the terminal self._lm_param = "~lm" self._dic_param = "~dict" # Configure mics with gstreamer launch config if rospy.has_param(self._device_name_param): self.device_name = rospy.get_param(self._device_name_param) self.device_index = self.pulse_index_from_name(self.device_name) self.launch_config = "pulsesrc device=" + str(self.device_index) rospy.loginfo("Using: pulsesrc device=%s name=%s", self.device_index, self.device_name) else: self.launch_config = 'gconfaudiosrc' rospy.loginfo("Launch config: %s", self.launch_config) self.launch_config += " ! audioconvert ! audioresample " \ + '! vader name=vad auto-threshold=true ' \ + '! pocketsphinx name=asr ! fakesink' # + '! pocketsphinx hmm=en_US/hub4wsj_sc_8k name=asr ! fakesink' # Configure ROS settings self.started = False rospy.on_shutdown(self.shutdown) self.pub = rospy.Publisher('~output', String) rospy.Service("~start", Empty, self.start) rospy.Service("~stop", Empty, self.stop) if rospy.has_param(self._lm_param) and rospy.has_param(self._dic_param): self.start_recognizer() else: rospy.logwarn("lm dic and hmm parameters need to be set to start recognizer.")
def send_list_of_services(self): # # debug # for service in self.dic_sequence_services['list_sequence_services']: # print(service) # print('\n\n') # request service try: # time out of one second for waiting for service rospy.wait_for_service(self.namespace+'ServiceSequencer',1.0) try: request = rospy.ServiceProxy(self.namespace+'ServiceSequencer', ServiceSequence) service_sequence = json.dumps(self.dic_sequence_services['list_sequence_services']) reply = request(service_sequence = service_sequence) if reply.received == True: # if controller receives message print('Success') except rospy.ServiceException as exc: rospy.logwarn("Service did not process request: " + str(exc)) except rospy.ServiceException as exc: rospy.logwarn("Service did not process request: " + str(exc))
def __init__(self): rospy.init_node('twist_converter', anonymous=True) self.num_robots=int(rospy.get_param('~num_robots',1)) self.publishers = [None]*self.num_robots; self.subscribers = [None]*self.num_robots; if rospy.has_param('~robot_prefix'): #if there is a robot prefix assume that there is actually one or more #full_param_name = rospy.search_param('robot_prefix') #robot_prefix = rospy.get_param(full_param_name) robot_prefix=rospy.get_param('~robot_prefix') for r in range(self.num_robots): self.publishers[r]=rospy.Publisher(robot_prefix+str(r)+'/cmd_vel',Twist); self.subscribers[r] = rospy.Subscriber(robot_prefix+str(r)+'/image', Image, self.callback, r) else: # if no robot prefix, assume that there is only one robot self.publishers[0] = rospy.Publisher('cmd_vel',Twist);rospy.logwarn("assuming /cmd_vel, number of robots actually " +str(self.num_robots)) self.subscribers[0] = rospy.Subscriber("image", Image, self.callback, 0) self.data_uri = rospy.get_param("data_uri","/twist"); self.urls = (self.data_uri,'twist', "/stop","stop","/controller","controller") self.data = ['-10']*self.num_robots; self.datasize = [0]*self.num_robots; self.port=int(rospy.get_param("~port","8080")); #self.data_uri2 = rospy.get_param("data_uri","/pose"); rospy.logwarn("running")
def UnArming_Quad(self,base_name=""): #This function is used to arm the quad.""" #Un-Arming the Quad srv_path = self.namespace+'mavros/cmd/arming' # if base_name!="": # srv_path = "/%s/%s"%(base_name,srv_path) try: rospy.logwarn('Un-Arming Quad ...') rospy.wait_for_service(srv_path,2.0) try: arming = rospy.ServiceProxy(srv_path,CommandBool) arming_result=arming(False) if arming_result.success: rospy.logwarn('Quad is Un-Armed!!!!') # return True else: rospy.logwarn('Cannot Un-arm quad') # return False except: rospy.logwarn('Cannot Un-arm quad') # return False except: rospy.logwarn('No connection to Mavros')
def setAngle(self, turn): "set wheel angle" # Try various functions with domain [-1..1] to improve # sensitivity while retaining sign. At higher speeds, it may # make sense to limit the range, avoiding too-sharp turns and # providing better control sensitivity. #turn = math.pow(turn, 3) * ArtVehicle.max_steer_radians #enqueues first 5 data points based on the first movements on the wheel if(self.counter < 5): self.counter +=1 self.steeringQueue.put(self.steering.angle/180) else: #dequeue oldest value oldVal = self.steeringQueue.get() #get rate of change of angle by subtracting final(turn) - initial(oldVal) and dividing by .25s which is time it takes to get 5 values currentRate = (self.steering.angle/180 - oldVal)/.25 limit = 2.0361 * math.pow(.72898, self.pilot.pstate.current.speed) rospy.logwarn(str(currentRate) + ' ' + str(limit)) #if currentRate is above limit, restrict currentRate to the limit at the target velocity, else if it's at or below limit don't do anything #get new value of turn by using equation below: #(final angle - initial angle)/change time = limit; angle final = limit * change in time + angle inital if(math.fabs(currentRate > limit)): rospy.logwarn('Success') turn = limit*.25 + oldVal #put new steering angle into queue self.steeringQueue.put(turn) # ensure maximum wheel angle never exceeded self.pilot.steer(turn)
def execute(self): if self.display_name is not None: running_service = '%s [%s]'%(self.service_description,self.display_name) else: running_service = '%s [%s]'%(self.service_description,self.name_) if self.needs_reset: rospy.loginfo('%s already [%s], needs reset'%(running_service,self.get_status())) return self.get_status() else: rospy.logwarn("running? " + str(self.running) + "; " + str(self.finished_with_success)) if not self.running: # Thread is not running if self.finished_with_success is None: # Service was never called try: self.service_thread.start() rospy.loginfo('%s running'%running_service) self.running = True return self.set_status('RUNNING') except Exception, errtxt: self.status_msg = '%s thread failed'%running_service rospy.logwarn(self.status_msg) self.running = False self.needs_reset = True rospy.logerr('failed -- %s'%self.status_msg) return self.set_status('FAILURE -- %s'%self.status_msg) else:# If thread is running
def ref_generation(): xbox_ctrl = rospy.Publisher('xboxctrl', numpy_msg(Floats)) rospy.init_node('xboxbridge', anonymous=True) rate = rospy.Rate(100) # Run no faster than 100hz UDP_IP = "192.168.10.81" UDP_PORT = 27300 sock = socket.socket(socket.AF_INET, # Internet socket.SOCK_DGRAM) # UDP sock.bind((UDP_IP, UDP_PORT)) sock.setblocking(0) packer = struct.Struct('c c c c f f f f B B h') while not rospy.is_shutdown(): try: data = sock.recv(48) # buffer size is 48 bytes if len(data) == 24: if data[0] == "X" and data[1] == "B" and data[2] == "O" and data[3] == "X": output = packer.unpack(data) ctrl = numpy.array([output[4],output[5],output[6],output[7],output[8]], dtype=numpy.float32) xbox_ctrl.publish(ctrl) #print output except: # print "Nothing" if str(sys.exc_info()[1]) != '[Errno 11] Resource temporarily unavailable': rospy.logwarn("Unexpected error: %s", sys.exc_info()[1]) nothing = 0 rate.sleep()
def _getSSH(self, host, user, pw=None, do_connect=True): ''' @return: the paramiko ssh client @rtype: L{paramiko.SSHClient} @raise BadHostKeyException: - if the server's host key could not be verified @raise AuthenticationException: - if authentication failed @raise SSHException: - if there was any other error connecting or establishing an SSH session @raise socket.error: - if a socket error occurred while connecting ''' session = SSHhandler.SSH_SESSIONS.get(host, paramiko.SSHClient()) if session is None: t = SSHhandler.SSH_SESSIONS.pop(host) del t session = SSHhandler.SSH_SESSIONS.get(host, paramiko.SSHClient()) if session._transport is None: session.set_missing_host_key_policy(paramiko.AutoAddPolicy()) while (session.get_transport() is None or not session.get_transport().authenticated) and do_connect: try: session.connect(host, username=user, password=pw, timeout=3) except Exception, e: # import traceback # print traceback.format_exc() if str(e) in ['Authentication failed.', 'No authentication methods available']: res, user, pw = self._requestPW(user, host) if not res: return None self.SSH_AUTH[host] = user else: rospy.logwarn("ssh connection to %s failed: %s", host, str(e)) return None else: SSHhandler.SSH_SESSIONS[host] = session if not session.get_transport() is None: session.get_transport().set_keepalive(10)
def _update(self, event): if self.robot and self.traj: now = time.time() if (now - self.traj_t0) <= self.traj.points[-1].time_from_start.to_sec(): setpoint = sample_traj(self.traj, now - self.traj_t0) try: self.robot.send_servoj(999, setpoint.positions, 4 * self.RATE) except socket.error: pass else: # Off the end if self.goal_handle: last_point = self.traj.points[-1] state = self.robot.get_joint_states() position_in_tol = within_tolerance(state.position, last_point.positions, self.joint_goal_tolerances) velocity_in_tol = within_tolerance(state.velocity, last_point.velocities, [0.005]*6) if position_in_tol and velocity_in_tol: # The arm reached the goal (and isn't moving). Succeeding self.goal_handle.set_succeeded() self.goal_handle = None elif now - (self.traj_t0 + last_point.time_from_start.to_sec()) > self.goal_time_tolerance.to_sec(): # Took too long to reach the goal. Aborting rospy.logwarn("Took too long to reach the goal.\nDesired: %s\nactual: %s\nvelocity: %s" % \ (last_point.positions, state.position, state.velocity)) self.goal_handle.set_aborted(text="Took too long to reach the goal") self.goal_handle = None
def on_automode_message(self, msg): if msg.data == True: # if autonomous mode requested if self.state == self.STATE_IDLE: if self.wptnav.pose != False: # if we have a valid pose (num_wpt, next_wpt) = self.wptlist.status() if num_wpt > 0: self.state = self.STATE_NAVIGATE if self.wptnav.state == self.wptnav.STATE_STANDBY: self.wptnav.resume() rospy.loginfo(rospy.get_name() + ": Resuming waypoint navigation") elif self.wptnav.state == self.wptnav.STATE_STOP: self.goto_first_wpt() rospy.loginfo(rospy.get_name() + ": Switching to waypoint navigation") else: if self.no_route_plan_warn == False: rospy.logwarn(rospy.get_name() + ": No route plan available") self.no_route_plan_warn = True else: # no valid pose yet if self.automode_warn == False: self.automode_warn = True rospy.logwarn(rospy.get_name() + ": Absolute pose is required for autonomous navigation") else: # if manual mode requested if self.state != self.STATE_IDLE: self.state = self.STATE_IDLE self.wptnav.standby() rospy.loginfo(rospy.get_name() + ": Switching to manual control")
def callback(self,msg,id): #get the data from the message and store as a string try: self.data[id] = msg.data; self.datasize[id]=msg.width*msg.height*4; #assumes rgba8 except Exception, err: rospy.logwarn("Cannot convert the Image message due to %s, with robot %s" % err, id)
'the pointcloud message topic to be subscribed, default `/kitti/points_raw`', default='/kitti/points_raw') parser.add_argument( '--pub_topic', type=str, help= 'the pointcloud message topic to be published, default `/squeeze_seg/points`', default='/squeeze_seg/points') parser.add_argument( '--pub_feature_map_topic', type=str, help= 'the 2D spherical surface image message topic to be published, default `/squeeze_seg/feature_map`', default='/squeeze_seg/feature_map') parser.add_argument( '--pub_label_map_topic', type=str, help= 'the corresponding ground truth label image message topic to be published, default `/squeeze_seg/label_map`', default='/squeeze_seg/label_map') args = parser.parse_args() rospy.init_node('segment_node') node = SegmentNode(sub_topic=args.sub_topic, pub_topic=args.pub_topic, pub_feature_map_topic=args.pub_feature_map_topic, pub_label_map_topic=args.pub_label_map_topic, FLAGS=FLAGS) rospy.logwarn("finished.")
def _compute_reward(self, observations, done): reward = 0 if not done: # Cost for uncertainty in tracking and check if target is still visible # self.uncertainty_cost(observations, epsilon = -2, lower_bound = -4) # Only if target is in sight and visible # if not self.lost_target_flag: # '''Heading Reward''' # [flag,desired_heading] = self.is_in_desired_heading(observations,epsilon=0.5) # if flag: # reward+=0.16*self.detection_data # else: # reward+=0.0 '''Following reward in world workspace.''' # [flag,distance_from_target] = self.is_in_desired_distance(observations,epsilon_up=7,epsilon_down=3.5) [flag, distance_from_target] = self.is_in_desired_distance(observations, epsilon=0.5) if flag: reward += 0.25 * self.detection_data else: reward += 0.01 * self.detection_data # Following height reward in world workspace [flag, height] = self.is_in_MAV_desired_altitude(observations, epsilon=0.5) if flag: reward += 0.25 * self.detection_data else: reward += 0.01 * self.detection_data # '''Following velocity reward in world workspace''' # [flag,vel_err] = self.is_in_desired_velocity(observations, epsilon=0.1) # if flag: # reward += 0.2*self.detection.data # else: # reward+= 0.01*self.detection.data # '''Perception centering reward''' # [flag,distance_to_imgcenter] = self.is_image_centered(observations, epsilon=0.1) # if flag: # reward += 0.16*self.detection_data # else: # reward += 0.01*self.detection_data # reward+= -1*(observations[3])*10*distance_to_imgcenter**2 -1*(1-observations[3])*10 + 10 # reward+= -1*distance_to_imgcenter**2 + 1 # '''Perception height cost / Tracking reward in image space''' # [flag,height_ratio] = self.is_desired_height(observations, epsilon=0.05) # if flag: # reward += 0.16*self.detection_data*(1-self.half_visible) # else: # reward += 0.01*self.detection_data*(1-self.half_visible) # reward += 10*detection.data*(-1*(height_ratio-self.desired_height_ratio)**2 + 0.5) # if flag: # reward += observations[3]*self.desired_height_reward # else: # reward +=0 # reward += -1*(observations[3])*20*(height_ratio-self.desired_height_ratio)**2 - 1*(1-observations[3])*10 + 10 '''Minimize person velocity in image plane''' [flag, flow] = self.is_flow_zero(observations, epsilon=0.5) if flag: reward += 0.25 * self.detection.data else: reward += 0.01 * self.detection.data # '''Penalize Workspace Violation''' # if self._MAV_out_ws: # reward -= 0.5 # Perception joint cost / Joint tracking cost in image space # joint_r= self.joint_detections_reward(observations, epsilon=0.5) # reward+=self.joint_detections_reward_gain*joint_r # If there has been a decrease in the distance towards target, we reward it [flag, rad_distance_from_target ] = self.is_in_desired_rad_distance(observations, epsilon_up=7, epsilon_down=3.5) delta_error = rad_distance_from_target - self.previous_distance self.previous_distance = rad_distance_from_target if delta_error < 0.0 and rad_distance_from_target > np.sqrt( self.desired_distance**2 + self.desired_distance**2) + 0.5: reward += 0.25 * self.detection_data rospy.logwarn( "################### DECREASING ERROR: GOOD JOB ###########################" ) else: reward += 0.0 target = self.get_gt_target() # rospy.logerr("@@@@ 1. d2img_center: " + str(distance_to_imgcenter)) rospy.logerr("@@@@ 2. MAV altitude : " + str(height)) # rospy.logerr("@@@@ 3. flow: " + str(flow)) # rospy.logerr("@@@@ 3. h_ratio: " + str(height_ratio)+" image_height: "+str(self.image_height)) rospy.logerr("@@@@ 4. d2t: " + str(distance_from_target)) # rospy.logerr("@@@@ 5. velocity err: " + str(vel_err)) rospy.logerr("@@@@ 6. joint_obs: " + str(observations[5:])) # rospy.logerr("@@@@ 4. joint_reward: " + str(self.joint_detections_reward_gain*joint_r)) # rospy.logerr("@@@@ 7. sum(joint_probabilities)/17: " + str(sum(observations[10:])/17)) # rospy.logerr("@@@@ 8. detection: " + str(observations[3])) ''' else: if self.is_in_desired_position(): reward += self.surface_reward else:distance_to_imgcenter/self.max_distance_to_imgcenter reward += -1*self.surface_reward ''' # else: # if self.cumulated_steps != self.step_limit: # reward+=-20*(self.step_limit - self.cumulated_steps) if reward >= 0.8: rospy.logerr("") rospy.logerr( "@@@@@@@@@@@@@@@@@@@@@@@@ AWESOME AGENT @@@@@@@@@@@@@@@@@@@@") rospy.logerr("") rospy.logwarn("reward=" + str(reward)) rospy.logdebug("reward=" + str(reward)) self.reward_msg.data = reward self._reward_pub.publish(self.reward_msg) self.cumulated_reward += reward rospy.logdebug("Cumulated_reward=" + str(self.cumulated_reward)) self.cumulated_steps += 1 rospy.logdebug("Cumulated_steps=" + str(self.cumulated_steps)) return reward
def _get_obs(self): """ Read robot state :return: """ rospy.logdebug("Start Get Observation ==>") # We get the pose of the MAV # pose = self.get_gt_odom() # target = self.get_target() # target_velocity = self.get_target_velocity() # feedback = self.get_detections_feedback() alphapose = self.get_alphapose() bbox = self.get_alphapose_bbox() joint_detections = np.array(alphapose.res).reshape((17, 3)) bbox_coordinates = np.array(bbox.bbox) detection = self.get_alphapose_detection() # self.uncertainty_cost([], epsilon = -2, lower_bound = -4) # if self.lost_target_flag: # detection = 0 # else: # detection = 1 # normalizing features joint_detections[:, 0] = [ float(joint_detections[k, 0] / self.image_width) for k in range(len(joint_detections[:, 0])) ] joint_detections[:, 1] = [ float(joint_detections[k, 1] / self.image_height) for k in range(len(joint_detections[:, 1])) ] joints_flattened = joint_detections[:, 0:2].reshape(34, ) # joint_probabilities = joint_detections[:,2] # We round to only two decimals to avoid very big Observation space position, velocity of robot, position and velocity of target, # center of image, head and feet position if bbox_coordinates[0] <= 2 or bbox_coordinates[ 1] <= 2 or bbox_coordinates[ 2] >= self.image_width - 2 or bbox_coordinates[ 3] >= self.image_height - 2: self.half_visible = 1 else: self.half_visible = 0 xcenter = round((bbox_coordinates[2] - bbox_coordinates[0]) / 2) + bbox_coordinates[0] ycenter = round((bbox_coordinates[3] - bbox_coordinates[1]) / 2) + bbox_coordinates[1] observations = [\ float(detection.data*xcenter/self.image_width), float(detection.data*ycenter/self.image_height), \ float(detection.data*abs(bbox_coordinates[3]-bbox_coordinates[1])/self.image_height), \ float(detection.data*(xcenter/self.image_width - self.prev_center[0])), float(detection.data*(ycenter/self.image_height - self.prev_center[1])) ] #+list(joints_flattened) # detection.data, \ # abs(pose.position.z/15) # # feedback.xcenter - self.prev_feedback.xcenter, \ # # feedback.ycenter - self.prev_feedback.ycenter \ #+ list(joint_probabilities) # observations = list(joints_flattened) + [detection.data*xcenter/self.image_width, detection.data*ycenter/self.image_height, \ # detection.data*abs(bbox_coordinates[3]-bbox_coordinates[1])/self.image_height] self.detection_data = detection.data rospy.logwarn("Center==>" + str(xcenter) + " " + str(ycenter)) self.prev_center = [observations[0], observations[1]] rospy.logwarn("Observations==>" + str(observations)) rospy.logdebug("Observations==>" + str(observations)) rospy.logdebug("END Get Observation ==>") self.act_pretrained, _states = self.pretrained_model.predict( observation=observations[0:5], deterministic=True) return observations
def _check_all_systems_ready(self): """ Wait for actor to respawn and move """ rospy.logwarn("Wait for actor to respawn and move") rospy.sleep(5)
except (rospy.ServiceException, rospy.exceptions.ROSException), e: rospy.logwarn(str(e)) rospy.logwarn("service exception from world_model") # Get all missing objects try: rospy.wait_for_service( '/env/asr_world_model/get_missing_object_list', timeout=5) get_missing_object_list = rospy.ServiceProxy( '/env/asr_world_model/get_missing_object_list', GetMissingObjectList) self.missing_pbd_typeAndId = get_missing_object_list( ).missingObjects except (rospy.ServiceException, rospy.exceptions.ROSException), e: rospy.logwarn(str(e)) rospy.logwarn("service exception from world_model") return 'no_objects_found' # We need to filter the search_objects in random/cropbox search mode # cropbox record doesn't use object_detection mode = rospy.get_param("/scene_exploration_sm/mode") if mode == 4 or mode == 5: self.searched_object_types = set() for obj in self.missing_pbd_typeAndId: self.searched_object_types.add(str(obj.type)) global numberOfObjectDetectionRuns global numberOfAllSearchedObjectTypes numberOfObjectDetectionRuns += 1 numberOfAllSearchedObjectTypes += len(self.searched_object_types)
cumulated_reward = 0 done = False if qlearn.epsilon > 0.05: qlearn.epsilon *= epsilon_discount #print(str(x)+"=============="+str(len(qlearn.q))+"================================================================================") # Initialize the environment and get first state of the robot observation = env.reset() state = ''.join(map(str, observation)) # Show on screen the actual situation of the robot # env.render() # for each episode, we test the robot for nsteps #for i in range(nsteps): i = 0 while (True): rospy.logwarn("############### Start Step=>" + str(i)) # Pick an action based on the current state action = qlearn.chooseAction(state) rospy.logwarn("Next action is:%d", action) # Execute the action in the environment and get feedback observation, reward, done, info = env.step(action) rospy.logwarn(str(observation) + " " + str(reward)) cumulated_reward += reward if highest_reward < cumulated_reward: highest_reward = cumulated_reward nextState = ''.join(map(str, observation)) # Make the algorithm learn based on the results rospy.logwarn("# state we were=>" + str(state))
def execute(self, userdata=None): arm = self._arm_designator.resolve() if not arm: rospy.logerr("Could not resolve arm") return "failed" gravitation = 9.81 try_current = 0 measure_force = MeasureForce(self._robot) # To be able to determine the weight of the object the difference between the default weight and the weight with # the object needs to be taken. Note that initially the weight with the object is set to the default weight to # be able to enter the while loop below. arm_weight = measure_force.get_force() arm_with_object_weight = arm_weight weight_object = numpy.linalg.norm(numpy.subtract(arm_weight, arm_with_object_weight)) / gravitation while weight_object < self._minimal_weight and try_current < self._try_num: if try_current == 0: self._robot.speech.speak("Let me try to pick up the garbage") else: self._robot.speech.speak("I failed to pick up the trash, let me try again") rospy.loginfo("The weight I felt is %s", weight_object) try_current += 1 # This opening and closing is to make sure that the gripper is empty and closed before measuring the forces # It is necessary to close the gripper since the gripper is also closed at the final measurement # arm.send_gripper_goal('open') # arm.wait_for_motion_done() # arm.send_gripper_goal('close', max_torque=1.0) # arm.wait_for_motion_done() arm_weight = measure_force.get_force() rospy.loginfo("Empty weight %s", arm_weight) # Open gripper arm.send_gripper_goal('open') arm.wait_for_motion_done() # Go down and grab try: arm.move_down_until_force_sensor_edge_up(timeout=7) except TimeOutException: rospy.logwarn("No forces were felt, however no action is taken!") pass # self._robot.torso.send_goal("grab_trash_down") # self._robot.torso.wait_for_motion_done() # rospy.sleep(3) arm.send_gripper_goal('close', max_torque=1.0) arm.wait_for_motion_done() # Go up and back to pre grasp position self._robot.torso.send_goal("grab_trash_up") self._robot.torso.wait_for_motion_done() arm_with_object_weight = measure_force.get_force() rospy.loginfo("Full weight %s", arm_with_object_weight) weight_object = numpy.linalg.norm(numpy.subtract(arm_weight, arm_with_object_weight)) / gravitation rospy.loginfo("weight_object = {}".format(weight_object)) # Go back and pull back arm self._robot.head.look_up() self._robot.head.wait_for_motion_done() self._robot.base.force_drive(-0.5, 0, 0, 2.0) arm.send_joint_goal('handover') arm.wait_for_motion_done() arm.send_joint_goal('reset') arm.wait_for_motion_done() self._robot.head.reset() # # if weight_object < self._minimal_weight: # return "failed" self._robot.speech.speak("Look at this I can pick up the trash!") handed_entity = EntityInfo(id="trash") arm.occupied_by = handed_entity return "succeeded"
def update_action_state(self, string_msg): if string_msg.data in self.allowed_action_state_list: self.action_state = string_msg.data else: rospy.logwarn('update_action_state: \'%s\' value not allowed for action state' %(string_msg.data))
def create_osm_node_dict(xml_file_location): ''' Receives: xml_file_location: a string with the location of the OSM file to be processed Returns: osm_node_dict: a dictionary with all the nodes in the map osm_tag_dict: a dictionary with all the found tags, and it's corresponding node ids. Each key of the fictionary corresponds to a tag, and the value of each key is a list with the id/ids of node/s with this tag. origin_node: an OMSNode corresponding to the origin node If the OSM Map does not define an origin node, it will return None ''' XML_file = xml_file_location tree = ET.parse(XML_file) xml_root = tree.getroot() osm_node_dict = dict() osm_tag_dict = dict() origin_node = None # Dict of traffic light nodes to their series, which is a # string denoting whether they are to be turned on or # off at a timestep osm_traffic_light_nodes = dict() # dict of crosswalk nodes: # the key is the crosswalk node, # the value its id osm_crosswalk_nodes = defaultdict(list) for node in xml_root.findall('node'): add_flag = True for tag in node.findall('tag'): node_id = int( node.get('id') ) origin_lat = float( node.get('lat') ) origin_lon = float( node.get('lon') ) if tag.get('k') == "origin": if tag.get('v') == "true": origin_node = RoadAuxClasses.OSMNode(node_id, origin_lat, origin_lon) # This node should not be added to the node list osm_tag_dict['origin'] = [] osm_tag_dict['origin'].append(node_id) continue elif tag.get('k') == 'traffic': if tag.get('v') == 'light': if tag.get('series') == None: logwarn('Traffic light requires a series') else: osm_traffic_light_nodes[node_id] = tag.get('series') elif tag.get('k') == 'crosswalk': osm_crosswalk_nodes[int(tag.get('v'))].append(node_id) else: tag_string = tag.get('v') special_node = RoadAuxClasses.OSMNode(node_id, origin_lat, origin_lon, tag = tag_string) osm_node_dict[node_id] = special_node if osm_tag_dict.has_key(tag_string): osm_tag_dict[tag_string].append(node_id) else: osm_tag_dict[tag_string] = [node_id] continue node_id = int( node.get('id') ) lat = float( node.get('lat') ) lon = float( node.get('lon') ) osm_node_dict[node_id] = RoadAuxClasses.OSMNode(node_id, lat, lon) return [osm_node_dict, osm_tag_dict, origin_node, osm_traffic_light_nodes, osm_crosswalk_nodes]
def update_system_state(self, string_msg): if string_msg.data in self.allowed_system_state_list: self.system_state = string_msg.data else: rospy.logwarn('update_system_state: \'%s\' value not allowed for system state' %(string_msg.data))
def run(self): if self.active_mission_len > 0: try: if self.manual_mission.fsm_state == manual_mission.State.IDLE: self.active_waypoint_gps = self.active_mission_gps[ self.active_mission_idx] else: self.active_waypoint_gps = self.manual_mission.mission[ self.manual_mission.mission_idx] except IndexError as err: rospy.logwarn(err) rospy.logwarn( "Can't assign active waypoint. Mission is not up to date yet." ) if self.main_mode in PAUSE_LIST_MAIN or self.sub_mode in PAUSE_LIST_SUB: # print("Setting paused!") self.fsm_state = State.PAUSED # ------------------------------------------------------------------------------ # if self.fsm_state == State.GROUNDED: if self.new_mission: # prepare for upload and change state self.active_mission_gps = self.pending_mission_gps self.active_mission_ml = self.gps_to_mavlink( self.active_mission_gps) self.new_mission = False self.fsm_state = State.CLEARING_MISSION self.clear_mission_pub.publish(Empty()) # ------------------------------------------------------------------------------ # elif self.fsm_state == State.REQUESTING_UPLOAD: request = UploadMissionRequest( drone_id=self.id, waypoints=self.active_mission_ml.waypoints) response = self.upload_mission(request) if response.success: self.fsm_state = State.UPLOADING self.upload_done = False # ------------------------------------------------------------------------------ # elif self.fsm_state == State.UPLOADING: # only a positive ack counts as a succesful upload (see callback) if self.upload_done: if self.state == "Standby": request = TakeoffDroneRequest(on_the_spot=True, alt=20) response = self.takeoff(request) if response.success: self.fsm_state = State.TAKEOFF elif self.state == "Active": self.manual_mission.stop_running() self.manual_mission.reset() self.fsm_state = State.SYNC_WP_IDX elif self.upload_failed: rospy.Timer(rospy.Duration(UPLOAD_DELAY), self.wait_cb, oneshot=True) self.wait = True self.fsm_state = State.WAITING self.upload_failed = False # ------------------------------------------------------------------------------ # elif self.fsm_state == State.TAKEOFF: if self.sub_mode == "Takeoff": response = self.arm() if response.success: self.fsm_state = State.ARMING else: if self.cmd_try_again: request = TakeoffDroneRequest(on_the_spot=True, alt=20) response = self.takeoff(request) if response.success: self.cmd_try_again = False # ------------------------------------------------------------------------------ # elif self.fsm_state == State.ARMING: if self.armed and self.relative_alt > 19: self.gcs_status = DroneInfo.Run response = self.set_mode(flight_modes.MISSION) if response.success: self.fsm_state = State.SET_MISSION else: if self.cmd_try_again: response = self.arm() if response.success: self.cmd_try_again = False # ------------------------------------------------------------------------------ # elif self.fsm_state == State.SYNC_WP_IDX: if self.manual_mission.mission_idx == self.active_mission_idx: response = self.set_mode(flight_modes.MISSION) if response.success: self.fsm_state = State.SET_MISSION else: print("[drone]: Manual idx = {}, Active idx = {}".format( self.manual_mission.mission_idx, self.active_mission_idx)) self.set_current_mission_pub.publish( Int16(self.manual_mission.mission_idx)) # ------------------------------------------------------------------------------ # elif self.fsm_state == State.SET_MISSION: if self.sub_mode == "Mission": self.gcs_status = DroneInfo.Run request = ChangeSpeedRequest(MISSION_SPEED) response = self.change_speed(request) if response.success: self.fsm_state = State.SET_SPEED else: if self.cmd_try_again: response = self.set_mode(flight_modes.MISSION) if response.success: self.cmd_try_again = False # ------------------------------------------------------------------------------ # elif self.fsm_state == State.SET_SPEED: if self.speed_ack: self.fsm_state = State.FLYING_MISSION self.speed_ack = False else: if self.cmd_try_again: request = ChangeSpeedRequest(MISSION_SPEED) response = self.change_speed(request) if response.success: self.cmd_try_again = False # ------------------------------------------------------------------------------ # elif self.fsm_state == State.FLYING_MISSION: if self.new_mission: self.manual_mission.start_running() self.active_mission_idx = 0 self.new_mission = False # elif self.manual_mission.fsm_state == manual_mission.State.ON_THE_WAY: self.fsm_state = State.CLEARING_MISSION self.clear_mission_pub.publish(Empty()) if self.is_on_goal(): # self.fsm_state = State.LANDING lat_goal = self.active_mission_gps[-1].latitude lon_goal = self.active_mission_gps[-1].longitude # reposition to a lower altitude above the goal request = GotoWaypointRequest(relative_alt=True, ground_speed=MISSION_SPEED, latitude=lat_goal, longitude=lon_goal, altitude=10) response = self.reposition(request) if response.success: rospy.Timer(rospy.Duration(HOLD_TIME), self.hold_cb, oneshot=True) self.hold = True self.fsm_state = State.HOLD elif self.loiter and self.ground_speed < 0.5: self.gcs_status = DroneInfo.holding # ------------------------------------------------------------------------------ # elif self.fsm_state == State.REPOSITION: if self.new_mission: self.manual_mission.start_running() self.active_mission_idx = 0 self.new_mission = False # elif self.manual_mission.fsm_state == manual_mission.State.ON_THE_WAY: self.fsm_state = State.CLEARING_MISSION self.clear_mission_pub.publish(Empty()) else: if self.cmd_try_again: request = GotoWaypointRequest( relative_alt=True, ground_speed=MISSION_SPEED, latitude=self.holding_waypoint.position.latitude, longitude=self.holding_waypoint.position.longitude, altitude=self.holding_waypoint.position.altitude) response = self.reposition(request) if response.success: self.cmd_try_again = False # ------------------------------------------------------------------------------ # elif self.fsm_state == State.LANDING: if self.state == "Standby": self.fsm_state = State.GROUNDED self.gcs_status = DroneInfo.Land # reset states self.reset() self.manual_mission.reset() else: if self.cmd_try_again: response = self.set_mode(flight_modes.MISSION) if response.success: self.cmd_try_again = False # ------------------------------------------------------------------------------ # elif self.fsm_state == State.CLEARING_MISSION: if self.upload_done: self.upload_done = False self.fsm_state = State.REQUESTING_UPLOAD # ------------------------------------------------------------------------------ # elif self.fsm_state == State.PAUSED: if self.state == "Active" and self.sub_mode == "Mission": self.fsm_state = State.FLYING_MISSION elif self.state == "Standby" or not self.armed: self.fsm_state = State.GROUNDED # ------------------------------------------------------------------------------ # elif self.fsm_state == State.WAITING: if not self.wait: self.fsm_state = State.REQUESTING_UPLOAD # ------------------------------------------------------------------------------ # elif self.fsm_state == State.HOLD: if not self.hold: response = self.set_mode(flight_modes.MISSION) if response.success: self.fsm_state = State.LANDING else: if self.cmd_try_again: lat_goal = self.active_mission_gps[-1].latitude lon_goal = self.active_mission_gps[-1].longitude # reposition to a lower altitude above the goal request = GotoWaypointRequest(relative_alt=True, ground_speed=MISSION_SPEED, latitude=lat_goal, longitude=lon_goal, altitude=10) response = self.reposition(request) if response.success: self.cmd_try_again = False
# prepare to call AuctionService in the node that will be the auctioneer service_path = nearest_node + '/auction_server' rospy.wait_for_service(service_path) auctioneer_service = rospy.ServiceProxy(service_path, auction_srvs.srv.AuctionService) try: auctioneer_service_resp = auctioneer_service(role, auction_type, sending_node, nodes_collected, auction_data) rospy.loginfo("[message] Auctioneer response") rospy.loginfo(auctioneer_service_resp) except rospy.ServiceException, e: rospy.logwarn("Service did not process request: %s", e) # here we want to unregister services (clean-up procedure) # ATTENTION: here we need to revert the order of calling services, since the call to auction_config_server will shutdown auctioneer services rospy.set_param('/auction_closed', True) # Getting results for statistic studies # get collected nodes nodes_collected_list = rospy.get_param('/nodes_collected').split(',') # remove '' strings while '' in nodes_collected_list: nodes_collected_list.remove('') # remove duplicates nodes_collected_list = list(set(nodes_collected_list)) nodes_collected_str = ','.join(nodes_collected_list)
def new_mission_cb(mission_msg): try: auth = rospy.ServiceProxy('/dji_sdk/sdk_control_authority', SDKControlAuthority) rospy.loginfo('Service. Sdk control authority:') resp = auth(1) rospy.loginfo(resp.result) if not resp.result: return except rospy.ServiceException as e: rospy.logwarn(e) return try: mission = rospy.ServiceProxy('/dji_sdk/mission_waypoint_upload', MissionWpUpload) mission_task_msg = MissionWaypointTask() mission_task_msg.velocity_range = 10; mission_task_msg.idle_velocity = 10; mission_task_msg.action_on_finish = MissionWaypointTask.FINISH_RETURN_TO_HOME; mission_task_msg.mission_exec_times = 1; mission_task_msg.yaw_mode = MissionWaypointTask.YAW_MODE_AUTO; mission_task_msg.trace_mode = MissionWaypointTask.TRACE_POINT; mission_task_msg.action_on_rc_lost = MissionWaypointTask.ACTION_AUTO; mission_task_msg.gimbal_pitch_mode = MissionWaypointTask.GIMBAL_PITCH_FREE; rospy.loginfo('Received mission from objective:') rospy.loginfo(mission_msg) for item in mission_msg.waypoints: cmd_parameter = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] mission_task_msg.mission_waypoint.append(MissionWaypoint( latitude = item.latitude, longitude = item.longitude, altitude = item.altitude, damping_distance = 2, target_yaw = 0, has_action = 1, target_gimbal_pitch = 0, turn_mode = 0, action_time_limit = 64000, waypoint_action = MissionWaypointAction( action_repeat = 10, command_parameter = cmd_parameter ))) rospy.logdebug(mission_task_msg) rospy.loginfo('Service. Mission waypoint upload:') resp = mission(mission_task_msg) rospy.loginfo(resp.result) if not resp.result: return except rospy.ServiceException as e: rospy.logwarn(e) return try: start = rospy.ServiceProxy('/dji_sdk/mission_waypoint_action', MissionWpAction) resp = start(0) rospy.loginfo('Service. Mission waypoint action:') rospy.loginfo(resp.result) if not resp.result: return except rospy.ServiceException as e: rospy.logwarn(e) return
rospy.logdebug("############### START EPISODE=>" + str(x)) cumulated_reward = 0 done = False if qlearn.epsilon > 0.05: qlearn.epsilon *= epsilon_discount # Initialize the environment and get first state of the robot observation = env.reset() state = ''.join(map(str, observation)) # Show on screen the actual situation of the robot # env.render() # for each episode, we test the robot for nsteps for i in range(nsteps): rospy.logwarn("############### Start Step=>" + str(i)) # Pick an action based on the current state action = qlearn.chooseAction(state) rospy.logdebug("Next action is:%d", action) # Execute the action in the environment and get feedback observation, reward, done, info = env.step(action) rospy.logdebug(str(observation) + " " + str(reward)) cumulated_reward += reward if highest_reward < cumulated_reward: highest_reward = cumulated_reward nextState = ''.join(map(str, observation)) # Make the algorithm learn based on the results rospy.logdebug("# state we were=>" + str(state))
def __init__(self, language_code='it-IT', last_contexts=None): """Initialize all params and load data""" """ Constants and params """ self.CHUNK = 4096 self.FORMAT = pyaudio.paInt16 self.CHANNELS = 1 self.RATE = 44100 #was 16000 self.USE_AUDIO_SERVER = rospy.get_param( '/dialogflow_client/use_audio_server', False) if self.USE_AUDIO_SERVER: print("USO AUDIOSERVER") else: print("USO IL MICROFONO") self.PLAY_AUDIO = rospy.get_param('/dialogflow_client/play_audio', True) self.DEBUG = rospy.get_param('/dialogflow_client/debug', False) # Register Ctrl-C sigint signal.signal(signal.SIGINT, self._signal_handler) """ Dialogflow setup """ # Get hints/clues rp = rospkg.RosPack() file_dir = rp.get_path('dialogflow_ros') + '/config/context.yaml' with open(file_dir, 'r') as f: try: self.phrase_hints = load(f) except YAMLError: rospy.logwarn( "DF_CLIENT: Unable to open phrase hints yaml file!") self.phrase_hints = [] # Dialogflow params project_id = rospy.get_param('/dialogflow_client/project_id', 'robot-15de1') session_id = str(uuid4()) # Random self._language_code = language_code self.last_contexts = last_contexts if last_contexts else [] # DF Audio Setup audio_encoding = AudioEncoding.AUDIO_ENCODING_LINEAR_16 # Possibel models: video, phone_call, command_and_search, default self._audio_config = InputAudioConfig( audio_encoding=audio_encoding, language_code=self._language_code, sample_rate_hertz=self.RATE, phrase_hints=self.phrase_hints, model='default') self._output_audio_config = OutputAudioConfig( audio_encoding=OutputAudioEncoding.OUTPUT_AUDIO_ENCODING_LINEAR_16) # Create a session self._session_cli = dialogflow_v2beta1.SessionsClient() self._session = self._session_cli.session_path(project_id, session_id) rospy.logdebug("DF_CLIENT: Session Path: {}".format(self._session)) """ ROS Setup """ results_topic = rospy.get_param('/dialogflow_client/results_topic', '/dialogflow_client/results') requests_topic = rospy.get_param('/dialogflow_client/requests_topic', '/dialogflow_client/requests') text_req_topic = requests_topic + '/string_msg' text_event_topic = requests_topic + '/string_event' msg_req_topic = requests_topic + '/df_msg' event_req_topic = requests_topic + '/df_event' self._results_pub = rospy.Publisher(results_topic, DialogflowResult, queue_size=10) rospy.Subscriber(text_req_topic, String, self._text_request_cb) rospy.Subscriber(text_event_topic, String, self._text_event_cb) rospy.Subscriber(msg_req_topic, DialogflowRequest, self._msg_request_cb) rospy.Subscriber(event_req_topic, DialogflowEvent, self._event_request_cb) self.pub_chatter = rospy.Publisher('/chatter', String, queue_size=10) """ Audio setup """ # Mic stream input setup self.audio = pyaudio.PyAudio() self._server_name = rospy.get_param('/dialogflow_client/server_name', '127.0.0.1') self._port = rospy.get_param('/dialogflow_client/port', 4444) print("Audio Server at: ", self._server_name, ", on port ", self._port) if self.PLAY_AUDIO: self._create_audio_output() rospy.logdebug("DF_CLIENT: Last Contexts: {}".format( self.last_contexts)) rospy.loginfo("DF_CLIENT: Ready!")
def _gen_xarco(): rospy.init_node('modify_xacro') if len(sys.argv) < 3: rospy.logwarn('usage: modify_xacro_node model new_model replace') return model = sys.argv[1] new_model = sys.argv[2] replace = False if len(sys.argv) == 4: if sys.argv[3].upper() == 'REPLACE': replace = True rospy.logwarn('origin xacro file will be replaced') rospy.wait_for_service('tmr/ask_item') ask_item = rospy.ServiceProxy('tmr/ask_item', AskItem) res_dh = ask_item('dh', 'DHTable', 1.0) res_dd = ask_item('dd', 'DeltaDH', 1.0) if not res_dh.value.startswith('DHTable={') or not res_dh.value.endswith('}'): rospy.logerr('invalid dh') return if not res_dd.value.startswith('DeltaDH={') or not res_dd.value.endswith('}'): rospy.logerr('invalid delta_dh') return rospy.loginfo(res_dh.value) rospy.loginfo(res_dd.value) dh_strs = res_dh.value[9:-1].split(',') dd_strs = res_dd.value[9:-1].split(',') ''' res_dh = 'DHTable={0,-90,0,145.1,0,-277,277,-90,0,429,0,0,-187,187,0,0,411.5,0,0,-162,162,90,90,0,-122.2,0,-187,187,0,90,0,106,0,-187,187,0,0,0,114.4,0,-277,277}' res_dd = 'DeltaDH={-0.001059821,0.02508766,0.009534874,0,0.001116668,0.06614932,0.308224,0.0287381,0.06797475,-0.0319523,0.3752921,0.06614756,-0.006998898,0.06792655,-0.06083903,0.02092069,0.02965812,-0.1331249,0.06793034,0.02077797,0.08265772,0.03200645,0.01835932,0.06145732,0.08273286,0.6686108,0.6972408,-0.1793097,-0.0794057,1.425708}' rospy.loginfo(res_dh) rospy.loginfo(res_dd) dh_strs = res_dh[9:-1].split(',') dd_strs = res_dd[9:-1].split(',') ''' if len(dh_strs) != 42: rospy.logerr('invalid dh') return if len(dd_strs) != 30: rospy.logerr('invalid delta_dh') return dh = [float(i) for i in dh_strs] dd = [float(i) for i in dd_strs] # find xacro path curr_path = os.path.dirname(os.path.abspath(__file__)) dirs = ['src', 'devel'] ind = -1 for d in dirs: ind = curr_path.find(d) if (ind != -1): break if (ind == -1) : rospy.logerr('can not find workspace directory') return src_path = curr_path[:ind] + 'src' xacro_path = '' for dirpath, dirnames, filenames in os.walk(src_path): if dirpath.endswith('tmr_description'): xacro_path = dirpath + '/xacro' break if (xacro_path == ''): rospy.logerr('can not find xacro directory') return xacro_name = '/macro.' + model + '.urdf.xacro' new_xacro_name = '/macro.' + new_model + '.urdf.xacro' file_in = xacro_path + xacro_name file_out = xacro_path + new_xacro_name link_tag = '<!--LinkDescription-->' link_head = '<?xml version=\'1.0\' encoding=\'UTF-8\'?>\n' link_start = '<data xmlns:xacro="http://wiki.ros.org/xacro">' link_end = '</data>' rospy.loginfo(file_in) fr = open(file_in, 'r') data_in = fr.read() fr.close() datas = data_in.split(link_tag) if len(datas) < 3: rospy.logerr('invalid tmr...xacro') return link_data = link_start + datas[1] + link_end root = ET.fromstring(link_data) udh = urdf_DH_from_tm_DH(dh, dd) xyzs, rpys = xyzrpys_from_urdf_DH(udh) modify_urdf(root, xyzs, rpys, udh, '${prefix}') link_data = ET.tostring(root, encoding='UTF-8').decode('UTF-8') link_data = link_data.replace('ns0', 'xacro') link_data = link_data.replace(link_head, '', 1) link_data = link_data.replace(link_start, link_tag, 1) link_data = link_data.replace(link_end, link_tag, 1) data_out = datas[0] + link_data + datas[2] file_save = '' if replace: file_save = file_in rospy.loginfo('copy and rename origin xacro file') shutil.copyfile(file_in, file_out) else: file_save = file_out rospy.loginfo(file_save) fw = open(file_save, 'w') fw.write(data_out) fw.close()
def get_classification(self, image): """Determines the color of the traffic light in the image Args: image (cv::Mat): image containing the traffic light Returns: int: ID of traffic light color (specified in styx_msgs/TrafficLight) """ #TODO implement light color prediction with self.detection_graph.as_default(): with tf.Session(graph=self.detection_graph) as sess: image_tensor = self.detection_graph.get_tensor_by_name('image_tensor:0') detect_boxes = self.detection_graph.get_tensor_by_name('detection_boxes:0') detect_scores = self.detection_graph.get_tensor_by_name('detection_scores:0') detect_classes = self.detection_graph.get_tensor_by_name('detection_classes:0') num_detections = self.detection_graph.get_tensor_by_name('num_detections:0') image_expanded = np.expand_dims(image, axis=0) (boxes, scores, classes, num) = sess.run( [detect_boxes, detect_scores, detect_classes, num_detections], feed_dict={image_tensor: image_expanded}) vis_util.visualize_boxes_and_labels_on_image_array( image, np.squeeze(boxes), np.squeeze(classes).astype(np.int32), np.squeeze(scores), self.category_index, use_normalized_coordinates=True, max_boxes_to_draw=5, line_thickness=5) boxes = np.squeeze(boxes) scores = np.squeeze(scores) classes = np.squeeze(classes).astype(np.int32) keep = scores > 0.5 # if scores[0] > 0.5: # state = self.class_map[classes[0]] # else: # state = TrafficLight.UNKNOWN #### select the dominant traffic light class if np.any(keep): boxes = boxes[keep] scores = scores[keep] classes = classes[keep] members, index, counts = np.unique(classes, return_inverse=True, return_counts=True) member_scores = np.zeros((len(members),)) for i in range(len(members)): member_scores[i] = np.sum(scores[index == i]) select = np.argmax(member_scores) winner = members[select] state = self.class_map[winner] else: state = TrafficLight.UNKNOWN ### log rospy.logwarn("detected light : {} ".format(classes[0])) return image, state
def close(self): try: self._srv_arm.unregister() except AttributeError, e: rospy.logwarn("Failed to unregister /tuck_arm service")
self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False) self.server.start() rospy.spin() def actionCb(self, goal): """ Take an input command of width to open gripper. """ rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position) # send command to gripper self.model.setCommand(goal.command) # publish feedback while True: if self.server.is_preempt_requested(): self.server.set_preemtped() rospy.loginfo('Gripper Controller: Preempted.') return # TODO: get joint position, break when we have reached goal break self.server.set_succeeded() rospy.loginfo('Gripper Controller: Succeeded.') def stateCb(self, msg): self.state = msg if __name__=='__main__': try: rospy.logwarn("Please use gripper_controller (no .py)") GripperActionController() except rospy.ROSInterruptException: rospy.loginfo('Hasta la Vista...')
def _signal_handler(self, signal, frame): rospy.logwarn("\nDF_CLIENT: SIGINT caught!") self.exit()
class CalculatePersonPosition(smach.State): def __init__(self, controller, controller_2=None, sensor=None, max_distance=2.5, onlyhorizontal=False, knownperson=True): self.person_sensor = controller self.max_distance = max_distance self.person_id = sensor self.talk_known = controller_2 self.ignoreknownperson = knownperson self.talks = [ 'Oh, ich denke Dich habe ich schon begrüsst', 'Dich kenne ich schon, ich mache weiter', 'Oh schön dich wieder zu sehen, bis gleich.', 'Wen haben wir denn da, dich kenne ich. Gleich gehen die Vorträge los!', 'Hallo noch mal. Wie wäre es wenn wir uns nachher unterhalten?' ] self.counter = 0 # https://answers.ros.org/question/10777/service-exception-using-tf-listener-in-rospy self.tf = tf.TransformListener() input = [] self.onlyhorizontal = onlyhorizontal if onlyhorizontal: input = ['old_vertical'] smach.State.__init__( self, input_keys=input, outcomes=['success', 'repeat', 'no_person_found', 'known'], output_keys=['person_angle_vertical', 'person_angle_horizontal']) def execute(self, userdata): self.person = None self.person_sensor.clearPerson() rospy.sleep(0.1) self.person = self.person_sensor.getDetectedPerson() self.pose = None self.transformid = None self.dist = self.max_distance for p in self.person: pose = p.pose dist = distance(pose.pose.position) rospy.logwarn if not self.tf.frameExists('base_link'): rospy.logwarn('Base_link does not exist!') if not self.tf.frameExists(pose.header.frame_id): rospy.logwarn('%s does not exist!' % str(pose.header.frame_id)) if dist < self.dist and self.tf.frameExists( pose.header.frame_id) and self.tf.frameExists('base_link'): try: print(p.pose) self.tf.waitForTransform('base_link', 'CameraDepth_optical_frame', rospy.Time.now(), rospy.Duration(4.0)) po = self.tf.transformPose("base_link", pose) self.dist = dist self.pose = po.pose self.transformid = p.transformid except Exception, e: print("Exception %s" % e) return 'repeat' if dist > self.max_distance and self.max_distance == self.dist: print('Detected person to far away. Distance: %s ' % dist) if self.pose: (vertical, horizontal) = rotation(self.pose) self.counter = 0 if self.onlyhorizontal: userdata.person_angle_vertical = userdata.old_vertical else: userdata.person_angle_vertical = vertical userdata.person_angle_horizontal = horizontal if self.dist < 1.8 and self.transformid is not None: try: known, name = self.person_id.identify(self.transformid) if known and self.ignoreknownperson: rospy.loginfo("Person is known, iterating") if self.talk_known is not None: self.talk_known.say_something( random.choice(self.talks)) return 'known' else: return 'success' except Exception, e: rospy.logwarn( "Something went wront while identifying a person") return 'success'
def process_trajectory(self, traj): num_points = len(traj.points) # make sure the joints in the goal match the joints of the controller if set(self.joint_names) != set(traj.joint_names): res = FollowJointTrajectoryResult().INVALID_JOINTS msg = 'Incoming trajectory joints do not match the joints of the controller' rospy.logerr(msg) rospy.logerr(' self.joint_names={}' % (set(self.joint_names))) rospy.logerr(' traj.joint_names={}' % (set(traj.joint_names))) self.action_server.set_aborted(result=res, text=msg) return # make sure trajectory is not empty if num_points == 0: msg = 'Incoming trajectory is empty' rospy.logerr(msg) self.action_server.set_aborted(text=msg) return # correlate the joints we're commanding to the joints in the message # map from an index of joint in the controller to an index in the trajectory lookup = [traj.joint_names.index(joint) for joint in self.joint_names] durations = [0.0] * num_points # find out the duration of each segment in the trajectory durations[0] = traj.points[0].time_from_start.to_sec() for i in range(1, num_points): durations[i] = (traj.points[i].time_from_start - traj.points[i - 1].time_from_start).to_sec() if not traj.points[0].positions: res = FollowJointTrajectoryResult().INVALID_GOAL msg = 'First point of trajectory has no positions' rospy.logerr(msg) self.action_server.set_aborted(result=res, text=msg) return trajectory = [] time = rospy.Time.now() + rospy.Duration(0.01) for i in range(num_points): seg = Segment(self.num_joints) if traj.header.stamp == rospy.Time(0.0): seg.start_time = (time + traj.points[i].time_from_start ).to_sec() - durations[i] else: seg.start_time = ( traj.header.stamp + traj.points[i].time_from_start).to_sec() - durations[i] seg.duration = durations[i] # Checks that the incoming segment has the right number of elements. if traj.points[i].velocities and len( traj.points[i].velocities) != self.num_joints: res = FollowJointTrajectoryResult().INVALID_GOAL msg = 'Command point %d has %d elements for the velocities' % ( i, len(traj.points[i].velocities)) rospy.logerr(msg) self.action_server.set_aborted(result=res, text=msg) return if len(traj.points[i].positions) != self.num_joints: res = FollowJointTrajectoryResult().INVALID_GOAL msg = 'Command point %d has %d elements for the positions' % ( i, len(traj.points[i].positions)) rospy.logerr(msg) self.action_server.set_aborted(result=res, text=msg) return for j in range(self.num_joints): if traj.points[i].velocities: seg.velocities[j] = traj.points[i].velocities[lookup[j]] if traj.points[i].positions: seg.positions[j] = traj.points[i].positions[lookup[j]] trajectory.append(seg) rospy.loginfo('Trajectory start requested at %.3lf, waiting...', traj.header.stamp.to_sec()) rate = rospy.Rate(self.update_rate) while traj.header.stamp > time: time = rospy.Time.now() rate.sleep() end_time = traj.header.stamp + rospy.Duration(sum(durations)) seg_end_times = [ rospy.Time.from_sec(trajectory[seg].start_time + durations[seg]) for seg in range(len(trajectory)) ] rospy.loginfo( 'Trajectory start time is %.3lf, end time is %.3lf, total duration is %.3lf', time.to_sec(), end_time.to_sec(), sum(durations)) self.trajectory = trajectory traj_start_time = rospy.Time.now() for seg in range(len(trajectory)): rospy.logdebug( 'current segment is %d time left %f cur time %f' % (seg, durations[seg] - (time.to_sec() - trajectory[seg].start_time), time.to_sec())) rospy.logdebug('goal positions are: %s' % str(trajectory[seg].positions)) # first point in trajectories calculated by OMPL is current position with duration of 0 seconds, skip it if durations[seg] == 0: rospy.logdebug( 'skipping segment %d with duration of 0 seconds' % seg) continue multi_packet = {} for port, joints in self.port_to_joints.items(): vals = [] for joint in joints: j = self.joint_names.index(joint) start_position = self.joint_states[joint].current_pos if seg != 0: start_position = trajectory[seg - 1].positions[j] desired_position = trajectory[seg].positions[j] desired_velocity = max( self.min_velocity, abs(desired_position - start_position) / durations[seg]) self.msg.desired.positions[j] = desired_position self.msg.desired.velocities[j] = desired_velocity # probably need a more elegant way of figuring out if we are dealing with a dual controller if hasattr(self.joint_to_controller[joint], "master_id"): master_id = self.joint_to_controller[joint].master_id slave_id = self.joint_to_controller[joint].slave_id master_pos, slave_pos = self.joint_to_controller[ joint].pos_rad_to_raw(desired_position) spd = self.joint_to_controller[joint].spd_rad_to_raw( desired_velocity) vals.append((master_id, master_pos, spd)) vals.append((slave_id, slave_pos, spd)) else: motor_id = self.joint_to_controller[joint].motor_id pos = self.joint_to_controller[joint].pos_rad_to_raw( desired_position) spd = self.joint_to_controller[joint].spd_rad_to_raw( desired_velocity) vals.append((motor_id, pos, spd)) multi_packet[port] = vals for port, vals in multi_packet.items(): self.port_to_io[port].set_multi_position_and_speed(vals) while time < seg_end_times[seg]: # check if new trajectory was received, if so abort current trajectory execution # by setting the goal to the current position if self.action_server.is_preempt_requested(): msg = 'New trajectory received. Aborting old trajectory.' multi_packet = {} for port, joints in self.port_to_joints.items(): vals = [] for joint in joints: cur_pos = self.joint_states[joint].current_pos motor_id = self.joint_to_controller[joint].motor_id pos = self.joint_to_controller[ joint].pos_rad_to_raw(cur_pos) vals.append((motor_id, pos)) multi_packet[port] = vals for port, vals in multi_packet.items(): self.port_to_io[port].set_multi_position(vals) self.action_server.set_preempted(text=msg) rospy.logwarn(msg) return rate.sleep() time = rospy.Time.now() # Verifies trajectory constraints for j, joint in enumerate(self.joint_names): if self.trajectory_constraints[ j] > 0 and self.msg.error.positions[ j] > self.trajectory_constraints[j]: res = FollowJointTrajectoryResult().PATH_TOLERANCE_VIOLATED msg = 'Unsatisfied position constraint for %s, trajectory point %d, %f is larger than %f' % \ (joint, seg, self.msg.error.positions[j], self.trajectory_constraints[j]) rospy.logwarn(msg) self.action_server.set_aborted(result=res, text=msg) return # let motors roll for specified amount of time rospy.sleep(self.goal_time_constraint) for i, j in enumerate(self.joint_names): rospy.logdebug( 'desired pos was %f, actual pos is %f, error is %f' % (trajectory[-1].positions[i], self.joint_states[j].current_pos, self.joint_states[j].current_pos - trajectory[-1].positions[i])) # Checks that we have ended inside the goal constraints for (joint, pos_error, pos_constraint) in zip(self.joint_names, self.msg.error.positions, self.goal_constraints): if pos_constraint > 0 and abs(pos_error) > pos_constraint: res = FollowJointTrajectoryResult().GOAL_TOLERANCE_VIOLATED msg = 'Aborting because %s joint wound up outside the goal constraints, %f is larger than %f' % \ (joint, pos_error, pos_constraint) rospy.logwarn(msg) self.action_server.set_aborted(result=res, text=msg) break else: res = FollowJointTrajectoryResult().SUCCESSFUL msg = 'Trajectory execution successfully completed' rospy.loginfo(msg) self.action_server.set_succeeded(result=res, text=msg)
def RTL(self): init_time = time.time() velocity = 0.3 ds = velocity / 60.0 transition_time = time.time() - init_time rospy.logwarn('Time in setup: ' + str(transition_time)) self.rate.sleep() height = self.drone_pose.pose.position.z rospy.loginfo('Position: (' + str(self.drone_pose.pose.position.x) + ', ' + str(self.drone_pose.pose.position.y) + ', ' + str(self.drone_pose.pose.position.z) + ')') self.set_position(0, 0, height) self.rate.sleep() rospy.loginfo('Position: (' + str(self.drone_pose.pose.position.x) + ', ' + str(self.drone_pose.pose.position.y) + ', ' + str(self.drone_pose.pose.position.z) + ')') rospy.loginfo('Goal Position: (' + str(self.goal_pose.pose.position.x) + ', ' + str(self.goal_pose.pose.position.y) + ', ' + str(self.goal_pose.pose.position.z) + ')') while not self.chegou(): rospy.loginfo('Executing State RTL') rospy.loginfo("STARING HOME") self.set_position(0, 0, height) self.rate.sleep() t = 0 self.set_position(0, 0, height - ds) self.rate.sleep() init_time = time.time() while not (self.drone_pose.pose.position.z < -0.1) and time.time( ) - init_time < (height / velocity) * 1.2: #20% tolerance in time rospy.loginfo('Executing State RTL') rospy.loginfo('Height: ' + str(abs(self.drone_pose.pose.position.z))) ################# Velocity Control ################# self.set_vel(0, 0, -0.3, 0, 0, 0) #################### Position Control ############## # if not self.chegou(): # rospy.logwarn ('LANDING AT ' + str(velocity) + 'm/s') # if t <= height: # t += ds # self.set_position(0,0,height - t) # self.rate.sleep() # # else: # if t <= height: # t += ds # self.set_position(0,0,height - t) # else: # self.set_position(0,0,0) # self.rate.sleep() #################################################### rospy.logwarn("DESARMANDO DRONE " + self.mav_name) self.arm(False) return "succeeded"
def traffic_cb(self, msg): # Callback for /traffic_waypoint message. # Set speed for waypoints before red lights and stop points. # Recover waypoint stop position stop_waypoint_index = msg.data # Check if its a new index point if self.last_stop_index == stop_waypoint_index: return should_brake = True if self.current_waypoint_index is None: # Ignore return if stop_waypoint_index == -1: # No Red light ahead, recover original waypoitns # Accelerate stop_waypoint_index = self.current_waypoint_index + LOOKAHEAD_WPS if stop_waypoint_index > len(self.waypoints): stop_waypoint_index = len(self.waypoints) should_brake = False distance_full = 99 rospy.logwarn("Accelerate Car") else: # Compute distance to stop distance_full = self.distance(self.waypoints, self.current_waypoint_index, stop_waypoint_index - 1) if distance_full > 100: #ignore rospy.logwarn("Ignore Light Distance") return # Set the number of slow (or accelerate) steps steps = float(int(distance_full / 10)) if steps == 0: steps = 1 if distance_full < 30: # Minimum number of steps steps = 4 self.last_stop_index = stop_waypoint_index # Compute linear acceleration/slowdown current_speed = self.get_waypoint_velocity( self.waypoints[self.current_waypoint_index]) if not should_brake and current_speed >= MAX_SPEED: # Ignore, no red lights and speed is already high rospy.logwarn("Ignore Light Speed") return # Step slow down or acceleration linear_acceleration = current_speed / steps if not should_brake: linear_acceleration = MAX_SPEED / steps distance_divider = (distance_full / steps) # Update waypoints for i in range(self.current_waypoint_index, stop_waypoint_index + 1): # Compute partial speed until stop velocity = 0 short_distance = self.distance(self.waypoints, self.current_waypoint_index, i) multi = int(short_distance / distance_divider) diff_velocity = float(multi + 1) * linear_acceleration if should_brake: # Braking velocity = current_speed - diff_velocity if velocity < 0: velocity = 0 else: # Accelerating velocity = current_speed + diff_velocity if velocity > MAX_SPEED: velocity = MAX_SPEED rospy.logwarn("Set speed {} {} {} {} {}".format( i, velocity, multi, linear_acceleration, short_distance)) self.set_waypoint_velocity(self.waypoints, i, velocity) # Force to Publish new waypoints self.waypoints_changed = True
def __init__(self): rospy.init_node('tl_detector') self.i = 0 #stop line self.traffic_line_index = [] config_string = rospy.get_param("/traffic_light_config") self.config = yaml.load(config_string) #traffic light self.lights = [] self.traffic_light_index = [] self.signal_classes = ['STOP', 'STOP', 'GO', 'Unknown', 'Unknown'] sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_cb) #position self.position = None sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb) #waypoints self.waypoints = None sub2 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb) #camera_image self.camera_image = None self.has_image = False sub6 = rospy.Subscriber('/image_color', Image, self.image_cb) self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1) self.car_position = rospy.Publisher('/car_position', Int32, queue_size=1) ''' /vehicle/traffic_lights provides you with the location of the traffic light in 3D map space and helps you acquire an accurate ground truth data source for the traffic light classifier by sending the current color state of all traffic lights in the simulator. When testing on the vehicle, the color state will not be available. You'll need to rely on the position of the light and the camera image to predict it. ''' self.bridge = CvBridge() self.listener = tf.TransformListener() self.state = TrafficLight.UNKNOWN self.last_state = TrafficLight.UNKNOWN self.last_wp = -1 self.state_count = 0 self.i = 0 #load classifier,time comsuming self.light_classifier = TLClassifier() self.color_Classifier = ColorClassifier() rospy.logwarn('You can launch simulator now!') #set spin rate according to time required for classification rate = rospy.Rate(5) start_time = 0 while not start_time: start_time = rospy.Time.now().to_sec() while not rospy.is_shutdown(): self.detect_tl() rate.sleep()
def execute(self): if self.needs_reset: rospy.logwarn('Success Decorator needs already returned '+ self.get_status()) return self.get_status() if not self.finished: if not self.started: if self.timeout == -1: # dont start timer, wait forever self.started = True else: self.timer = rospy.Timer(rospy.Duration(1.0*self.timeout), self.timed_out, oneshot=True) self.started = True self.child_status_ = self.children_[0].execute() if self.child_status_[:7] == 'SUCCESS': rospy.logwarn('WAIT SUCCESS DECORATOR ['+self.name_+']: REPORTED SUCCESS') self.needs_reset = True return self.set_status('SUCCESS') elif self.child_status_ == 'RUNNING': rospy.logwarn('WAIT SUCCESS DECORATOR ['+self.name_+']: CHILD RUNNING, WAITING') return self.set_status('RUNNING') else: rospy.logwarn('WAIT SUCCESS DECORATOR ['+self.name_+']: CHILD FAILED, RESET, WAITING') self.children_[0].reset() return self.set_status('RUNNING') else: # started self.child_status_ = self.children_[0].execute() if self.child_status_[:7] == 'SUCCESS': rospy.logwarn('WAIT SUCCESS DECORATOR ['+self.name_+']: REPORTED SUCCESS') self.needs_reset = True return self.set_status('SUCCESS') elif self.child_status_ == 'RUNNING': rospy.logwarn('WAIT SUCCESS DECORATOR ['+self.name_+']: CHILD RUNNING, WAITING') return self.set_status('RUNNING') else: rospy.logwarn('WAIT SUCCESS DECORATOR ['+self.name_+']: CHILD FAILED, RESET, WAITING') self.children_[0].reset() return self.set_status('RUNNING') else: # finished rospy.logwarn('Success Decorator timed out') return self.set_status('FAILURE')
def __init__(self, default_port='/dev/ttyUSB1'): """ @param default_port: default serial port to use for establishing a connection to the UM7 IMU sensor. This will be overridden by ~port param if available. """ rospy.init_node('imu_um7') self.port = rospy.get_param('~port', default_port) self.frame_id = rospy.get_param('~frame_id', "/imu") self.gps_frame_id = rospy.get_param('~gps_frame_id', "/gps") self.throttle_rate = rospy.get_param('~throttle_rate', 10000) self.reset_mag = rospy.get_param('~reset_mag', False) self.reset_accel = rospy.get_param('~reset_accel', False) self.mag_zero_x = rospy.get_param('~mag_zero_x', False) self.mag_zero_y = rospy.get_param('~mag_zero_y', False) self.mag_zero_z = rospy.get_param('~mag_zero_z', False) rospy.loginfo("serial port: %s" % (self.port)) self.link = rospy.get_param('~link', 'imu_link') rospy.loginfo("tf link: {}".format(self.link)) self.imu_data = Imu() self.imu_data = Imu(header=rospy.Header(frame_id=self.link)) # These covariance calculations are based on those bound in the existing # UM7 ROS package at: # https://github.com/ros-drivers/um7 linear_acceleration_stdev = rospy.get_param( "~linear_acceleration_stdev", 4.0 * 1e-3 * 9.80665) angular_velocity_stdev = rospy.get_param("~angular_velocity_stdev", np.deg2rad(0.06)) linear_acceleration_cov = linear_acceleration_stdev * linear_acceleration_stdev angular_velocity_cov = angular_velocity_stdev * angular_velocity_stdev # From the UM7 datasheet for the dynamic accuracy from the EKF. orientation_x_stdev = rospy.get_param("~orientation_x_stdev", np.deg2rad(3.0)) orientation_y_stdev = rospy.get_param("~orientation_y_stdev", np.deg2rad(3.0)) orientation_z_stdev = rospy.get_param("~orientation_z_stdev", np.deg2rad(5.0)) orientation_x_covar = orientation_x_stdev * orientation_x_stdev orientation_y_covar = orientation_y_stdev * orientation_y_stdev orientation_z_covar = orientation_z_stdev * orientation_z_stdev self.imu_data.orientation_covariance = [ orientation_x_covar, 0, 0, 0, orientation_y_covar, 0, 0, 0, orientation_z_covar ] self.imu_data.angular_velocity_covariance = [ angular_velocity_cov, 0, 0, 0, angular_velocity_cov, 0, 0, 0, angular_velocity_cov ] self.imu_data.linear_acceleration_covariance = [ linear_acceleration_cov, 0, 0, 0, linear_acceleration_cov, 0, 0, 0, linear_acceleration_cov ] # Set up the imu data message self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=1) # Set up the Roll-Pitch-Yaw (rpy) message self.rpy_data = Vector3Stamped() self.rpy_pub = rospy.Publisher('imu/rpy', Vector3Stamped, queue_size=1) # Set up the magnometer message self.mag_data = Vector3Stamped() self.mag_pub = rospy.Publisher('imu/mag', Vector3Stamped, queue_size=1) # what data to get from the UM7 self.statevars = [ 'health', 'roll', 'pitch', 'yaw', 'mag_proc_x', 'mag_proc_y', 'mag_proc_z', 'accel_proc_x', 'accel_proc_y', 'accel_proc_z', 'gyro_proc_x', 'gyro_proc_y', 'gyro_proc_z', 'quat_a', 'quat_b', 'quat_c', 'quat_d', 'gps_latitude', 'gps_longitude', 'gps_altitude' ] # Masks for parsing out the health data from the IMU self.NUM_SATS_USED = 0b11111100000000000000000000000000 self.HDOP = 0b00000011111111110000000000000000 # Not used as of 05/23/19 self.NUM_SATS_IN_VIEW = 0b00000000000000001111110000000000 # Set up the GPS NavSatFix publisher self.fix_data = NavSatFix() self.fix_pub = rospy.Publisher('fix', NavSatFix, queue_size=1) imu_connected = False while not rospy.is_shutdown() and not imu_connected: try: self.driver = um7.UM7('s', self.port, self.statevars, baud=115200) imu_connected = True rospy.loginfo("Imu initialization completed") self.received = -1 if self.reset_mag: self.driver.set_mag_reference() if self.reset_accel: cmd_seq.append(Um6Drv.CMD_SET_ACCEL_REF) if self.mag_zero_x and self.mag_zero_y and self.mag_zero_z: rospy.loginfo("Magnetometer calibration: %.3f %.3f %.3f", self.mag_zero_x, self.mag_zero_y, self.mag_zero_z) except SerialException: rospy.logwarn( "Serial error communicating with IMU. Will retry in 2.0 seconds." ) rospy.sleep(2.0)
# TODO: make action on the gripper as categorical policy # action[-1] = reloc_rescale_gripper(action[-1]) next_obs, manager_reward, done = env.step_demo( action, time_step=ep_len ) # reward R_t-> for high-level manager -> for sum(R_t:t+c-1) randomize_world() # update episodic logs # Ignore the "done" signal if it comes from hitting the time # horizon (that is, when it's an artificial terminal signal # that isn't based on the agent's state) -> if done = False for the max_timestep # DO NOT Make done = True when it hits timeout ep_ret += manager_reward # reward in terms of achieving episodic task done = False if ep_len == max_ep_len else done if done: rospy.logwarn( '=============== Now epsiode %d ends with done signal! ====================', episode_num) next_meas_stt = np.concatenate(next_obs['observation']['meas_state'], axis=0) # s_t next_c_obs = next_obs['observation']['color_obs'] #o_t next_aux_stt = np.concatenate(next_obs['observation']['auxiliary'], axis=0) # s_aux next_full_stt = np.concatenate([next_meas_stt, next_aux_stt], axis=0) # append manager transition manager_temp_transition[-1] = done # TODO verfiy manager_temp_transition[-2] += manager_reward # sum(R_t:t+c) manager_temp_transition[0].append(next_meas_stt) # append s_seq manager_temp_transition[1].append(next_aux_stt) # append o_seq manager_temp_transition[2].append(next_c_obs) # append a_seq manager_temp_transition[3].append(action) # append a_seq
def gather_and_publish(self): """ Method that runs while ROS is up. It fetches, then processes the data from the IMU. It only publishes the data when there are subscribers """ while not rospy.is_shutdown(): now = rospy.Time.now() if (now.to_sec() - self.imu_data.header.stamp.to_sec() ) * self.throttle_rate < 1.0: # Ignore data at this rate (ok for a boat) return else: self.driver.catchallsamples(self.statevars, 0.1) self.imu_data.header.stamp = now # If someone is subscribed to these messages, then process the data and # publish it. This avoids spending CPU cycles to process data no other # nodes need if self.imu_pub.get_num_connections() != 0: self.imu_data.orientation = Quaternion() # IMU outputs [w,x,y,z] NED, convert to [w, x, -y, -z] ENU in world frame self.imu_data.orientation.w = self.driver.state['quat_a'] self.imu_data.orientation.x = self.driver.state['quat_b'] self.imu_data.orientation.y = -self.driver.state['quat_c'] self.imu_data.orientation.z = -self.driver.state['quat_d'] # convert to radians from degrees and scale according to values # in the UM7 datasheet # again note NED to ENU conversion self.imu_data.angular_velocity.x = np.deg2rad( self.driver.state['roll_rate'] / 16) self.imu_data.angular_velocity.y = np.deg2rad( -self.driver.state['pitch_rate'] / 16) self.imu_data.angular_velocity.z = np.deg2rad( -self.driver.state['yaw_rate'] / 16) # again note NED to ENU conversion self.imu_data.linear_acceleration.x = self.driver.state[ 'accel_proc_x'] * GRAVITY # data['DATA_LINEAR_ACCEL'][1] self.imu_data.linear_acceleration.y = -self.driver.state[ 'accel_proc_y'] * GRAVITY # data['DATA_LINEAR_ACCEL'][0] self.imu_data.linear_acceleration.z = -self.driver.state[ 'accel_proc_z'] * GRAVITY # -(data['DATA_LINEAR_ACCEL'][2]) self.imu_pub.publish(self.imu_data) # If someone is subscribed to these messages, then process the data and # publish it. This avoids spending CPU cycles to process data no other # nodes need if self.rpy_pub.get_num_connections() != 0: self.rpy_data.header = self.imu_data.header self.rpy_data.vector.x = np.deg2rad(self.driver.state['roll']) self.rpy_data.vector.y = np.deg2rad( -self.driver.state['pitch']) self.rpy_data.vector.z = np.deg2rad(-self.driver.state['yaw']) self.rpy_pub.publish(self.rpy_data) # If someone is subscribed to these messages, then process the data and # publish it. This avoids spending CPU cycles to process data no other # nodes need if self.mag_pub.get_num_connections() != 0: self.mag_data.header = self.imu_data.header self.mag_data.vector.x = self.driver.state['mag_proc_y'] self.mag_data.vector.y = self.driver.state['mag_proc_x'] self.mag_data.vector.z = -self.driver.state['mag_proc_z'] self.mag_pub.publish(self.mag_data) # Now, gather and process the GPS data # If someone is subscribed to these messages, then process the data and # publish it. This avoids spending CPU cycles to process data no other # nodes need if self.fix_pub.get_num_connections() != 0: self.fix_data.header.stamp = rospy.get_rostime() self.fix_data.header.frame_id = self.gps_frame_id self.fix_data.status.service = NavSatStatus.SERVICE_GPS # Mask the returned health register to determine the number of # satellites in use. self.number_of_sats_used = ( self.NUM_SATS_USED & self.driver.state['health']) >> 26 self.number_of_sats_inView = ( self.NUM_SATS_IN_VIEW & self.driver.state['health']) >> 10 # GPS needs at least 4 satellites to have a reliable fix # So, if we don't have 4, we publish that we don't have a fix if self.number_of_sats_used < 4: self.fix_data.status.status = NavSatStatus.STATUS_NO_FIX rospy.logwarn("No GPS fix.") else: self.fix_data.status.status = NavSatStatus.STATUS_FIX # Publish the GPS data # TODO: 05/23/19 - JEV - Should be publish NaN or 0 if we don't # have a fix? self.fix_data.latitude = self.driver.state['gps_latitude'] self.fix_data.longitude = self.driver.state['gps_longitude'] self.fix_data.altitude = self.driver.state['gps_altitude'] self.fix_data.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(self.fix_data)
def spin(self): # state s = self.sensor_state odom = Odometry(header=rospy.Header(frame_id=self.odom_frame), child_frame_id=self.base_frame) js = JointState(name=[ "left_wheel_joint", "right_wheel_joint", "front_castor_joint", "back_castor_joint" ], position=[0, 0, 0, 0], velocity=[0, 0, 0, 0], effort=[0, 0, 0, 0]) r = rospy.Rate(self.update_rate) last_cmd_vel = 0, 0 last_cmd_vel_time = rospy.get_rostime() last_js_time = rospy.Time(0) # We set the retry count to 0 initially to make sure that only # if we received at least one sensor package, we are robust # agains a few sensor read failures. For some strange reason, # sensor read failures can occur when switching to full mode # on the Roomba. sensor_read_retry_count = 0 while not rospy.is_shutdown(): last_time = s.header.stamp curr_time = rospy.get_rostime() # SENSE/COMPUTE STATE try: self.sense(s) transform = self.compute_odom(s, last_time, odom) # Future-date the joint states so that we don't have # to publish as frequently. js.header.stamp = curr_time + rospy.Duration(1) except select.error: # packet read can get interrupted, restart loop to # check for exit conditions continue except DriverError: if sensor_read_retry_count > 0: rospy.logwarn( 'Failed to read sensor package. %d retries left.' % sensor_read_retry_count) sensor_read_retry_count -= 1 continue else: raise sensor_read_retry_count = self._SENSOR_READ_RETRY_COUNT # Reboot Create if we detect that charging is necessary. if s.charging_sources_available > 0 and \ s.oi_mode == 1 and \ s.charging_state in [0, 5] and \ s.charge < 0.93*s.capacity: rospy.loginfo("going into soft-reboot and exiting driver") self._robot_reboot() rospy.loginfo("exiting driver") break # Reboot Create if we detect that battery is at critical level switch to passive mode. if s.charging_sources_available > 0 and \ s.oi_mode == 3 and \ s.charging_state in [0, 5] and \ s.charge < 0.15*s.capacity: rospy.loginfo("going into soft-reboot and exiting driver") self._robot_reboot() rospy.loginfo("exiting driver") break # PUBLISH STATE self.sensor_state_pub.publish(s) self.odom_pub.publish(odom) if self.publish_tf: self.publish_odometry_transform(odom) # 1hz, future-dated joint state if curr_time > last_js_time + rospy.Duration(1): self.joint_states_pub.publish(js) last_js_time = curr_time self._diagnostics.publish(s, self._gyro) if self._gyro: self._gyro.publish(s, last_time) # ACT if self.req_cmd_vel is not None: # check for velocity command and set the robot into full mode if not plugged in if s.oi_mode != self.operate_mode and s.charging_sources_available != 1: if self.operate_mode == 2: self._robot_run_safe() else: self._robot_run_full() # check for bumper contact and limit drive command req_cmd_vel = self.check_bumpers(s, self.req_cmd_vel) # Set to None so we know it's a new command self.req_cmd_vel = None # reset time for timeout last_cmd_vel_time = last_time else: #zero commands on timeout if last_time - last_cmd_vel_time > self.cmd_vel_timeout: last_cmd_vel = 0, 0 # double check bumpers req_cmd_vel = self.check_bumpers(s, last_cmd_vel) # send command self.drive_cmd(*req_cmd_vel) # record command last_cmd_vel = req_cmd_vel r.sleep()