def __init__(self): ButtonServer.global_server = self self.handler = MyRequestHandler rospy.init_node('button_server') rospy.on_shutdown(self.on_shutdown) self.pub = rospy.Publisher("buttons",std_msgs.msg.String,queue_size=1) self.port = rospy.get_param("~port",5180) self.blist = [] rospy.loginfo("Started Button Server on port %d" % self.port) i=0 while True: bname = rospy.get_param("~button%d"%i,"") btext = rospy.get_param("~button%d_text"%i,bname) bcolor = rospy.get_param("~button%d_color"%i,"") bstyle = rospy.get_param("~button%d_style"%i,"") if bname != "": self.blist.append(Button(bname,btext,bcolor,bstyle)) else: break i = i+1 if len(self.blist)==0: self.blist = [Button("test","Debug Button","lightgreen")] self.buildPage() self.root = roslib.packages.get_pkg_dir('button_server') os.chdir(self.root) self.httpd = MyServer(("", self.port), self.handler)
def main(name): try: from python_qt_binding.QtGui import QApplication except: print >> sys.stderr, "please install 'python_qt_binding' package!!" sys.exit(-1) masteruri = init_cfg_path() parser = init_arg_parser() args = rospy.myargv(argv=sys.argv) parsed_args = parser.parse_args(args[1:]) # Initialize Qt global app app = QApplication(sys.argv) # decide to show main or echo dialog global main_form if parsed_args.echo: main_form = init_echo_dialog(name, masteruri, parsed_args.echo[0], parsed_args.echo[1], parsed_args.hz) else: main_form = init_main_window(name, masteruri, parsed_args.file) # resize and show the qt window if not rospy.is_shutdown(): os.chdir(PACKAGE_DIR) # change path to be able to the images of descriptions main_form.resize(1024, 720) screen_size = QApplication.desktop().availableGeometry() if main_form.size().width() >= screen_size.width() or main_form.size().height() >= screen_size.height()-24: main_form.showMaximized() else: main_form.show() exit_code = -1 rospy.on_shutdown(finish) exit_code = app.exec_()
def __init__(self): rospy.init_node('Gocrazy', anonymous=False) self.distance = 0 self.angle = 0 rospy.loginfo("To stop TurtleBot CTRL + C") # What function to call when you ctrl + c rospy.on_shutdown(self.shutdown) # Create a publisher which can "talk" to TurtleBot and tell it to move # Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if # you're not using TurtleBot2 self.cmd_vel = rospy.Publisher( 'cmd_vel_mux/input/navi', Twist, queue_size=10) # TurtleBot will stop if we don't keep telling it to move. How often # should we tell it to move? 10 HZ r = rospy.Rate(10) move_cmd = Twist() move_cmd.linear.x = self.distance move_cmd.angular.y = self.angle while not rospy.is_shutdown(): # publish the velocity self.cmd_vel.publish(move_cmd) # wait for 0.1 seconds (10 HZ) and publish again r.sleep()
def __init__(self): self.name = rospy.get_name()[1:] self.action_list = ActionList(self.name) self.action_list.update() self.collection_list = CollectionList(self.name) self.collection_list.update() self.action_sequence_queue = [] self.action = None # Reference to the action client or preview publisher. self.execute_steps_relay = None step_action_server_topic = rospy.get_param('/free_gait/action_server') self.execute_steps_client = actionlib.SimpleActionClient(step_action_server_topic, free_gait_msgs.msg.ExecuteStepsAction) self.execute_action_server = actionlib.SimpleActionServer("~execute_action", free_gait_msgs.msg.ExecuteActionAction, \ execute_cb=self._execute_action_callback, auto_start = False) self.execute_action_server.register_preempt_callback(self.preempt) self.execute_action_server.start() step_preview_topic = rospy.get_param('/free_gait/preview_topic') self.preview_publisher = rospy.Publisher(step_preview_topic, free_gait_msgs.msg.ExecuteStepsActionGoal, queue_size=1) rospy.Service('~update', std_srvs.srv.Trigger, self.update) rospy.Service('~list_actions', free_gait_msgs.srv.GetActions, self.list_actions) rospy.Service('~list_collections', free_gait_msgs.srv.GetCollections, self.list_collections) rospy.Service('~send_action', free_gait_msgs.srv.SendAction, self._send_action_callback) rospy.Service('~preview_action', free_gait_msgs.srv.SendAction, self._preview_action_callback) rospy.Service('~send_action_sequence', free_gait_msgs.srv.SendActionSequence, self._send_action_sequence_callback) rospy.on_shutdown(self.preempt)
def __init__(self, name) : rospy.on_shutdown(self._on_node_shutdown) self.current_node = "Unknown" self.nogo_pre_active = False self.nogo_active = False self.nogos=[] self.stop_services=["/enable_motors", "/emergency_stop", "/task_executor/set_execution_status"] self.activate_services=["/enable_motors", "/reset_motorstop", "/task_executor/set_execution_status"] #Waiting for Topological Map self._map_received=False rospy.Subscriber('/topological_map', TopologicalMap, self.MapCallback) rospy.loginfo("Waiting for Topological map ...") while not self._map_received : pass rospy.loginfo(" ...done") self.update_service_list() print self.av_stop_services, self.av_activate_services #Subscribing to Localisation Topics rospy.loginfo("Subscribing to Localisation Topics") rospy.Subscriber('/current_node', String, self.currentNodeCallback) rospy.loginfo(" ...done") self.nogos = self.get_no_go_nodes() print self.nogos self._killall_timers=False t = Timer(1.0, self.timer_callback) t.start() rospy.loginfo("All Done ...") rospy.spin()
def __init__(self): """ JARVIS User Interface Node """ rospy.on_shutdown(self.cleanup) rospy.wait_for_service('return_grips') self.msg = GoalID() self.newGrips = jarvis_perception.msg.GraspArray() self.legalUtterances = ['upper','lower','top','bottom','back','front','near','far','rear','left','right','anywhere','center'] self.lastUtterance = '' self.axisAlignedBox = '' self.person_trans = '' self.person_rot = '' plotting = True self.likelihood = None self.var = None try: grip_server = rospy.ServiceProxy('return_grips',ReturnGrips) self.grips = grip_server() print self.grips pubgrips = rospy.Publisher('return_grips', jarvis_perception.msg.GraspArray, queue_size=10) except rospy.ServiceException, e: print "Service call failed: %s"%e
def __init__(self): # set up openrave self.env = rave.Environment() self.env.StopSimulation() self.env.Load("robots/pr2-beta-static.zae") # todo: use up-to-date urdf self.robot = self.env.GetRobots()[0] self.joint_listener = TopicListener("/joint_states", sm.JointState) # rave to ros conversions joint_msg = self.get_last_joint_message() ros_names = joint_msg.name inds_ros2rave = np.array([self.robot.GetJointIndex(name) for name in ros_names]) self.good_ros_inds = np.flatnonzero(inds_ros2rave != -1) # ros joints inds with matching rave joint self.rave_inds = inds_ros2rave[self.good_ros_inds] # openrave indices corresponding to those joints self.update_rave() self.larm = Arm(self, "l") self.rarm = Arm(self, "r") self.lgrip = Gripper(self, "l") self.rgrip = Gripper(self, "r") self.head = Head(self) self.torso = Torso(self) self.base = Base(self) rospy.on_shutdown(self.stop_all)
def __init__(self): #start node rospy.init_node("map_saver") rospy.on_shutdown(self.die) #get parameters from server self.save_dir = rospy.get_param("map_save_dir","map_log/") #default to .ros/map_log update_rate = rospy.get_param("map_save_update_rate",120) #update once every 2 min #create directory and set clean_open self.clean_open=self.create_dir() #create dictionaries self.edges={} #dictionary of edge_id:edge_obj self.nodes={} #dictionary of node_id:node_obj #begin listening rospy.Subscriber("/node",GVGNode, self.node_handler) rospy.Subscriber("/edge",GVGEdgeMsg, self.edge_handler) self.updated=False #while alive and able to open the file self.alive=True while(self.clean_open and self.alive): if(self.updated): self.dump(str(rospy.get_time())) self.updated=False rospy.sleep(update_rate)
def __init__(self): ''' Creates a new instance. Find the topic of the master_discovery node using U{master_discovery_fkie.interface_finder.get_changes_topic() <http://docs.ros.org/api/master_discovery_fkie/html/modules.html#interface-finder-module>}. Also the parameter C{~ignore_hosts} will be analyzed to exclude hosts from sync. ''' self.masters = {} # the connection to the local service master self.materuri = masteruri_from_master() '''@ivar: the ROS master URI of the C{local} ROS master. ''' self.__lock = threading.RLock() # load interface self._load_interface() # subscribe to changes notifier topics topic_names = interface_finder.get_changes_topic(masteruri_from_master()) self.sub_changes = dict() '''@ivar: `dict` with topics {name: U{rospy.Subscriber<http://docs.ros.org/api/rospy/html/rospy.topics.Subscriber-class.html>}} publishes the changes of the discovered ROS masters.''' for topic_name in topic_names: rospy.loginfo("listen for updates on %s", topic_name) self.sub_changes[topic_name] = rospy.Subscriber(topic_name, MasterState, self._rosmsg_callback_master_state) self.__timestamp_local = None self.__own_state = None self.update_timer = None self.own_state_getter = None self._join_threads = dict() # threads waiting for stopping the sync thread # initialize the ROS services rospy.Service('~get_sync_info', GetSyncInfo, self._rosservice_get_sync_info) rospy.on_shutdown(self.finish) self.obtain_masters()
def _test_smach_execute2(): rospy.init_node('smach') rospy.on_shutdown(myhook) sq = Sequence( outcomes = ['succeeded','aborted','preempted'], connector_outcome = 'succeeded') with sq: Sequence.add('CHECK', CheckSmachEnabledState(), transitions={'aborted':'SLEEP'}) Sequence.add('SLEEP', SleepState(10)) rospy.loginfo("Executing test...") TransformListenerSingleton.get() #outcome = sq.execute() #rospy.loginfo("outcome: %s" % outcome) # Create and start the introspection server sis = smach_ros.IntrospectionServer('server_name', sq, '/SM_ROOT') sis.start() # Execute the state machine print "EXECUTING.." outcome = sq.execute() # Wait for ctrl-c to stop the application print "SPINNING.ms." rospy.spin() #rospy.signal_shutdown("shutting down now") sis.stop()
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 __init__(self): rospy.init_node('nav_test', anonymous=False) #what to do if shut down (e.g. ctrl + C or failure) rospy.on_shutdown(self.shutdown) #tell the action client that we want to spin a thread by default self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("wait for the action server to come up") #allow up to 5 seconds for the action server to come up self.move_base.wait_for_server(rospy.Duration(5)) #we'll send a goal to the robot to move 3 meters forward goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'base_link' goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = 3.0 #3 meters goal.target_pose.pose.orientation.w = 1.0 #go forward #start moving self.move_base.send_goal(goal) #allow TurtleBot up to 60 seconds to complete task success = self.move_base.wait_for_result(rospy.Duration(60)) if not success: self.move_base.cancel_goal() rospy.loginfo("The base failed to move forward 3 meters for some reason") else: # We made it! state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Hooray, the base moved 3 meters forward")
def main(): rospy.init_node("baxter_velocity_control") # # FK Position # print '\n*** Baxter Position FK ***\n' # print kin.forward_position_kinematics() # camera = Camera() wobbler = Wobbler() # for i in range(300): # print i # J = kin.jacobian_pseudo_inverse() # A = np.matrix(J) # twist = np.array([[.0],[.0],[.0],[.0],[.0],[.0]]) # q_dot = A*twist # q_dot_list = [i[0] for i in q_dot.tolist()] # # print q_dot_list # wobbler.q_dot = q_dot_list # wobbler.wobble() wobbler.q_dot = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] wobbler.wobble() # rospy.spin() rospy.on_shutdown(wobbler.clean_shutdown) print ("Done.")
def __init__(self): rospy.on_shutdown(self.cleanup) self.voice = rospy.get_param("~voice", "voice_kal_diphone")#voice_don_diphone, voice_kal_diphone self.wavepath = rospy.get_param("~wavepath", "") self.command_to_phrase = COMMAND_TO_PHRASE # this is deffined in wheelchairint/keywords_tocommand.py # Create the sound client object self.soundhandle = SoundClient() # Announce that we are ready for input rospy.sleep(2) self.soundhandle.stopAll() rospy.sleep(1) self.soundhandle.playWave(self.wavepath + "/R2D2.wav") rospy.sleep(2) self.soundhandle.say("I am ready", self.voice) rospy.sleep(1.5) self.soundhandle.say("Give me an order", self.voice) rospy.sleep(2) rospy.loginfo("Say one a commands...") # Subscribe to the recognizer output rospy.Subscriber('recognizer/output', String, self.rec_out_callback) r = rospy.Rate(0.5) while not rospy.is_shutdown(): # rospy.loginfo("wheelchair talk is running correctly.") r.sleep()
def __init__(self, name): self.name = name self.getConfig() self.mission_status = MissionStatus() # Init internal state self.init_trajectory = False self.init_keep_pose = False self.init_goto = False # Create "absolute" client self.client_absolute = actionlib.SimpleActionClient('absolute_movement', WorldWaypointReqAction) self.client_absolute.wait_for_server() # Create publisher self.pub_mission_status = rospy.Publisher("/safety_g500/mission_status", MissionStatus) rospy.Timer(rospy.Duration(1.0), self.pubMissionStatus) # Create Subscriber rospy.Subscriber("/navigation_g500/nav_sts", NavSts, self.updateNavSts) self.nav = NavSts() # Create services self.enable_trajectory = rospy.Service('/control_g500/enable_trajectory', Empty, self.enableTrajectory) self.disable_trajectory = rospy.Service('/control_g500/disable_trajectory', Empty, self.disableTrajectory) self.enable_keep_position = rospy.Service('/control_g500/enable_keep_position', Empty, self.enableKeepPosition) self.disable_keep_position = rospy.Service('/control_g500/disable_keep_position', Empty, self.disableKeepPosition) self.goto = rospy.Service('/control_g500/goto', GotoSrv, self.goto) # Map shutdown function rospy.on_shutdown(self.stop)
def __init__(self): rospy.init_node('monitor_fake_battery', anonymous=False) rospy.on_shutdown(self.shutdown) # Set the low battery threshold (between 0 and 100) self.low_battery_threshold = rospy.get_param('~low_battery_threshold', 50) # Initialize the state machine sm_battery_monitor = StateMachine(outcomes=[]) with sm_battery_monitor: # Add a MonitorState to subscribe to the battery level topic StateMachine.add('MONITOR_BATTERY', MonitorState('battery_level', Float32, self.battery_cb), transitions={'invalid':'RECHARGE_BATTERY', 'valid':'MONITOR_BATTERY', 'preempted':'MONITOR_BATTERY'},) # Add a ServiceState to simulate a recharge using the set_battery_level service StateMachine.add('RECHARGE_BATTERY', ServiceState('battery_simulator/set_battery_level', SetBatteryLevel, request=100), transitions={'succeeded':'MONITOR_BATTERY', 'aborted':'MONITOR_BATTERY', 'preempted':'MONITOR_BATTERY'}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('monitor_battery', sm_battery_monitor, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = sm_battery_monitor.execute() intro_server.stop()
def __init__(self): # initiliaze rospy.init_node('GoForward', anonymous=False) # tell user how to stop TurtleBot rospy.loginfo("To stop TurtleBot CTRL + C") # What function to call when you ctrl + c rospy.on_shutdown(self.shutdown) # Create a publisher which can "talk" to TurtleBot and tell it to move # Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if you're not using TurtleBot2 self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) #TurtleBot will stop if we don't keep telling it to move. How often should we tell it to move? 10 HZ r = rospy.Rate(10); # Twist is a datatype for velocity move_cmd = Twist() # let's go forward at 0.2 m/s move_cmd.linear.x = 0.3 # let's turn at 0 radians/s move_cmd.angular.z = 0 # as long as you haven't ctrl + c keeping doing... while not rospy.is_shutdown(): # publish the velocity self.cmd_vel.publish(move_cmd) # wait for 0.1 seconds (10 HZ) and publish again r.sleep()
def __init__(self): rospy.init_node("baxter_hydra_teleop") self.status_display = baxter_faces.FaceImage() rospy.loginfo("Getting robot state... ") self.rs = baxter_interface.RobotEnable() self.hydra_msg = Hydra() self.hydra_msg_lock = threading.Lock() self.gripper_left = baxter_interface.Gripper("left") self.gripper_right = baxter_interface.Gripper("right") self.mover_left = LimbMover("left") self.mover_right = LimbMover("right") self.mover_head = HeadMover() self.happy_count = 0 # Need inertia on how long unhappy is displayed self.hydra_msg = Hydra() rospy.on_shutdown(self._cleanup) rospy.Subscriber("/hydra_calib", Hydra, self._hydra_cb) rospy.Timer(rospy.Duration(1.0 / 30), self._main_loop) rospy.loginfo( "Press left or right button on Hydra to start the teleop") while not self.rs.state().enabled and not rospy.is_shutdown(): rospy.Rate(10).sleep() self.mover_left.enable() self.mover_right.enable() self.mover_head.set_pose()
def __init__(self): self.node_name = "face_recog_eigen" rospy.init_node(self.node_name) rospy.on_shutdown(self.cleanup) self.bridge = CvBridge() self.face_names = StringArray() self.size = 4 face_haar = 'haarcascade_frontalface_default.xml' self.haar_cascade = cv2.CascadeClassifier(face_haar) self.face_dir = 'face_data_eigen' # self.model = cv2.createFisherFaceRecognizer() self.model = cv2.createEigenFaceRecognizer() (self.im_width, self.im_height) = (112, 92) rospy.loginfo("Loading data...") # self.fisher_train_data() self.load_trained_data() rospy.sleep(3) # self.img_sub = rospy.Subscriber("/asus/rgb/image_raw", Image, self.img_callback) self.img_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.img_callback) # self.img_pub = rospy.Publisher('face_img', Image, queue_size=10) self.name_pub = rospy.Publisher('face_names', StringArray, queue_size=10) self.all_names_pub = rospy.Publisher('all_face_names', StringArray, queue_size=10) rospy.loginfo("Detecting faces...")
def __init__(self): self.node_name = rospy.get_name() self.br = tf.TransformBroadcaster() self.CPR = 70 # encoder pusle per round 2*75 self.radius = 0.0215 # radius of wheel in meter self.width = 0.22 # width of robot in meter self.x = 0 # robot position x in meter self.y = 0 # robot position y in meter self.theta = 0 # robot pose theta in radian self.encoder_pos_L = 0 # post left wheel encoder self.encoder_pos_R = 0 # post right wheel encoder self.encoder_pre_L = 0 # present left wheel encoder self.encoder_pre_R = 0 # present right wheel encoder self.R = 0 self.P = 0 # setup pi GPIO GPIO.setmode(GPIO.BCM) GPIO.setup(22, GPIO.IN) GPIO.setup(23, GPIO.IN) GPIO.setup(24, GPIO.IN) GPIO.setup(27, GPIO.IN) # interrupt callback functions GPIO.add_event_detect(23, GPIO.RISING, callback = self.Encoder_L) GPIO.add_event_detect(27, GPIO.RISING, callback = self.Encoder_R) # subscriber self.sub_RP = rospy.Subscriber("~orientationRP", Float64MultiArray, self.cbOrientation) rospy.on_shutdown(self.custom_shutdown) # shutdown method self.update_timer = rospy.Timer(rospy.Duration.from_sec(1), self.update) # timer for update robot pose, update rate = 1 Hz rospy.loginfo("[%s] Initialized " %self.node_name)
def __init__(self): rospy.loginfo("Starting valve task controller...") rospy.on_shutdown(self.cleanup) # Initialize the state machine self.running = False self.safed = False self.sm = smach.StateMachine(outcomes=['DONE', 'FAILED', 'SAFE'], input_keys=['sm_input'], output_keys=['sm_output']) # Populate the state machine from the modules with self.sm: self.safemode = SafeMode.SAFEMODE() # Add start state smach.StateMachine.add('StartTask', StartTask.STARTTASK(), transitions={'Start':'FindValve'}, remapping={'input':'sm_input', 'output':'sm_data'}) # Add finding step smach.StateMachine.add('FindValve', FindValve.FINDVALVE(), transitions={'Success':'PositionRobot', 'Failure':'ErrorHandler', 'Fatal':'SafeMode'}, remapping={'input':'sm_data', 'output':'sm_data'}) # Add positioning step smach.StateMachine.add('PositionRobot', PositionRobot.POSITIONROBOT(), transitions={'Success':'PlanTurning', 'Failure':'ErrorHandler', 'Fatal':'SafeMode'}, remapping={'input':'sm_data', 'output':'sm_data'}) # Add planning step smach.StateMachine.add('PlanTurning', PlanTurning.PLANTURNING(), transitions={'Success':'ExecuteTurning', 'Failure':'ErrorHandler', 'Fatal':'SafeMode'}, remapping={'input':'sm_data', 'output':'sm_data'}) # Add execution step smach.StateMachine.add('ExecuteTurning', ExecuteTurning.EXECUTETURNING(), transitions={'Success':'FinishTask', 'Failure':'ErrorHandler', 'Fatal':'SafeMode'}, remapping={'input':'sm_data', 'output':'sm_data'}) # Add finishing step smach.StateMachine.add('FinishTask', FinishTask.FINISHTASK(), transitions={'Success':'DONE', 'Failure':'ErrorHandler', 'Fatal':'SafeMode'}, remapping={'input':'sm_data', 'output':'sm_output'}) # Add manual mode smach.StateMachine.add('ManualMode', ManualMode.MANUALMODE(), transitions={'Success':'ErrorHandler', 'Failure':'ErrorHandler', 'Fatal':'SafeMode'}, remapping={'input':'sm_data', 'output':'sm_data'}) # Add safe mode smach.StateMachine.add('SafeMode', self.safemode, transitions={'Safed':'SAFE'}, remapping={'input':'sm_data', 'output':'sm_output'}) # Add Error handler smach.StateMachine.add('ErrorHandler', ErrorHandler.ERRORHANDLER(), transitions={'ReFind':'FindValve', 'RePosition':'PositionRobot', 'RePlan':'PlanTurning', 'ReExecute':'ExecuteTurning', 'ReFinish':'FinishTask', 'GoManual':'ManualMode', 'Failed':'FAILED', 'Fatal':'SafeMode'}, remapping={'input':'sm_data', 'output':'sm_output'}) # Set up the introspection server self.sis = smach_ros.IntrospectionServer('valve_task_smach_server', self.sm, '/SM_ROOT') self.sis.start()
def init_ros(): ''' ROS node initialization. Initialize the node, advertise topics, and any other ROS housekeeping. ''' global topics, param_handler rospy.init_node('sickldmrs', log_level=rospy.DEBUG) node_name = rospy.get_name().split('/')[-1] rospy.loginfo('node %s starting up.' % node_name) topics['cloud'] = rospy.Publisher('/%s/cloud' % node_name, numpy_msg(PointCloud2)) topics['scan0'] = rospy.Publisher('/%s/scan0' % node_name, numpy_msg(LaserScan)) topics['scan1'] = rospy.Publisher('/%s/scan1' % node_name, numpy_msg(LaserScan)) topics['scan2'] = rospy.Publisher('/%s/scan2' % node_name, numpy_msg(LaserScan)) topics['scan3'] = rospy.Publisher('/%s/scan3' % node_name, numpy_msg(LaserScan)) rospy.on_shutdown(node_shutdown_hook)
def __init__(self): rospy.on_shutdown(self.cleanup) self.speed = 0.1 self.buildmap = False self.follower = False self.navigation = False self.msg = Twist() # Create the sound client object self.soundhandle = SoundClient() rospy.sleep(1) self.soundhandle.stopAll() # Announce that we are ready for input rospy.sleep(1) self.soundhandle.say('Hi, my name is Petrois') rospy.sleep(3) self.soundhandle.say("Say one of the navigation commands") # publish to cmd_vel, subscribe to speech output self.pub_ = rospy.Publisher("cmd_vel", Twist, queue_size=2) rospy.Subscriber('recognizer/output', String, self.speechCb) r = rospy.Rate(10.0) while not rospy.is_shutdown(): self.pub_.publish(self.msg) r.sleep()
def main(): """Experiment module""" box_fit = BoxFit() camera = Webcam(box_fit.img_dir) rospy.on_shutdown(box_fit.clean_shutdown) box_fit.set_neutral() box_fit.take_reference_images(camera) box_fit.compress_object() box_fit.process_images() plotting = Plotting(box_fit.img_dir) rostopic_filename = plotting.directory + 'rostopic_data.txt' csv_filename = plotting.directory + 'sizes.csv' webcam_data_filename = plotting.directory + 'webcam_data.txt' timing_filename = plotting.directory + 'timestamps.txt' plotting.parseTimingFile(timing_filename) csvdict = plotting.parseCSV(csv_filename, webcam_data_filename) #print csvdict rostopicdict = plotting.parseRostopic(rostopic_filename) #print rostopicdict mergedict = plotting.mergeTiming(rostopicdict, csvdict) savefile = plotting.directory + 'merge.csv' plotting.saveAsCSV(savefile, mergedict)
def __init__(self): rospy.init_node("position_test") rospy.on_shutdown(self.shutdown) rate = rospy.get_param("~rate", 100) r = rospy.Rate(rate) self.head_pan_joint = rospy.get_param("~head_pan_joint", "head_pan_joint") self.joints = [self.head_pan_joint] self.default_joint_speed = rospy.get_param("~default_joint_speed", 0.2) self.joint_state = JointState() rospy.Subscriber("joint_states", JointState, self.update_joint_state) while not rospy.is_shutdown(): rospy.sleep(1) rospy.loginfo("waiting for joint_states") rospy.wait_for_message("joint_states", JointState) current_pan = self.joint_state.position[self.joint_state.name.index(self.head_pan_joint)] print "A" print current_pan print "B" print current_pan * 180 / math.pi
def Serve(self,scan_msg, pose_msg): print scan_msg.ranges # time.sleep(0.1) command2 = Twist() if self.succeeded: self.generateGoal(scan_msg, pose_msg) rospy.on_shutdown(self.shutdown) self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("wait for the action server to come up: ") self.move_base.wait_for_server(rospy.Duration(5)) goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'odom' goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose = Pose(Point(self.goal_x, self.goal_y, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000)) self.move_base.send_goal(goal) success = self.move_base.wait_for_result(rospy.Duration(20)) if not success: self.move_base.cancel_goal() rospy.loginfo("The base failed to reach the desired pose") self.succeeded =False else: # We made it! state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Hooray, reached the desired pose") self.succeeded =True
def __init__(self, script_path): rospy.init_node('talkback') rospy.on_shutdown(self.cleanup) # Set the default TTS voice to use self.voice = rospy.get_param("~voice", "voice_don_diphone") # Set the wave file path if used self.wavepath = rospy.get_param("~wavepath", script_path + "/../sounds") # Create the sound client object self.soundhandle = SoundClient() # Wait a moment to let the client connect to the # sound_play server rospy.sleep(1) # Make sure any lingering sound_play processes are stopped. self.soundhandle.stopAll() # Announce that we are ready for input self.soundhandle.playWave(self.wavepath + "/R2D2a.wav") rospy.sleep(1) self.soundhandle.say("Ready", self.voice) rospy.loginfo("Say one of the navigation commands...") # Subscribe to the recognizer output and set the callback function rospy.Subscriber('/recognizer/output', String, self.talkback)
def __init__(self): rospy.init_node('pilot', anonymous=True) self.pub_takeoff = rospy.Publisher( '/ardrone/takeoff', Empty, queue_size=1) self.pub_land = rospy.Publisher('/ardrone/land', Empty, queue_size=1) self.pub_reset = rospy.Publisher('/ardrone/reset', Empty, queue_size=1) # Gazebo Reset Simulator self.reset_world = rospy.ServiceProxy('/gazebo/reset_world', srv_Empty) rospy.Subscriber("/gazebo/model_states", ModelStates, self.update_navdata) # self.pub_cmd = rospy.Publisher('/cmd_vel',Twist, 1) self.pub_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1) self.pos_x = 0 self.pos_y = 0 self.roll = 0 self.pitch = 0 self.yaw = 0 self.target_yaw = 270 self.command = Twist() self.VelCommandTimer = rospy.Timer(rospy.Duration( COMMAND_PERIOD / 1000.0), self.send_cmd) rospy.on_shutdown(self.land)
def main(): """RSDK End Effector Position Example: Keyboard Control Use your dev machine's keyboard to control end effector positions. """ epilog = """ See help inside the example with the '?' key for key bindings. """ arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__, epilog=epilog) parser.parse_args(rospy.myargv()[1:]) print("Initializing node... ") rospy.init_node("rsdk_joint_position_keyboard") print("Getting robot state... ") rs = baxter_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled def clean_shutdown(): print("\nExiting example...") if not init_state: print("Disabling robot...") rs.disable() rospy.on_shutdown(clean_shutdown) print("Enabling robot... ") rs.enable() map_keyboard() print("Done.")
def __init__(self): self.x = 0 self.y = 0 self.z = 0 rospy.init_node("cmps09") # Opens up serial port self.port = rospy.get_param("~port", "") self.hz = rospy.get_param("~hz", 20) self.timeout = rospy.get_param("~timeout", 0.2) if self.port != "": rospy.loginfo("CMPS09 using port %s.", self.port) else: rospy.logerr("No port specified for CMPS09!") exit(1) try: self.transport = serial.Serial(port=self.port, baudrate=9600, timeout=self.timeout) self.transport.open() except serial.serialutil.SerialException as e: rospy.logerr("CMPS09: %s" % e) exit(1) # Registers shutdown handler to close the serial port we just opened. rospy.on_shutdown(self.shutdown_handler) # Registers as publisher self.pub = rospy.Publisher("raw_mag", Point)
# rospy.Rate(10) def myhook(): msg = drive_param() angle = 0. speed = -1.0 msg.angle = angle msg.velocity = speed pub.publish(msg) msg = drive_param() angle = 0. speed = 0. msg.angle = angle msg.velocity = speed pub.publish(msg) msg = drive_param() angle = 0. speed = 0. msg.angle = angle msg.velocity = speed pub.publish(msg) if __name__ == "__main__": rospy.init_node("pid_controller", anonymous=True) rospy.Subscriber("path_planner", Float64MultiArray, desired_track) rospy.Subscriber("slam_out_pose", PoseStamped, control) rospy.on_shutdown(myhook) rospy.spin()
derivative = erroradj - preverror if erroradj > 0: rot_speed = ((kp * erroradj) + (kd * derivative) + min_speed ) # turning right is positive value else: rot_speed = ((kp * erroradj) + (kd * derivative) - min_speed) preverror = erroradj #cual es tu mejor choice para definir error?(afect derivative) el erroradj or el true error #--------CONTROLLER ENDS HERE----------------------------------------- # BUILD ARRAY message speedmsg = Float32MultiArray() speedmsg.data = [lin_speed, rot_speed] pub_speeds.publish(speedmsg) rate.sleep() # INITIALIZE PUBLISHERS & SUBSCRIBERS sub_theta = rospy.Subscriber('theta_gyro', Float32MultiArray, Get_IMU_Data) # from IMU sub_theta_ref = rospy.Subscriber('ref_theta', Float64, Get_Reference) # from kbd or Xbox controller pub_speeds = rospy.Publisher('speed_commands', Float32MultiArray, queue_size=1) rospy.init_node('Controller_heading', anonymous=False) rate = rospy.Rate(100) # 100 Hz sampling/publishing frequency if __name__ == '__main__': rospy.on_shutdown(cleanupOnExit) control() rospy.spin()
def __init__(self): # Initialize rospy.init_node('MakeSquare', anonymous=False) # What to do you ctrl + c (call shutdown function written below) rospy.on_shutdown(self.shutdown)
def __init__(self): # Give the node a name rospy.init_node('out_and_back', anonymous=False) # Set rospy to execute a shutdown function when exiting rospy.on_shutdown(self.shutdown) # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=5) # How fast will we update the robot's movement? rate = 20 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Set the forward linear speed to 0.15 meters per second linear_speed = 0.15 # Set the travel distance in meters goal_distance = 1.0 # Set the rotation speed in radians per second angular_speed = 0.5 # Set the angular tolerance in degrees converted to radians angular_tolerance = radians(1.0) # Set the rotation angle to Pi radians (180 degrees) goal_angle = pi # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Set the odom frame self.odom_frame = '/odom' # Find out if the robot uses /base_link or /base_footprint try: self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_footprint' except (tf.Exception, tf.ConnectivityException, tf.LookupException): try: self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_link' except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") rospy.signal_shutdown("tf Exception") # Initialize the position variable as a Point type position = Point() # Loop once for each leg of the trip for i in range(2): # Initialize the movement command move_cmd = Twist() # Set the movement command to forward motion move_cmd.linear.x = linear_speed # Get the starting position values (position, rotation) = self.get_odom() x_start = position.x y_start = position.y # Keep track of the distance traveled distance = 0 # Enter the loop to move along a side while distance < goal_distance and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) r.sleep() # Get the current position (position, rotation) = self.get_odom() # Compute the Euclidean distance from the start distance = sqrt(pow((position.x - x_start), 2) + pow((position.y - y_start), 2)) # Stop the robot before the rotation move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Set the movement command to a rotation move_cmd.angular.z = angular_speed # Track the last angle measured last_angle = rotation # Track how far we have turned turn_angle = 0 while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) r.sleep() # Get the current rotation (position, rotation) = self.get_odom() # Compute the amount of rotation since the last loop delta_angle = normalize_angle(rotation - last_angle) # Add to the running total turn_angle += delta_angle last_angle = rotation # Stop the robot before the next leg move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Stop the robot for good self.cmd_vel.publish(Twist())
def Mission_strategy(): global action, count_stop, Arm_state_flag, Sent_data_flag, image_point, cntr_point, item_name, get_image_flag, suck_point, change_position global final_x, final_y, center_x, center_y, yolo_z, down_stop_flag, diff_item, picture_count, pressure_info, switch_flag #print('in top of mission updown ', [Arm_state_flag, Sent_data_flag,Arm_state_flag == Arm_status.Idle and Sent_data_flag == 1]) # print([Arm_state_flag, Sent_data_flag]) if Arm_state_flag == Arm_status.Idle and Sent_data_flag == 1: Sent_data_flag = 0 Arm_state_flag = Arm_status.Isbusy for case in switch(action): #傳送指令給socket選擇手臂動作 if case(0): #Arm to BOX above global is_metal, is_bottle, is_paper print("---Arm to BOX above---") is_metal = 0 is_bottle = 0 is_paper = 0 pos.x = 1.5 pos.y = 45.5 pos.z = star_config.yolo_z pos.pitch = -90 pos.roll = 0 pos.yaw = 0 action = 1 print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ', pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw) ArmTask.Speed_Mode(1) ArmTask.Arm_Mode(4, 1, 0, 30, 2) #speed up ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 100, 2) #action,ra,grip,vel,both break if case(1): #get_obj_coordinate & in postition print("---get item information & ready to catch---") time.sleep(0.75) switch_flag = 0 get_image_flag = 1 #let yolo identify item time.sleep(0.5) get_image_flag = 0 realsense_look() # show_orientation() pos.x = pos.x pos.y = pos.y pos.z = -2.35 pos.pitch = -90 pos.roll = 0 pos.yaw = 0 sent_sucker_signal("on") action = 2 print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ', pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw) ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 100, 2) #action,ra,grip,vel,both break if case(2): #catch items in box print("---catch item stage 1---") sent_sucker_signal("on") pos.x = cntr_point.center_point_x pos.y = cntr_point.center_point_y pos.z = -2.65 pos.pitch = -90 pos.roll = 0 pos.yaw = 0 action = 12 if cntr_point.center_point_y < 51: if cntr_point.center_point_x > 1.5: pos.yaw = -90 elif cntr_point.center_point_x < 1.5: pos.yaw = 90 print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ', pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw) ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 100, 2) #action,ra,grip,vel,both break if case(3): #down & suck the item pos.x = pos.x pos.y = pos.y pos.z = pos.z - 0.4 pos.pitch = -90 pos.roll = 0 pos.yaw = pos.yaw ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 100, 2) #action,ra,grip,vel,both # arduino_sucker_info() if pos.z <= -26.35 or down_stop_flag == 0: # -13.45 print("---catch item stage 3---") action = 4 print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ', pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw) break if case(4): #arm up #time.sleep(0.5) print("---Arm up---") pos.x = pos.x pos.y = pos.y pos.z = 0 pos.pitch = -90 pos.roll = 0 pos.yaw = pos.yaw action = 5 if down_stop_flag == 1: action = 0 # if suck_point < 4: # suck_point = suck_point +1 # else: # suck_point = 0 print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ', pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw) ArmTask.Speed_Mode(0) ArmTask.Arm_Mode(4, 1, 0, 30, 2) #speed down ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 100, 2) #action,ra,grip,vel,both #========================new function======================================# if image_point.label == "bottle": diff_item = 0 action = 7 elif image_point.label == "paper": diff_item = 1 action = 7 elif image_point.label == "metal": diff_item = 2 action = 7 else: diff_item = 0 action = 7 print("cant see anything") break if case(5): #move to camera sight print("---move to camera sight---") pos.x = -13 pos.y = 55 pos.z = 1.35 pos.pitch = -90 pos.roll = 0 pos.yaw = 0 action = 6 print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ', pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw) ArmTask.Speed_Mode(1) ArmTask.Arm_Mode(4, 1, 0, 10, 2) #speed up ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 50, 2) #action,ra,grip,vel,both time.sleep(1) #please delete break if case(6): #analyst item & pos global is_metal, is_bottle, is_paper print("---analyst item---") time.sleep(1) action = 10 yolo_receive() if image_point.label == "metal": is_metal = is_metal + 1 elif image_point.label == "bottle": is_bottle = is_bottle + 1 elif image_point.label == "paper": is_paper = is_paper + 1 if is_metal == 2 or is_bottle == 2 or is_metal == 2: #if first two times is the same then direct identify item identify_item() action = 7 pos.x = -13 pos.y = 55 pos.z = 1.35 pos.pitch = -90 pos.roll = 0 pos.yaw = 0 ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 60, 2) #action,ra,grip,vel,both elif picture_count >= 2: #maybe can delete? identify_item() action = 7 pos.x = -13 pos.y = 55 pos.z = 1.35 pos.pitch = -90 pos.roll = 0 pos.yaw = 0 ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 60, 2) #action,ra,grip,vel,both picture_count = picture_count + 1 break if case(7): #select final point print("---select final point---") picture_count = 0 if diff_item == 0: #bottle final_x = 1.5 final_y = 35.5 print("Its bottle!") action = 8 elif diff_item == 1: #paper final_x = 33 final_y = 43.5 print("Its paper!") action = 8 elif diff_item == 2: #metal final_x = -24 final_y = 43.5 print("Its metal!") action = 8 elif diff_item == 3: action = 10 print("cant identify item!") else: print("unkown item") break if case(8): #move to final point print("---move to final point---") pos.x = final_x pos.y = final_y pos.z = star_config.yolo_z pos.pitch = -90 pos.roll = 0 pos.yaw = pos.yaw action = 9 print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ', pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw) ArmTask.Speed_Mode(1) ArmTask.Arm_Mode(4, 1, 0, 30, 2) #speed up ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 60, 2) #action,ra,grip,vel,both break if case(9): #drop item to container & recycle print("---drop item to container---") if down_stop_flag == 0: count_stop = count_stop + 1 time.sleep(0.75) action = 0 sent_sucker_signal("off") suck_point = 0 change_position = 0 print("round: ", count_stop) break #===============================================================================# if case(10): #use when second camera get print("---spin the item---") pos.x = -13 pos.y = 55 pos.z = 1.35 pos.pitch = -90 pos.roll = 0 pos.yaw = pos.yaw + 30 action = 6 print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ', pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw) ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 100, 2) #action,ra,grip,vel,both break if case(11): #ensure realsense not to look for same point print("---change realsense position---") if suck_point == 1: #realsense to front pos.x = 1.5 pos.y = 48.5 pos.z = star_config.yolo_z elif suck_point == 2: #realsense to rear pos.x = 1.5 pos.y = 42.5 pos.z = star_config.yolo_z elif suck_point == 3: #realsense to front(down) pos.x = 1.5 pos.y = 45.5 pos.z = -4.65 elif suck_point == 4: #realsense to rear(down) pos.x = 1.5 pos.y = 42.5 pos.z = -4.65 else: #realsense to original position pos.x = 1.5 pos.y = 45.5 pos.z = star_config.yolo_z print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ', pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw) ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 60, 2) #action,ra,grip,vel,both action = 1 if case(12): #catch items in box(down) print("---catch item stage 2---") pos.x = cntr_point.center_point_x pos.y = cntr_point.center_point_y pos.z = cntr_point.z_depth pos.pitch = -90 pos.roll = 0 pos.yaw = pos.yaw action = 3 print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ', pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw) ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 80, 2) #action,ra,grip,vel,both break if case(): # default, could also just omit condition or 'if True' rospy.on_shutdown(myhook) ArmTask.rospy.on_shutdown(myhook)
def __init__(self): # Initializing publisher with buffer size of 10 messages self.pub_ = rospy.Publisher("recognized", String, queue_size=10) # initialize node rospy.init_node("asr_control") # Call custom function on node shutdown rospy.on_shutdown(self.shutdown) # Params # File containing language model _lm_param = "~lm" # Dictionary _dict_param = "~dict" # Hidden markov model. Default has been provided below _hmm_param = "~hmm" # Gram file contains the rules and grammar _gram = "~gram" # Name of rule within the grammar _rule = "~rule" _grammar = "~grammar" # check if lm or grammar mode. Default = grammar self._use_lm = 0 self.in_speech_bf = False # Setting param values if rospy.has_param(_hmm_param): self.hmm = rospy.get_param(_hmm_param) if rospy.get_param(_hmm_param) == ":default": if os.path.isdir( "/usr/local/lib/python2.7/dist-packages/pocketsphinx/model/" ): rospy.loginfo("Loading the default acoustic model") self.hmm = "/usr/local/lib/python2.7/dist-packages/pocketsphinx/model/en-us/" rospy.loginfo("Done loading the default acoustic model") else: rospy.logerr( "No language model specified. Couldn't find default model." ) return else: rospy.logerr( "No language model specified. Couldn't find default model.") return if rospy.has_param( _dict_param) and rospy.get_param(_dict_param) != ":default": self.dict = rospy.get_param(_dict_param) else: rospy.logerr( "No dictionary found. Please add an appropriate dictionary argument." ) return if rospy.has_param( _grammar) and rospy.get_param(_grammar) != ":default": pass else: rospy.logerr( "No grammar found. Please add an appropriate grammar along with gram file." ) return if rospy.has_param( _lm_param) and rospy.get_param(_lm_param) != ':default': self._use_lm = 1 self.class_lm = rospy.get_param(_lm_param) elif rospy.has_param(_gram) and rospy.has_param(_rule): self._use_lm = 0 self.gram = rospy.get_param(_gram) self.rule = rospy.get_param(_rule) else: rospy.logerr( "Couldn't find suitable parameters. Please take a look at the documentation" ) return # All params satisfied. Starting recognizer self.start_recognizer()
def __init__(self): ## Init node rospy.init_node("uisee_driver") # Shutdown function rospy.on_shutdown(self.shutdown) ## Get Parameters # joy axes and buttons index self.left_joy_lr = rospy.get_param("~left_joy_lr", 0) self.Y = rospy.get_param("Y", 0) self.B = rospy.get_param("B", 1) self.A = rospy.get_param("A", 2) self.X = rospy.get_param("X", 3) #self.LB = rospy.get_param("LB", 4) self.RB = rospy.get_param("RB", 5) self.LT = rospy.get_param("LT", 6) self.RT = rospy.get_param("RT", 7) # the amount of change of velocity self.v_delta = rospy.get_param("~v_delta", 10.0) self.v_c = rospy.get_param("~v_c", 30.0) self.s_turn = rospy.get_param("~s_turn", 2.0) # gear interval self.v_min = rospy.get_param("~v_min", 15.0) self.v_max = rospy.get_param("~v_max", 45.0) self.w1_scale = rospy.get_param("~gear_w1", 3.0) self.w2_scale = rospy.get_param("~gear_w2", 5.0) self.w3_scale = rospy.get_param("~gear_w3", 10.0) # for local computer, use 127.0.0.1 target_ip = rospy.get_param("~target_ip", '192.168.1.108') # img self.data_path = rospy.get_param("~data_path", './dataset/img/') self.count = rospy.get_param("~img_count", 0) self.num_frames = rospy.get_param("~num_frames", 10) ## Init Variables self.steer = 0.0 #self.last_speed = 0.0 self.gear_w1_pressed = False self.gear_w2_pressed = False self.gear_w3_pressed = False self.data_flag = False ## Uisee Simulator #connection self.id = usimpy.UsimCreateApiConnection(target_ip, 17771, 5000, 10000) ret = usimpy.UsimStartSim(self.id, 10000) print(ret) #control self.control = usimpy.UsimSpeedAdaptMode() #states self.states = usimpy.UsimVehicleState() #collision self.collision = usimpy.UsimCollision() #image self.image = usimpy.UsimCameraResponse() # Joy subscriber (global topic. Make sure the joy node has been run) self.joy_sub = rospy.Subscriber("joy", Joy, self.joy_callback, queue_size=10) # start a timer to publish control msg in 10hz self.timer_pub = rospy.Timer(rospy.Duration(0.1), self.cmd_publish)
def __init__(self): rospy.init_node('GpsrSmach') rospy.on_shutdown(self.shutdown) rospy.logerr('you are all sillyb') self.smach_bool = False # xm_arm_moveit_name('nav_pose')#最开始时,把机械臂降下来 self.sm_EnterRoom = StateMachine( outcomes=['succeeded', 'aborted', 'error']) with self.sm_EnterRoom: # if use arm, use this state # self.sm_EnterRoom.userdata.arm_mode_1 =0 # self.sm_EnterRoom.userdata.arm_ps = PointStamped() # StateMachine.add('NAV_POSE', # ArmCmd(), # transitions={'succeeded':'DOOR_DETECT','aborted':'NAV_POSE','error':'error'}, # remapping ={'arm_ps':'arm_ps','mode':'arm_mode_1'}) #first detect the door is open or not StateMachine.add('DOOR_DETECT', DoorDetect().door_detect_, transitions={ 'invalid': 'WAIT', 'valid': 'DOOR_DETECT', 'preempted': 'aborted' }) # after clear the costmap, wait for some seconds self.sm_EnterRoom.userdata.rec = 3.0 StateMachine.add('WAIT', Wait(), transitions={ 'succeeded': 'SIMPLE_MOVE', 'error': 'error' }, remapping={'rec': 'rec'}) # before executing the nav-stack, move directly through the door may make the nav-stack easier self.sm_EnterRoom.userdata.go_point = Point(0.1, 0.0, 0.0) StateMachine.add('SIMPLE_MOVE', SimpleMove_move(), remapping={'point': 'go_point'}, transitions={ 'succeeded': 'NAV_1', 'aborted': 'NAV_1', 'error': 'error' }) # go to the known Pose in the map to get the gpsr-command self.sm_EnterRoom.userdata.start_waypoint = gpsr_target['speaker'][ 'pos'] StateMachine.add('NAV_1', NavStack(), transitions={ 'succeeded': 'succeeded', 'aborted': 'NAV_1', 'error': 'error' }, remapping={'pos_xm': 'start_waypoint'}) self.sm_Find = StateMachine(outcomes=['succeeded', 'aborted', 'error'], input_keys=['target', 'current_task']) # 寻找人 with self.sm_Find: # for the people-finding task, pay attention that at the end of the task, # run a simple executable file to make the cv-node release the KinectV2 # otherwise, the find-object task may throw error and all the cv-node may # break down self.sm_Person = StateMachine( outcomes=['succeeded', 'aborted', 'error']) with self.sm_Person: self.sm_Person.userdata.rec = 2.0 # run the people_tracking node StateMachine.add('RUNNODE', RunNode(), transitions={ 'succeeded': 'WAIT', 'aborted': 'succeeded' }) StateMachine.add('WAIT', Wait(), transitions={ 'succeeded': 'GET_PEOPLE_POS', 'error': 'error' }, remapping={'rec': 'rec'}) #the data should be PointStamped() # 这里必须先运行节点,也就是用RunNode()状态 self.sm_Person.userdata.pos_xm = Pose() StateMachine.add('GET_PEOPLE_POS', FindPeople().find_people_, transitions={ 'invalid': 'NAV_PEOPLE', 'valid': 'SPEAK', 'preempted': 'aborted' }, remapping={'pos_xm': 'pos_xm'}) # this state will use the userdata remapping in the last state StateMachine.add('NAV_PEOPLE', NavStack(), transitions={ 'succeeded': 'SPEAK', 'aborted': 'NAV_PEOPLE', 'error': 'error' }, remapping={'pos_xm': 'pos_xm'}) self.sm_Person.userdata.sentences = 'I find you' StateMachine.add('SPEAK', Speak(), transitions={ 'succeeded': 'CLOSEKINECT', 'error': 'error' }, remapping={'sentences': 'sentences'}) # close the KinectV2 StateMachine.add('CLOSEKINECT', CloseKinect(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted' }) self.sm_Pos = StateMachine( outcomes=['succeeded', 'aborted', 'error'], input_keys=['target', 'current_task']) with self.sm_Pos: # this command may be foolish, but it may be used in the next competition # get the name of the target # in the target_gpsr.py we define some Pose() , they are all named as ${room_name}+'_table_1(2)' # because each room we have some best detecting positions, we should make xm go to these places # for object-find tasks self.sm_Pos.userdata.pose = Pose() self.sm_Pos.userdata.mode_1 = 1 StateMachine.add('GET_POS', GetPos(), remapping={ 'target': 'target', 'current_task': 'current_task', 'pose': 'pose', 'mode': 'mode_1' }, transitions={ 'succeeded': 'NAV_HEHE', 'aborted': 'aborted', 'error': 'error' }) StateMachine.add('NAV_HEHE', NavStack(), remapping={'pos_xm': 'pose'}, transitions={ 'succeeded': 'GET_TARGET', 'aborted': 'NAV_HEHE', 'error': 'error' }) self.sm_Pos.userdata.target_mode = 0 self.sm_Pos.userdata.name = '' StateMachine.add('GET_TARGET', GetTarget(), remapping={ 'target': 'target', 'current_task': 'current_task', 'mode': 'target_mode', 'current_target': 'name' }, transitions={ 'succeeded': 'FIND_OBJECT', 'aborted': 'aborted', 'error': 'error' }) self.sm_Pos.userdata.object_pos = PointStamped() StateMachine.add('FIND_OBJECT', FindObject(), transitions={ 'succeeded': 'SPEAK', 'aborted': 'GET_POS_1', 'error': 'error' }, remapping={ 'name': 'name', 'object_pos': 'object_pos' }) # if xm not finds the object in the first detecting-place, she should go to the second # specified place for detect-task, but as U can see in the target_gpsr.py, some room are # only one best detecting-place self.sm_Pos.userdata.pose_1 = Pose() self.sm_Pos.userdata.mode_2 = 2 StateMachine.add('GET_POS_1', GetPos(), remapping={ 'target': 'target', 'current_task': 'current_task', 'pose': 'pose_1', 'mode': 'mode_2' }, transitions={ 'succeeded': 'NAV_HAHA', 'aborted': 'aborted', 'error': 'error' }) StateMachine.add('NAV_HAHA', NavStack(), remapping={'pos_xm': 'pose_1'}, transitions={ 'succeeded': 'FIND_OBJECT_1', 'aborted': 'NAV_HAHA', 'error': 'error' }) self.sm_Pos.userdata.object_pos = PointStamped() StateMachine.add('FIND_OBJECT_1', FindObject(), transitions={ 'succeeded': 'SPEAK', 'aborted': 'SPEAK', 'error': 'error' }, remapping={ 'name': 'name', 'object_pos': 'object_pos' }) self.sm_Pos.userdata.sentences = 'I find it' StateMachine.add('SPEAK', Speak(), transitions={ 'succeeded': 'succeeded', 'error': 'error' }, remapping={'sentences': 'sentences'}) # this state is used for swithing the mode, finding people or finding object? StateMachine.add('PERSON_OR_POS', PersonOrPosition(), transitions={ 'person': 'PERSON', 'position': 'POS', 'error': 'error' }, remapping={ 'target': 'target', 'current_task': 'current_task' }) StateMachine.add('PERSON', self.sm_Person, transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'error': 'error' }) StateMachine.add('POS', self.sm_Pos, transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'error': 'error' }, remapping={ 'target': 'target', 'current_task': 'current_task' }) #nav tasks self.sm_Nav = StateMachine(outcomes=['succeeded', 'aborted', 'error'], input_keys=['target', 'current_task']) with self.sm_Nav: self.sm_Nav.userdata.pos_xm = Pose() self.sm_Nav.userdata.target_mode = 1 StateMachine.add('GETTARGET', GetTarget(), transitions={ 'succeeded': "NAV_GO", 'aborted': 'aborted', 'error': 'error' }, remapping={ 'target': 'target', 'current_task': "current_task", 'current_target': 'pos_xm', 'mode': 'target_mode' }) # StateMachine.add('FINDWAY', # FindWay(), # transitions = {'succeeded':'NAV_PATH1','aborted':'NAV_GO','error':'NAV_GO'}, # remapping={'way_path1':'way_path1','way_path2':'way_path2'}) # StateMachine.add('NAV_PATH1', # NavStack(), # transitions={'succeeded':'NAV_PATH2','aborted':'NAV_PATH1','error':'error'}, # remapping={'pos_xm':'way_path1'}) # StateMachine.add('NAV_PATH2', # NavStack(), # transitions = {'succeeded':'NAV_GO','aborted':'NAV_PATH2','error':'error'}, # remapping={'pos_xm':'way_path2'}) StateMachine.add('NAV_GO', NavStack(), transitions={ 'succeeded': 'SPEAK', 'aborted': 'NAV_GO', 'error': 'error' }, remapping={'pos_xm': 'pos_xm'}) self.sm_Nav.userdata.sentences = 'I arrive here' StateMachine.add('SPEAK', Speak(), transitions={ 'succeeded': 'succeeded', 'error': 'error' }, remapping={'sentences': 'sentences'}) #follow task # we will use the concurrence fun # the tutorials in the wiki too easy, it is no help for you to understand the concurrence # please see the test-snippet of the source of executive-smach # https://github.com/ros/executive_smach/blob/indigo-devel/smach_ros/test/concurrence.py self.sm_Follow = Concurrence(outcomes=['succeeded', 'aborted'], default_outcome='succeeded', outcome_map={ 'succeeded': { 'STOP': 'stop' }, 'aborted': { 'FOLLOW': 'aborted' } }, child_termination_cb=self.child_cb) with self.sm_Follow: self.meta_follow = StateMachine(outcomes=['succeeded', 'aborted']) with self.meta_follow: StateMachine.add('FOLLOW', SimpleFollow(), transitions={ 'succeeded': 'FOLLOW', 'aborted': 'aborted', 'preempt': 'succeeded' }) Concurrence.add('FOLLOW', self.meta_follow) Concurrence.add('STOP', CheckStop()) Concurrence.add('RUNNODE', RunNode()) # self.meta_Follow = StateMachine(outcomes =['succeeded','aborted','error']) # with self.meta_Follow: # StateMachine.add('RUNNODE', # RunNode(), # transitions={'succeeded':'FIND_PEOPLE','aborted':'succeeded'}) # self.meta_Follow.userdata.pos_xm = Pose() # StateMachine.add('FIND_PEOPLE', # FindPeople().find_people_, # remapping ={'pos_xm':'pos_xm'}, # transitions ={'invalid':'MOVE','valid':'FIND_PEOPLE','preempted':'aborted'}) # StateMachine.add('MOVE', # NavStack(), # remapping ={'pos_xm':'pos_xm'}, # transitions={'succeeded':'FIND_PEOPLE','aborted':'MOVE','error':'error'}) # self.meta_Stop = StateMachine(outcomes =['succeeded','aborted']) # with self.meta_Stop: # StateMachine.add('STOP', # StopFollow(), # transitions ={'succeeded':'succeeded','aborted':'STOP'}) # Concurrence.add('FOLLOW', # self.meta_Follow) # Concurrence.add('STOP', # self.meta_Stop) # speak task self.sm_Talk = StateMachine(outcomes=['succeeded', 'aborted', 'error']) with self.sm_Talk: self.sm_Talk.userdata.people_condition = list() StateMachine.add('SPEAK', Anwser(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted' }) self.sm_Pick = StateMachine(outcomes=['succeeded', 'aborted', 'error'], input_keys=['target', 'current_task']) with self.sm_Pick: self.sm_Pick.userdata.name = '' self.sm_Pick.userdata.target_mode = 0 self.sm_Pick.userdata.objmode = -1 StateMachine.add('RUNNODE_IMG', RunNode_img(), transitions={ 'succeeded': 'GETNAME', 'aborted': 'aborted' }) StateMachine.add('GETNAME', GetTarget(), remapping={ 'target': 'target', 'current_task': 'current_task', 'mode': 'target_mode', 'current_target': 'name' }, transitions={ 'succeeded': 'FIND_OBJECT', 'aborted': 'aborted', 'error': 'error' }) self.sm_Pick.userdata.object_pos = PointStamped() StateMachine.add('FIND_OBJECT', FindObject(), transitions={ 'succeeded': 'POS_JUSTFY', 'aborted': 'FIND_OBJECT', 'error': 'SPEAK' }, remapping={ 'name': 'name', 'object_pos': 'object_pos', 'objmode': 'objmode' }) #making the xm foreward the object may make the grasping task easier self.sm_Pick.userdata.pose = Pose() StateMachine.add('POS_JUSTFY', PosJustfy(), remapping={ 'object_pos': 'object_pos', 'pose': 'pose' }, transitions={ 'succeeded': 'NAV_TO', 'aborted': 'aborted', 'error': 'error' }) StateMachine.add('NAV_TO', NavStack(), transitions={ 'succeeded': 'RUNNODE_IMG2', 'aborted': 'NAV_TO', 'error': 'error' }, remapping={"pos_xm": 'pose'}) StateMachine.add('RUNNODE_IMG2', RunNode_img(), transitions={ 'succeeded': 'FIND_AGAIN', 'aborted': 'aborted' }) StateMachine.add('FIND_AGAIN', FindObject(), transitions={ 'succeeded': 'PICK', 'aborted': 'FIND_AGAIN', 'error': 'SPEAK' }, remapping={ 'name': 'name', 'object_pos': 'object_pos', 'objmode': 'objmode' }) self.sm_Pick.userdata.arm_mode_1 = 1 StateMachine.add('PICK', ArmCmd(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'error': 'error' }, remapping={ 'arm_ps': 'object_pos', 'mode': 'arm_mode_1' }) self.sm_Pick.userdata.sentences = 'xiao meng can not find things' StateMachine.add('SPEAK', Speak(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'error': 'error' }) self.sm_Place = StateMachine( outcomes=['succeeded', 'aborted', 'error']) with self.sm_Place: # place_ps please specified due to the scene self.sm_Place.userdata.place_ps = PointStamped() self.sm_Place.userdata.place_ps.header.frame_id = 'base_link' self.sm_Place.userdata.place_ps.point.x = 0.8 self.sm_Place.userdata.place_ps.point.y = 0.0 self.sm_Place.userdata.place_ps.point.z = 0.6 self.sm_Place.userdata.objmode = 2 # without moveit, if is just place it in a open space self.sm_Place.userdata.arm_mode_1 = 2 StateMachine.add('PLACE', ArmCmd(), transitions={ 'succeeded': 'succeeded', 'aborted': 'PLACE', 'error': 'error' }, remapping={ 'arm_ps': 'place_ps', 'mode': 'arm_mode_1' }) self.sm_GPSR = StateMachine(outcomes=['succeeded', 'aborted', 'error']) with self.sm_GPSR: self.sm_GPSR.userdata.target = list() self.sm_GPSR.userdata.action = list() self.sm_GPSR.userdata.task_num = 0 self.sm_GPSR.userdata.current_task = -1 self.sm_GPSR.userdata.current_turn = 1 self.sm_GPSR.userdata.turn = 3 self.sm_GPSR.userdata.pos_xm_door = gpsr_target['speaker']['pos'] self.sm_GPSR.userdata.pos_xm_exit = gpsr_target['exit']['pos'] self.sm_GPSR.userdata.sentences = 'please command me' StateMachine.add('SPEAK_RESTART', Speak(), transitions={ 'succeeded': 'RECEIVE_TASKS', 'aborted': 'RECEIVE_TASKS', 'error': 'RECEIVE_TASKS' }) # get the task due to the speech-node StateMachine.add('RECEIVE_TASKS', GetTask(), transitions={ 'succeeded': 'GET_NEXT_TASK', 'aborted': 'RECEIVE_TASKS', 'error': 'error' }, remapping={ 'target': 'target', 'action': 'action', 'task_num': 'task_num' }) #get the task in order and execute the task # if want to go to the door at the end of the task, please modify: # succeeded->succeeded --> succeeded->BYEBYE StateMachine.add('GET_NEXT_TASK', NextDo(), transitions={ 'succeeded': 'CHECK_TURN', 'aborted': 'aborted', 'error': 'error', 'find': 'FIND', 'go': 'GO', 'follow': 'FOLLOW', 'pick': 'GET_NEXT_TASK', 'place': 'GET_NEXT_TASK', 'talk': 'TALK', 'aborted': 'aborted', 'error': 'error' }, remapping={ 'action': 'action', 'current_task': 'current_task', 'task_num': 'task_num' }) StateMachine.add('CHECK_TURN', CheckTurn(), transitions={ 'succeeded': 'BACK_EXIT', 'continue': 'BACK_DOOR', 'error': 'error' }) StateMachine.add('BACK_DOOR', NavStack(), transitions={ 'succeeded': 'CHECK_TURN', 'aborted': 'BACK_DOOR', 'error': 'error' }, remapping={'pos_xm': 'pos_xm_door'}) StateMachine.add('BACK_EXIT', NavStack(), transitions={ 'succeeded': 'succeeded', 'aborted': 'BACK_EXIT', 'error': 'error' }, remapping={'pos_xm': 'pos_xm_exit'}) # all the task-group StateMachine.add('GO', self.sm_Nav, remapping={ 'target': 'target', 'current_task': 'current_task' }, transitions={ 'succeeded': 'GET_NEXT_TASK', 'aborted': 'GO' }) StateMachine.add('FIND', self.sm_Find, remapping={ 'target': 'target', 'current_task': 'current_task' }, transitions={ 'succeeded': 'GET_NEXT_TASK', 'aborted': 'FIND' }) StateMachine.add('FOLLOW', self.sm_Follow, transitions={ 'succeeded': 'CLOSE', 'aborted': 'FOLLOW' }) StateMachine.add('CLOSE', CloseKinect(), transitions={ 'succeeded': 'GET_NEXT_TASK', 'aborted': 'GET_NEXT_TASK' }) StateMachine.add('PICK', self.sm_Pick, remapping={ 'target': 'target', 'current_task': 'current_task' }, transitions={ 'succeeded': 'GET_NEXT_TASK', 'aborted': 'PICK' }) StateMachine.add('TALK', self.sm_Talk, transitions={ 'succeeded': 'GET_NEXT_TASK', 'aborted': 'TALK' }) StateMachine.add('PLACE', self.sm_Place, transitions={ 'succeeded': 'GET_NEXT_TASK', 'aborted': 'PLACE' }) self.sm_GPSR.userdata.string = 'I finish all the three tasks, I will go out the stage' StateMachine.add('BYEBYE', Speak(), transitions={ 'succeeded': 'GOOUT', 'error': 'error' }, remapping={'sentences': 'string'}) #no need self.sm_GPSR.userdata.waypoint = gpsr_target['end']['pos'] StateMachine.add('GOOUT', NavStack(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'error': 'error' }, remapping={"pos_xm": 'waypoint'}) intro_server = IntrospectionServer('enter_room', self.sm_EnterRoom, '/SM_ROOT') intro_server.start() out_1 = self.sm_EnterRoom.execute() intro_server.stop() #只运行一次 intro_server = IntrospectionServer('sm_gpsr', self.sm_GPSR, '/SM_ROOT') intro_server.start() out_2 = self.sm_GPSR.execute() intro_server.stop() self.smach_bool = True
def __init__(self): #give the node a name rospy.init_node('calibrate_linear', anonymous=False) #set rospy to execute a shutdown function when terminating the script rospy.on_shutdown(self.shutdown) #How fast will we check the odometry values? self.rate = 10 r = rospy.Rate(self.rate) #set the distance to travel self.test_distance = 1.5 self.speed = 0.2 self.tolerance = 0.01 self.odom_linear_scale_correction = 1.0 self.start_test = True #Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) #The base frame is base_footprint for the robot self.base_frame = rospy.get_param('~base_frame', '/base_footprint') #The odom frame is usually just /odom self.odom_frame = rospy.get_param('~odom_frame', '/odom') #initialize the tf listener self.tf_listener = tf.TransformListener() #give tf some time to fill its buffer rospy.sleep(2) #make sure we see the odom and base frames self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) self.position = Point() #get the starting position from the tf transform between the odom and base frames self.position = self.get_position() x_start = self.position.x y_start = self.position.y move_cmd = Twist() while not rospy.is_shutdown(): #Stop the robot by default move_cmd = Twist() if self.start_test: #get the current position from the tf transform between the odom and base frames self.position = self.get_position() #compute the euclidean distance from the target point distance = sqrt( pow((self.position.x - x_start), 2) + pow((self.position.y - y_start), 2)) #correct the estimate distance by the correction factor distance *= self.odom_linear_scale_correction #How close are we? error = distance - self.test_distance #are we close enough? if not self.start_test or abs(error) < self.tolerance: self.start_test = False params = False rospy.loginfo(params) else: #if not, move in the appropriate direction move_cmd.linear.x = copysign(self.speed, -1 * error) else: self.position = self.get_position() x_start = self.position.x y_start = self.position.y self.cmd_vel.publish(move_cmd) r.sleep() #stop the robot self.cmd_vel.publish(Twist())
def __init__(self): self.datainput = Datainput() self.pub_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=10) rospy.on_shutdown(self.shutdown) self.flag = 2
def exec_music(goal): r = MusicResult() fb = MusicFeedback() for i, f in enumerate(goal.freqs): fb.remaining_steps = len(goal.freqs) - i music.publish_feedback(fb) if music.is_preempt_requested(): write_freq(0) r.finished = False music.set_preempted(r) return write_freq(f) rospy.sleep(1.0 if i >= len(goal.durations) else goal.durations[i]) r.finished = True music.set_succeeded(r) def recv_buzzer(data): write_freq(data.data) if __name__ == '__main__': rospy.init_node('buzzer') rospy.Subscriber("buzzer", UInt16, recv_buzzer) music = actionlib.SimpleActionServer('music', MusicAction, exec_music, False) # 追加 music.start() # 追加 rospy.on_shutdown(write_freq) # 追加 rospy.spin()
def __init__(self): # Give the node a name rospy.init_node('calibrate_linear', anonymous=False) # Set rospy to execute a shutdown function when terminating the script rospy.on_shutdown(self.shutdown) # How fast will we check the odometry values? self.rate = rospy.get_param('~rate', 20) r = rospy.Rate(self.rate) # Set the distance to travel self.test_distance = rospy.get_param('~test_distance', 1.0) # meters self.speed = rospy.get_param('~speed', 0.15) # meters per second self.tolerance = rospy.get_param('~tolerance', 0.01) # meters self.odom_linear_scale_correction = rospy.get_param( '~odom_linear_scale_correction', 1.0) self.start_test = rospy.get_param('~start_test', True) # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) # Fire up the dynamic_reconfigure server dyn_server = Server(CalibrateLinearConfig, self.dynamic_reconfigure_callback) # Connect to the dynamic_reconfigure server dyn_client = dynamic_reconfigure.client.Client("calibrate_linear", timeout=60) # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot self.base_frame = rospy.get_param('~base_frame', '/base_link') # The odom frame is usually just /odom self.odom_frame = rospy.get_param('~odom_frame', '/odom') # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Make sure we see the odom and base frames self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) rospy.loginfo("Bring up rqt_reconfigure to control the test.") self.position = Point() # Get the starting position from the tf transform between the odom and base frames self.position = self.get_position() x_start = self.position.x y_start = self.position.y move_cmd = Twist() while not rospy.is_shutdown(): # Stop the robot by default move_cmd = Twist() if self.start_test: # Get the current position from the tf transform between the odom and base frames self.position = self.get_position() # Compute the Euclidean distance from the target point distance = sqrt( pow((self.position.x - x_start), 2) + pow((self.position.y - y_start), 2)) # Correct the estimated distance by the correction factor distance *= self.odom_linear_scale_correction # How close are we? error = distance - self.test_distance # Are we close enough? if not self.start_test or abs(error) < self.tolerance: self.start_test = False params = {'start_test': False} rospy.loginfo(params) dyn_client.update_configuration(params) else: # If not, move in the appropriate direction move_cmd.linear.x = copysign(self.speed, -1 * error) else: self.position = self.get_position() x_start = self.position.x y_start = self.position.y self.cmd_vel.publish(move_cmd) r.sleep() # Stop the robot self.cmd_vel.publish(Twist())
# print pose_target # self.group.set_goal_tolerance(0.1) self.group.set_pose_target(pose_target) # self.group.set_position_target((msg.data[0],msg.data[1],msg.data[2])) self.plan = self.group.plan() # print self.group.plan() ok = self.group.execute(self.plan, wait=True) print ok return ok # if ok: # break # # print self.plan # p += 0.01 # print p def onShutdown(self): rospy.loginfo("[%s] Shutting down..." % self.node_name) rospy.sleep(0.5) #To make sure that it gets published. rospy.loginfo("[%s] Shutdown" % self.node_name) if __name__ == '__main__': rospy.init_node('block_pick_and_place', anonymous=False) app = QApplication(sys.argv) block_pick_and_place = block_pick_and_place() block_pick_and_place.show() app.exec_() rospy.on_shutdown(block_pick_and_place.onShutdown) rospy.spin()
# rospy.loginfo("[%s] node %s msg %s" %(self.node_name, node_name, msg)) # rospy.loginfo("[%s] Node %s set to %s." %(self.node_name, node_name, node_state)) self.active_nodes = copy.deepcopy(active_nodes) def cbEvent(self,msg,event_name): if (msg.data == self.event_trigger_dict[event_name]): # Update timestamp rospy.loginfo("[%s] Event: %s" %(self.node_name,event_name)) self.state_msg.header.stamp = msg.header.stamp next_state = self._getNextState(self.state_msg.state,event_name) if next_state is not None: # Has a defined transition self.state_msg.state = next_state self.publish() def on_shutdown(self): rospy.loginfo("[%s] Shutting down." %(self.node_name)) if __name__ == '__main__': # Initialize the node with rospy rospy.init_node('fsm_node', anonymous=False) # Create the NodeName object node = FSM() # Setup proper shutdown behavior rospy.on_shutdown(node.on_shutdown) # Keep it spinning to keep the node alive rospy.spin()
def Mission_Trigger(): global action, Arm_state_flag, Sent_data_flag if Arm_state_flag == Arm_status.Idle and Sent_data_flag == 1: Sent_data_flag = 0 Arm_state_flag = Arm_status.Isbusy for case in switch(action): #傳送指令給socket選擇手臂動作 if case(0): pos.x = 10 pos.y = 36.8 pos.z = 11.35 pos.pitch = -90 pos.roll = 0 pos.yaw = 0 action = 1 print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ', pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw) #ArmTask.strategy_client_Arm_Mode(2,1,0,10,2)#action,ra,grip,vel,both ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 10, 2) #action,ra,grip,vel,both break if case(1): pos.x = 10 pos.y = 42 pos.z = 11.35 pos.pitch = -90 pos.roll = 0 pos.yaw = 0 action = 2 print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ', pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw) #ArmTask.strategy_client_Arm_Mode(2,1,0,10,2)#action,ra,grip,vel,both ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 10, 2) #action,ra,grip,vel,both break if case(2): pos.x = -10 pos.y = 42 pos.z = 11.35 pos.pitch = -90 pos.roll = 0 pos.yaw = 0 action = 3 print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ', pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw) #ArmTask.strategy_client_Arm_Mode(2,1,0,10,2)#action,ra,grip,vel,both ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 10, 2) #action,ra,grip,vel,both break if case(3): pos.x = -10 pos.y = 36.8 pos.z = 11.35 pos.pitch = -90 pos.roll = 0 pos.yaw = 0 action = 4 print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ', pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw) #ArmTask.strategy_client_Arm_Mode(2,1,0,10,2)#action,ra,grip,vel,both ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 10, 2) #action,ra,grip,vel,both break if case(4): pos.x = 0 pos.y = 36.8 pos.z = 11.35 pos.pitch = -90 pos.roll = 0 pos.yaw = 0 action = 0 print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ', pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw) #ArmTask.strategy_client_Arm_Mode(2,1,0,10,2)#action,ra,grip,vel,both ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll, pos.yaw) ArmTask.Arm_Mode(2, 1, 0, 10, 2) #action,ra,grip,vel,both break if case(): # default, could also just omit condition or 'if True' rospy.on_shutdown(myhook) ArmTask.rospy.on_shutdown(myhook)
s = self.sensor_values data.linear.x += accel if self.sensor_values.sum_all >= 50: data.linear.x = 0.0 elif data.linear.x <= 0.2: data.linear.x = 0.2 elif data.linear.x >= 0.8: data.linear.x = 0.8 if data.linear.x < 0.2: data.angular.z = 0.0 elif s.left_side < 10: data.angular.z = 0.0 else: target = 50 error = (target - s.left_side) / 50.0 data.angular.z = error * 3 * math.pi / 180.0 self.cmd_vel.publish(data) rate.sleep() if __name__ == '__main__': rospy.init_node('wall_Trace') rospy.wait_for_service('/motor_on') rospy.wait_for_service('/motor_off') rospy.on_shutdown(rospy.ServiceProxy('/motor_off', Trigger).call) rospy.ServiceProxy('/motor_on', Trigger).call() WallTrace().run()
veh_name=self.veh_name) self.pub_skeletons.publish(marker_array) def callback_beziers(self, beziers_msg): marker_array = beziers_to_marker_array(beziers_msg, veh_name=self.veh_name) self.pub_beziers.publish(marker_array) def callback_waypoint(self, waypoint_msg): point_msg = PointStamped() point_msg.header.frame_id = self.veh_name point_msg.point = waypoint_msg self.pub_waypoint.publish(point_msg) def loginfo(self, message): rospy.loginfo('[{0}] {1}'.format(self.node_name, message)) def setup_parameter(self, name, default): value = rospy.get_param(name, default) return value def on_shutdown(self): self.loginfo('Shutting down') if __name__ == '__main__': rospy.init_node('merganser_visualization_node', anonymous=False) visualization_node = VisualizationNode() rospy.on_shutdown(visualization_node.on_shutdown) rospy.spin()
print("Pick_home service already!!") self.task2 = rospy.Service('task2', task2_srv, self.task2_ser) rospy.wait_for_service('pose_estimation') self.pose_service = rospy.ServiceProxy('/pose_estimation', pose_estimation) print("Pose_estimation service already!!") rospy.wait_for_service('pick') self.pick_service = rospy.ServiceProxy('/pick', pick_srv) print("Picking service already!!") def task2_ser(self, req): pick_home_res = self.pick_service() pose_msg = self.pose_service() for i in range(len(pose_msg.obj_list)): result = self.pick_service(pose_msg.pose[i]) if result.result: return task2_srvResponse(pose_msg.tagID[i]) return task2_srvResponse("Task 2 is Fail") def onShutdown(self): rospy.loginfo("Shutdown.") if __name__ == '__main__': rospy.init_node('client_task2', anonymous=False) rospy.sleep(2) client_task2 = client_task2() rospy.on_shutdown(client_task2.onShutdown) rospy.spin()
def __init__(self): ''' Initiate self and subscribe to /r_values topic ''' # controller variables # with open('variables.yaml') as f: # data2 = yaml.load(f, Loader=yaml.FullLoader) self.running = np.float32(1) self.p34_star = np.array([[0],[0.8]]) self.p24_star = np.array([[-0.8],[0.8]]) self.p14_star = np.array([[-0.8],[0]]) self.d = np.float32(0.8) self.r_safe = params.r_safe self.dd = np.float32(np.sqrt(np.square(self.d)+np.square(self.d))) self.c = params.c self.cf = params.cf # new gain for formation control Nelson self.cI = params.cI self.cP = params.cP self.cD = params.cD self.calpha = params.calpha self.czeta = params.czeta self.U_old = np.array([0, 0]) self.U_oldd = np.array([0, 0]) self.k = 0 # current iteration of the Euler method self.h = params.h # stepsize self.zeta4_old = np.zeros((2,1)) self.zeta_values = np.empty(shape=(0,0)) self.zeta4 = np.zeros((2,1)) self.gamma_old = np.zeros((2,1)) # sta self.gamma = np.zeros((2,1)) self.pGoal = params.pGoal self.pcen_old = np.zeros((2,1)) self.Ug_lim = params.Ug_lim # Motion parameters self.x_dot = np.float32(0) self.y_dot = np.float32(0) self.r_dot = np.float32(0) self.mu_x = self.x_dot*np.array([0, -1, 0, -1, 0]) self.mut_x = self.x_dot*np.array([0, 1, 0, 1, 0]) self.mu_y = self.y_dot*np.array([-1, 0, 0, 0, 1]) self.mut_y = self.y_dot*np.array([1, 0, 0, 0, -1]) self.mu_r = self.r_dot*np.array([-1, -1, 0, 1, -1]) self.mut_r = self.r_dot*np.array([1, 1, 0, -1, 1]) self.mu = self.mu_x+self.mu_y+self.mu_r self.mut = self.mut_x+self.mut_y+self.mut_r # prepare Log arrays self.E3_log = np.array([]) self.E4_log = np.array([]) self.E5_log = np.array([]) self.Un = np.float32([]) self.U_log = np.array([]) self.time = np.float64([]) self.time_log = np.array([]) self.now = np.float64([rospy.get_time()]) self.begin = np.float64([rospy.get_time()]) self.l = 0 # prepare shutdown rospy.on_shutdown(self.shutdown) # prepare publisher self.pub_vel = rospy.Publisher('/n_4/cmd_vel', Twist, queue_size=1) self.velocity = Twist() self.pub_zeta = rospy.Publisher('/n_4/zeta_values', numpy_msg(Floats), queue_size=1) self.pub_pcen = rospy.Publisher('/n_4/pcen', numpy_msg(Floats), queue_size=1) # subscribe to r_values topic rospy.Subscriber('/n_4/r_values', numpy_msg(Floats), self.controller) rospy.Subscriber('/n_4/obstacles', numpy_msg(Floats), self.obstacleAvoidance) # Subscribe to the filtered odometry node rospy.Subscriber('/n_4/odometry/filtered', Odometry, self.formationMotion) rospy.Subscriber('/n_1/Ug_cmd_vel', Twist, self.Ug_cmd_vel) # subscribe to zeta_values topic of each controller rospy.Subscriber('/n_1/zeta_values', numpy_msg(Floats), self.zeta1_sub) rospy.Subscriber('/n_2/zeta_values', numpy_msg(Floats), self.zeta2_sub) rospy.Subscriber('/n_3/zeta_values', numpy_msg(Floats), self.zeta3_sub)
def __init__(self): self.objectCoord = objCoord() self.isApriltag_received = False rospy.logwarn("AprilTag Center Node [ONLINE]...") # rospy shutdown rospy.on_shutdown(self.cbShutdown) # Publish to Bool msg self.isApriltag_topic = "/isApriltag" self.isApriltag_sub = rospy.Subscriber(self.isApriltag_topic, Bool, self.cbIsApriltag) # Subscribe to apriltagList msg self.isApriltagN_topic = "/isApriltag/N" self.isApriltagN_sub = rospy.Subscriber(self.isApriltagN_topic, apriltagList, self.cbIsApriltagN) # Subscribe to apriltagCenter msg self.apriltagCenterX_topic = "/isApriltag/Center/X" self.apriltagCenterX_sub = rospy.Subscriber(self.apriltagCenterX_topic, apriltagCenter, self.cbIsApriltagCenterX) # Subscribe to apriltagCenter msg self.apriltagCenterY_topic = "/isApriltag/Center/Y" self.apriltagCenterY_sub = rospy.Subscriber(self.apriltagCenterY_topic, apriltagCenter, self.cbIsApriltagCenterY) # Subscribe to CompressedImage msg self.telloCameraInfo_topic = "/tello/camera/camera_info" self.telloCameraInfo_sub = rospy.Subscriber(self.telloCameraInfo_topic, CameraInfo, self.cbCameraInfo) # Subscribe to TelloStatus msg self.telloStatus_topic = "/tello/status" self.telloStatus_sub = rospy.Subscriber(self.telloStatus_topic, TelloStatus, self.cbTelloStatus) # Subscribe to Odometry msg self.telloOdom_topic = "/tello/odom" self.telloOdom_sub = rospy.Subscriber(self.telloOdom_topic, Odometry, self.cbTelloOdometry) # Subscribe to PoseWithCovariance msg self.telloIMU_topic = "/tello/imu" self.telloIMU_sub = rospy.Subscriber(self.telloIMU_topic, Imu, self.cbTelloIMU) # Publish to objCenter msg self.objCoord_topic = "/isApriltag/objCoord" self.objCoord_pub = rospy.Publisher(self.objCoord_topic, objCoord, queue_size=10) # Allow up to one second to connection rospy.sleep(1)
def __init__(self): rospy.init_node('Arduino', log_level=rospy.DEBUG) # Cleanup when termniating the node rospy.on_shutdown(self.shutdown) self.port = rospy.get_param("~port", "/dev/ttyACM0") self.baud = int(rospy.get_param("~baud", 57600)) self.timeout = rospy.get_param("~timeout", 0.5) self.base_frame = rospy.get_param("~base_frame", 'base_link') # Overall loop rate: should be faster than fastest sensor rate self.rate = int(rospy.get_param("~rate", 50)) r = rospy.Rate(self.rate) # Rate at which summary SensorState message is published. Individual sensors publish # at their own rates. self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10)) self.use_base_controller = rospy.get_param("~use_base_controller", False) # Set up the time for publishing the next SensorState message now = rospy.Time.now() self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate) self.t_next_sensors = now + self.t_delta_sensors # Initialize a Twist message self.cmd_vel = Twist() # A cmd_vel publisher so we can stop the robot when shutting down self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) # Initialize the controlller self.controller = Arduino(self.port, self.baud, self.timeout) # Make the connection self.controller.connect() rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud") # get arduino version self.year = 0 self.date = 0 self.year, self.date = self.controller.get_arduino_version() rospy.loginfo("arduino version is " + str(self.year) + str(self.date)) # Reserve a thread lock mutex = thread.allocate_lock() # Initialize the base controller if used if self.use_base_controller: self.myBaseController = BaseController(self.controller, self.base_frame) # Start polling the sensors and base controller while not rospy.is_shutdown(): if self.use_base_controller: mutex.acquire() self.myBaseController.poll() mutex.release() r.sleep()
def __init__(self): rospy.init_node('Follow') rospy.on_shutdown(self.shutdown) rospy.logerr('Let\'s go') self.trace = Concurrence(outcomes=['succeeded', 'aborted'], default_outcome='aborted', outcome_map={ 'succeeded': { 'STOP': 'stop' }, 'aborted': { 'FOLLOW': 'aborted' }, 'aborted': { 'FOLLOW': 'preempted' } }, child_termination_cb=self.child_cb, input_keys=['people_id']) with self.trace: self.meta_follow = StateMachine( ['succeeded', 'aborted', 'preempted'], input_keys=['people_id']) with self.meta_follow: self.meta_follow.userdata.pos_xm = Pose() self.meta_follow.userdata.rec = 0.2 StateMachine.add('FIND', LegTracker().tracker, transitions={ 'invalid': 'WAIT', 'valid': 'FIND', 'preempted': 'preempted' }, remapping={ 'pos_xm': 'pos_xm', 'people_id': 'people_id' }) StateMachine.add('WAIT', Wait(), transitions={ 'succeeded': 'META_NAV', 'error': 'META_NAV' }) self.meta_nav = Concurrence( outcomes=['time_over', 'get_pos', 'aborted'], default_outcome='aborted', outcome_map={ 'time_over': { 'WAIT': 'succeeded' }, 'get_pos': { 'NAV': 'succeeded' }, 'aborted': { 'NAV': 'aborted' } }, child_termination_cb=self.nav_child_cb, input_keys=['pos_xm']) with self.meta_nav: Concurrence.add('NAV', NavStack(), remapping={'pos_xm': 'pos_xm'}) Concurrence.add('WAIT', Wait_trace()) StateMachine.add('META_NAV', self.meta_nav, transitions={ 'get_pos': 'FIND', 'time_over': 'FIND', 'aborted': 'FIND' }) Concurrence.add('FOLLOW', self.meta_follow) Concurrence.add('STOP', CheckStop()) self.sm_GPSR = StateMachine(outcomes=['succeeded', 'aborted', 'error']) with self.sm_GPSR: self.sm_GPSR.userdata.target = list() self.sm_GPSR.userdata.action = list() self.sm_GPSR.userdata.task_num = 0 self.sm_GPSR.userdata.current_task = -1 self.sm_GPSR.userdata.people_id = -1 # StateMachine.add('RUNNODE',RunNode(), # transitions={'succeeded':'XM','aborted':'aborted'}) StateMachine.add( 'XM', self.trace, transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', #'preempted':'error' }) intro_server = IntrospectionServer('sm_gpsr', self.sm_GPSR, '/SM_ROOT') intro_server.start() out = self.sm_GPSR.execute() print out intro_server.stop() self.smach_bool = True
def main(): def state_attrdict(uav_state_msg): e = AttrDict() e.ac_id = uav_state_msg.ac_id e.time = uav_state_msg.header.stamp.to_sec() e.recv_time = rospy.get_rostime().to_sec() e.position = (uav_state_msg.position.x, uav_state_msg.position.y, uav_state_msg.position.z) e.attitude = (uav_state_msg.attitude.phi, uav_state_msg.attitude.psi, uav_state_msg.attitude.theta) return e def compute_stats(state, expected_state): """Compute errors between corresponding UAV states. Arguments: state: expected_state: Rerturns: skyscanner.msg.UAVStats object. """ def angle_diff(a, b): """Calculate the distance between two angles in degrees. Arguments: a: measured angle in degrees b: reference angle in degrees Returns: Distance between a and b including sign """ phi = np.abs(a - b) % 360 sign = 1 if not ((a - b >= 0 and a - b <= 180) or (a - b <= -180 and a - b >= -360)): sign = -1 if phi > 180: result = 360 - phi else: result = phi return result * sign stats = UAVStats() stats.header = Header(stamp=rospy.Time(state.time)) stats.ac_id = state.ac_id stats.heading_error = \ angle_diff(state.attitude[1]*np.pi/180, expected_state.attitude[1]*np.pi/180) * 180/np.pi stats.altitude_error = state.position[2] - expected_state.position[2] p1 = np.array(state.position[:2]) p2 = np.array(expected_state.position[:2]) stats.crosstrack_error = np.linalg.norm(p1 - p2) return stats def get_until(queue, condition): """Generator returning all elements in queue until condition is met. Arguments: queue: Queue.queue condition: fn(e) -> True or False where `e` element of queue. Must return True while condition is being met, False otherwise. """ try: e = queue.get(False) while condition(e): yield e e = queue.get(False) else: with queue.mutex: queue.queue.appendleft(e) except Empty: return def on_state_recv(state): """Handle state reception.""" ac_id = state.ac_id if ac_id not in ac: rospy.logwarn("[stats] ac_id '{}' not recognized".format(ac_id)) return if USE_MATH_TRAJ: publish_stats_math_traj(state) else: publish_stats_exp_pos(state) # else: def publish_stats_math_traj(state): global actual_traj msg_time = state.header.stamp.to_sec() cond = lambda e: msg_time < e.time_limit for s in get_until(ac[ac_id].traj_q, cond): actual_traj = s if actual_traj is None: return np_pos = np.array( [state.position.x, state.position.y, state.position.z]) stats = UAVStats() stats.header = Header(stamp=rospy.Time(msg_time)) stats.ac_id = state.ac_id stats.heading_error = 0 stats.altitude_error = 0 stats.crosstrack_error = actual_traj.crosstrack_error(np_pos)[1] rospy.loginfo(stats) ac[ac_id].stats_pub.publish(stats) def publish_stats_exp_pos(state): msg_time = state.header.stamp.to_sec() cond = lambda e: np.around(msg_time - e.time, decimals=DECIMAL_DIGITS ) >= 0 # Get first element meeting condition discarding the previous ones s = None for s in get_until(ac[ac_id].expected_states_q, cond): pass if s is None: return if s.time != msg_time: rospy.logwarn("[stats] s.time != msg_time '{}!={}'".format( s.time, msg_time)) stats = compute_stats(state_attrdict(state), s) rospy.loginfo(stats) ac[ac_id].stats_pub.publish(stats) def on_expected_state_recv(exp_state): """Handle expected state reception.""" ac_id = exp_state.ac_id if ac_id in ac: ac[ac_id].expected_states_q.put(state_attrdict(exp_state)) else: rospy.logwarn("[stats] ac_id '{}' not recognized".format(ac_id)) def on_trajectory_recv(trajectory_seq): """Handle TrajectorySequence message reception.""" print("trajectory recv") ac_id = trajectory_seq.ac_id if ac_id in ac: for trajectory in trajectory_seq.trajectories: if trajectory.circle is True: cir = Circumference(np.array([ trajectory.origin.x, trajectory.origin.y, trajectory.origin.z ]), -trajectory.radius, timeout=trajectory.duration, time_limit=trajectory.time_limit) ac[ac_id].traj_q.put(cir) else: lin = Segment(np.array([ trajectory.origin.x, trajectory.origin.y, trajectory.origin.z ]), np.array([ trajectory.destination.x, trajectory.destination.y, trajectory.destination.z ]), timeout=trajectory.duration, time_limit=trajectory.time_limit) ac[ac_id].traj_q.put(lin) else: rospy.logwarn("[stats] ac_id '{}' not recognized".format(ac_id)) def on_shutdown(): pass rospy.init_node('stats') rospy.on_shutdown(on_shutdown) # Setup math precision global ABS_TOL, DECIMAL_DIGITS, actual_traj ABS_TOL = rospy.get_param(rospy.search_param('abs_tol'), 1e-3) DECIMAL_DIGITS = int(np.around(np.log10(1. / ABS_TOL))) # rospy.Subscriber('/tick', Clock, on_tick_recv, queue_size=10) # Get aircraft namespaces aircraft_ns = rospy.get_param('aircrafts_ns') ac = {} for ns in aircraft_ns: ac_id = rospy.get_param(rospy.search_param('/'.join([ns, 'ac_id']))) ac[ac_id] = AttrDict() ac[ac_id].name = ns ac[ac_id].expected_states_q = Queue() ac[ac_id].traj_q = Queue() ac[ac_id].state_sub = rospy.Subscriber('/'.join([ns, 'uav_state']), UAVState, on_state_recv, queue_size=1) ac[ac_id].exp_state_sub = rospy.Subscriber('/'.join( [ns, 'expected_uav_state']), UAVState, on_expected_state_recv, queue_size=1) ac[ac_id].exp_state_sub = rospy.Subscriber('/'.join( [ns, 'trajectory_sequence']), TrajectorySequence, on_trajectory_recv, queue_size=1) ac[ac_id].stats_pub = rospy.Publisher('/'.join([ns, 'stats']), UAVStats, queue_size=10) rospy.spin()
def __init__(self): rospy.init_node('nav_test', anonymous=False) rospy.on_shutdown(self.shutdown) # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move base server") rospy.loginfo("Starting navigation test") goal = MoveBaseGoal() quaternion = Quaternion() while 1: #goal 1 goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = 0.0 quaternion.w = 1.0 goal.target_pose.pose.position.x = 2.0 goal.target_pose.pose.position.y = -0.1 goal.target_pose.pose.position.z = 0 goal.target_pose.pose.orientation = quaternion self.move_base.send_goal(goal) # Allow 1 minute to get there finished_within_time = self.move_base.wait_for_result( rospy.Duration(600)) # If we don't get there in time, abort the goal if not finished_within_time: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") else: state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Goal succeeded!") rospy.sleep(5) #goal 2 goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = 0.0 quaternion.w = 1.0 goal.target_pose.pose.position.x = 4.0 goal.target_pose.pose.position.y = -0.1 goal.target_pose.pose.position.z = 0 goal.target_pose.pose.orientation = quaternion self.move_base.send_goal(goal) # Allow 1 minute to get there finished_within_time = self.move_base.wait_for_result( rospy.Duration(600)) # If we don't get there in time, abort the goal if not finished_within_time: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") else: state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Goal succeeded!") rospy.sleep(5)
def __init__(self): rospy.init_node("ur5_vision", anonymous=False) self.bridge = cv_bridge.CvBridge() self.image_sub = rospy.Subscriber('/ur5/usbcam/image_raw', Image, self.image_callback) self.cxy_pub = rospy.Publisher('cxy', Twist, queue_size=10) rospy.loginfo("Starting node moveit_cartesian_path") rospy.on_shutdown(self.cleanup) # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the move group for the ur5_arm self.arm = moveit_commander.MoveGroupCommander('manipulator') # Get the name of the end-effector link end_effector_link = self.arm.get_end_effector_link() # Set the reference frame for pose targets reference_frame = "/base_link" # Set the ur5_arm reference frame accordingly self.arm.set_pose_reference_frame(reference_frame) # Allow replanning to increase the odds of a solution self.arm.allow_replanning(True) # Allow some leeway in position (meters) and orientation (radians) self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.1) # Get the current pose so we can add it as a waypoint start_pose = self.arm.get_current_pose(end_effector_link).pose # Initialize the waypoints list waypoints= [] # Set the first waypoint to be the starting pose # Append the pose to the waypoints list wpose = deepcopy(start_pose) # Set the next waypoint to the right 0.5 meters wpose.position.x = -0.2 wpose.position.y = -0.2 wpose.position.z = 0.3 waypoints.append(deepcopy(wpose)) wpose.position.x = 0.1052 wpose.position.y = -0.4271 wpose.position.z = 0.4005 wpose.orientation.x = 0.4811 wpose.orientation.y = 0.4994 wpose.orientation.z = -0.5121 wpose.orientation.w = 0.5069 waypoints.append(deepcopy(wpose)) if np.sqrt((wpose.position.x-start_pose.position.x)**2+(wpose.position.x-start_pose.position.x)**2 \ +(wpose.position.x-start_pose.position.x)**2)<0.1: rospy.loginfo("Warnig: target position overlaps with the initial position!") # self.arm.set_pose_target(wpose) # Set the internal state to the current state self.arm.set_start_state_to_current_state() # Plan the Cartesian path connecting the waypoints """moveit_commander.move_group.MoveGroupCommander.compute_cartesian_path( self, waypoints, eef_step, jump_threshold, avoid_collisios= True) Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """ plan, fraction = self.arm.compute_cartesian_path(waypoints, 0.01, 0.0, True) # plan = self.arm.plan() # If we have a complete plan, execute the trajectory if 1-fraction < 0.2: rospy.loginfo("Path computed successfully. Moving the arm.") num_pts = len(plan.joint_trajectory.points) rospy.loginfo("\n# intermediate waypoints = "+str(num_pts)) self.arm.execute(plan) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed")
rospy.loginfo('line_detector_node processing first image.') self.nprocessed += 1 def info(self): delta = time.time() - self.t0 if self.nreceived: skipped_perc = (100.0 * self.nskipped / self.nreceived) else: skipped_perc = 0 def fps(x): return '%.1f fps' % (x / delta) m = ('In the last %.1f s: received %d (%s) processed %d (%s) skipped %d (%s) (%1.f%%)' % (delta, self.nreceived, fps(self.nreceived), self.nprocessed, fps(self.nprocessed), self.nskipped, fps(self.nskipped), skipped_perc)) return m if __name__ == '__main__': rospy.init_node('line_detector',anonymous=False) line_detector_node = LineDetectorNode() rospy.on_shutdown(line_detector_node.onShutdown) rospy.spin()
self.time = IntegrationTime() self.find_threshold = rospy.ServiceProxy('/desaturator/find_threshold', Empty) self.save_srv = rospy.Service('~save', Empty, self.save_cb) self.data = open('calibration.dat', 'a') #self.timer = rospy.Timer(rospy.Duration(5), self.timer_cb) def hist_cb(self, msg): for b, l in zip(msg.bins, msg.limits): if b > 10: self.current = (l, self.time.get()) return #def timer_cb(self, event): def save_cb(self, req): rospy.loginfo('Finding threshold...') self.find_threshold() rospy.loginfo('Done, saving %.3f -- %i' % (self.current)) self.data.write('%.3f %i\n' % self.current) return [] def shutdown(self): self.data.close() if __name__ == '__main__': rospy.init_node(NODE) sdn = StoreDataNode() rospy.on_shutdown(lambda: sdn.shutdown()) rospy.spin()
def __init__(self): # Initialize the node # rospy.init_node('my_node_name', anonymous=True)# # The anonymous keyword argument is mainly used for nodes where # you normally expect many of them to be running and # don't care about their names (e.g. tools, GUIs) #see http://wiki.ros.org/rospy/Overview/Initialization%20and%20Shutdown rospy.init_node('doitall_node', anonymous=False) # tell user how to stop TurtleBot and print on Terminal rospy.loginfo("To stop Jakcal CTRL + C") # What function to call when you ctrl + c #rospy.on_shutdown(h) #Register handler to be called when rospy process begins shutdown. #h is a function that takes no arguments. #see http://wiki.ros.org/rospy/Overview/Initialization%20and%20Shutdown rospy.on_shutdown(self.shutdown) self.f1 = open('jackal_ground_truth_coordinates.txt', 'w+') self.f2 = open('jackal_amcl_coordinates.txt', 'w+') #with open('jackal_ground_truth_coordinates.txt', 'r+') as self.f1: pass #with open('jackal_amcl_coordinates.txt', 'r+') as self.f2: pass # Subscribe to the amcl_pose topic and set # the appropriate callbacks # see http://answers.ros.org/question/49282/amcl-and-laser_scan_matcher/ # see http://answers.ros.org/question/185907/help-with-some-code-send-a-navigation-goal/ #self.link_states = rospy.Subscriber('/gazebo/link_states', LinkStates, self.link_stateCallback) # Subscribe to the amcl_pose topic and set # the appropriate callbacks # see http://answers.ros.org/question/49282/amcl-and-laser_scan_matcher/ # see http://answers.ros.org/question/185907/help-with-some-code-send-a-navigation-goal/ self.model_states = rospy.Subscriber('/gazebo/model_states', ModelStates, self.model_stateCallback) # Subscribe to the amcl_pose topic and set # the appropriate callbacks # see http://answers.ros.org/question/49282/amcl-and-laser_scan_matcher/ # see http://answers.ros.org/question/185907/help-with-some-code-send-a-navigation-goal/ self.amcl_sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.amclPoseCallback) # Create publisher for cmd_vel topic # cmd_vel publisher can "talk" to Jackal and tell it to move self.cmd_vel = rospy.Publisher('/jackal_velocity_controller/cmd_vel', Twist, queue_size=10) # Create publisher for amcl initial pose # set the initial pose and covariance self.amcl_initialpose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=10) #initialize coordinates list self.jackal_amcl_coordinates = [] self.jackal_amcl_timestamps = [] #initialize coordinates list self.jackal_coordinates = [] self.jackal_timestamps = [] #injecing initial pose try: self.inject_initial_pose = sys.argv[3] except: self.inject_initial_pose = 'False' # Publishing rate = 100 Hz look for odometry #for sleeping and rate see #http://wiki.ros.org/rospy/Overview/Time self.rate = rospy.Rate(20)
detect_dict = {} # We consider that when there is a big Z axis component there has been a very bif front crash detection_dict = { "front": (message == "front" or message == "up" or message == "down"), "left": message == "left", "right": message == "right", "back": message == "back" } return detection_dict if __name__ == "__main__": rospy.init_node('imu_topic_subscriber', log_level=rospy.INFO) imu_reader_object = ImuTopicReader() rospy.loginfo(imu_reader_object.get_imu_data()) rate = rospy.Rate(0.5) # Twice per second ctrl_c = False def shutdownhook(): # WORKS BETTER THAN THE rospy.is_shut_down() global ctrl_c print "shutdown time!" ctrl_c = True rospy.on_shutdown(shutdownhook) while not ctrl_c: data = imu_reader_object.get_imu_data() rospy.loginfo(data) rate.sleep()
def __init__(self): # 设置rospy在终止节点时执行的关闭函数 rospy.on_shutdown(self.shutdown) # 获取参数 self.goal_distance = rospy.get_param("~goal_distance", 1.0) # meters self.goal_angle = radians(rospy.get_param( "~goal_angle", 90)) # degrees converted to radians self.linear_speed = rospy.get_param("~linear_speed", 0.2) # meters per second self.angular_speed = rospy.get_param("~angular_speed", 0.7) # radians per second self.angular_tolerance = radians( rospy.get_param("~angular_tolerance", 2)) # degrees to radians self.velocity_topic = rospy.get_param( '~vel_topic', '/mobile_base/mobile_base_controller/cmd_vel') self.rate_pub = rospy.get_param('~velocity_pub_rate', 20) self.rate = rospy.Rate(self.rate_pub) # 初始化发布速度的topic self.cmd_vel = rospy.Publisher(self.velocity_topic, Twist, queue_size=1) self.base_frame = rospy.get_param('~base_frame', '/base_link') #set the base_frame self.odom_frame = rospy.get_param('~odom_frame', '/odom') #set the odom_frame # 初始化tf self.tf_listener = tf.TransformListener() rospy.sleep(2) try: self.tf_listener.waitForTransform(self.base_frame, self.odom_frame, rospy.Time(0), rospy.Duration(5.0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logerr( 'tf catch exception when robot waiting for transform......') exit(-1) # 循环四次画出正方形 for i in range(4): self.Linear_motion() # 再进行旋转之前时机器人停止 move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1.0) self.Rotational_motion() move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1.0) #结束时使机器人停止 self.cmd_vel.publish(Twist())