我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用rospy.on_shutdown()。
def __init__(self,bool_direction): print "Beginning wall follow" #setup the node rospy.init_node('wall_follower', anonymous=False) rospy.on_shutdown(self.shutdown) self.right = bool_direction # node specific topics (remap on command line or in launch file) self.drive = rospy.Publisher('/vesc/ackermann_cmd_mux/input/navigation', AckermannDriveStamped, queue_size=5) #sets the subscriber rospy.Subscriber('scan', LaserScan, self.laserCall) rospy.Subscriber('blob_info', blob_detect,self.blobCall) rospy.spin() # always make sure to leave the robot stopped self.drive.publish(AckermannDriveStamped())
def main(): """Intera RSDK Joint Velocity Example: Wobbler Commands joint velocities of randomly parameterized cosine waves to each joint. Demonstrates Joint Velocity Control Mode. """ arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) parser.parse_args(rospy.myargv()[1:]) print("Initializing node... ") rospy.init_node("rsdk_joint_velocity_wobbler") wobbler = Wobbler() rospy.on_shutdown(wobbler.clean_shutdown) wobbler.wobble() print("Done.")
def main(): """RSDK Head Example: Wobbler Nods the head and pans side-to-side towards random angles. Demonstrates the use of the intera_interface.Head class. """ arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) parser.parse_args(rospy.myargv()[1:]) print("Initializing node... ") rospy.init_node("rsdk_head_wobbler") wobbler = Wobbler() rospy.on_shutdown(wobbler.clean_shutdown) print("Wobbling... ") wobbler.wobble() print("Done.")
def start_server(limb, rate, mode, valid_limbs): rospy.loginfo("Initializing node... ") rospy.init_node("sdk_{0}_joint_trajectory_action_server{1}".format( mode, "" if limb == 'all_limbs' else "_" + limb,)) rospy.loginfo("Initializing joint trajectory action server...") robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize() config_module = "intera_interface.cfg" if mode == 'velocity': config_name = ''.join([robot_name,"VelocityJointTrajectoryActionServerConfig"]) elif mode == 'position': config_name = ''.join([robot_name,"PositionJointTrajectoryActionServerConfig"]) else: config_name = ''.join([robot_name,"PositionFFJointTrajectoryActionServerConfig"]) cfg = importlib.import_module('.'.join([config_module,config_name])) dyn_cfg_srv = Server(cfg, lambda config, level: config) jtas = [] if limb == 'all_limbs': for current_limb in valid_limbs: jtas.append(JointTrajectoryActionServer(current_limb, dyn_cfg_srv, rate, mode)) else: jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode)) def cleanup(): for j in jtas: j.clean_shutdown() rospy.on_shutdown(cleanup) rospy.loginfo("Joint Trajectory Action Server Running. Ctrl-c to quit") rospy.spin()
def __init__(self): #?????????? m/s self.move_cmd_pub = rospy.Publisher('cmd_move_robot',g_msgs.Twist,queue_size=100) self.move_speed = config.go_along_circle_speed self.get_position = robot_state.robot_position_state() #????????? rad self.stop_tolerance = config.go_along_circle_angular_tolerance #???????? rospy.on_shutdown(self.brake) # ??sleep ??? ??????????? self.rate = 100.0 self.R = rospy.Rate(int(self.rate)) self.MODE = { 1:(-1, 1), 2:( 1,-1), 3:( 1, 1), 4:(-1,-1)}
def turn_to(self,goal_angular): rospy.on_shutdown(self.brake) #??????????? current_angular = start_angular = self.robot_state.get_robot_current_w()#?????????? is_arrive_goal = False r = rospy.Rate(100) delta_angular = current_angular - start_angular delta_upper_limit = abs(goal_angular) + 0.02 #???? delta_lower_limit = abs(goal_angular) - 0.02 #???? move_velocity = g_msgs.Twist() while not rospy.is_shutdown() and not is_arrive_goal: if abs(delta_angular)<=delta_upper_limit and abs(delta_angular) >= delta_lower_limit: #???? self.brake() is_arrive_goal = True break current_angular = self.robot_state.get_robot_current_w() if goal_angular > 0: move_velocity.angular.z = 0.1 else: move_velocity.angular.z = -0.1 delta_angular += abs(abs(current_angular) - abs(start_angular) ) start_angular = current_angular self.cmd_move_pub.publish(move_velocity) #??????????? r.sleep() self.brake()
def __init__(self): self.find_ball_client = rospy.ServiceProxy('volleyball_data',volleyballdata) self.cmd_vel_pub = rospy.Publisher('cmd_move_robot' , g_msgs.Twist , queue_size=100) self.cmd_position = get_robot_position.robot_position_state() self.move_speed = 0.21 #???????? rospy.on_shutdown(self.brake) #???????????? self.MODE = { 1:(-1, 1), 2:( 1,-1), 3:( 1, 1), 4:(-1,-1)} rospy.loginfo('waiting for the find ball..') self.find_ball_client.wait_for_service() rospy.loginfo('connect to the find ball!!!') #??????????????????
def __init__(self): self.node_name = "face_recog_fisher" rospy.init_node(self.node_name) rospy.on_shutdown(self.cleanup) self.bridge = CvBridge() self.face_names = StringArray() self.all_names = StringArray() self.size = 4 face_haar = 'haarcascade_frontalface_default.xml' self.haar_cascade = cv2.CascadeClassifier(face_haar) self.face_dir = 'face_data_fisher' 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 = "train_faces_eigen" rospy.init_node(self.node_name) rospy.on_shutdown(self.cleanup) self.bridge = CvBridge() self.size = 4 face_haar = 'haarcascade_frontalface_default.xml' self.haar_cascade = cv2.CascadeClassifier(face_haar) self.face_dir = 'face_data_eigen' self.face_name = sys.argv[1] self.path = os.path.join(self.face_dir, self.face_name) # self.model = cv2.createFisherFaceRecognizer() self.model = cv2.createEigenFaceRecognizer() self.cp_rate = 5 if not os.path.isdir(self.path): os.mkdir(self.path) self.count = 0 self.train_img_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.img_callback) # self.train_img_pub = rospy.Publisher('train_face', Image, queue_size=10) rospy.loginfo("Capturing data...")
def __init__(self): self.node_name = "hand_gestures" rospy.init_node(self.node_name) rospy.on_shutdown(self.cleanup) # self.cv_window_name = self.node_name # cv2.namedWindow("Depth Image", 1) # cv2.moveWindow("Depth Image", 20, 350) self.bridge = CvBridge() self.numFingers = RecognizeNumFingers() self.depth_sub = rospy.Subscriber("/asus/depth/image_raw", Image, self.depth_callback) self.num_pub = rospy.Publisher('num_fingers', Int32, queue_size=10, latch=True) # self.img_pub = rospy.Publisher('hand_img', Image, queue_size=10) rospy.loginfo("Waiting for image topics...")
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("waiting 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 tell it to move to a pose that's near the docking station self.goal = MoveBaseGoal() self.goal.target_pose.header.frame_id = 'odom' self.goal.target_pose.header.stamp = rospy.Time.now()
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.marker_id = rospy.get_param("~marker_id", 12) self.frame_id = rospy.get_param("~frame_id", "odom_meas") self.camera_frame_id = rospy.get_param("~camera_frame_id", "overhead_cam_frame") self.count = rospy.get_param("~count", 50) # local vars: self.calibrate_flag = False self.calibrated = False self.calibrate_count = 0 self.kb = kbhit.KBHit() self.trans_arr = np.zeros((self.count, 3)) self.quat_arr = np.zeros((self.count, 4)) self.trans_ave = np.zeros(3) self.quat_ave = np.zeros(3) # now create subscribers, timers, and publishers self.br = tf.TransformBroadcaster() self.kb_timer = rospy.Timer(rospy.Duration(0.1), self.keycb) rospy.on_shutdown(self.kb.set_normal_term) self.alvar_sub = rospy.Subscriber("ar_pose_marker", AlvarMarkers, self.alvarcb) return
def __init__(self,dis=0.0): State.__init__(self, outcomes=['succeeded','preempted','aborted'],input_keys=['target']) rospy.on_shutdown(self.shutdown) self.test_distance = target.y-dis self.rate = 200 self.r = rospy.Rate(self.rate) self.speed = rospy.get_param('~speed',0.08) self.tolerance = rospy.get_param('~tolerance', 0.01) self.cmd_vel = rospy.Publisher('/mobile_base_controller/smooth_cmd_vel',Twist,queue_size=5) self.base_frame = rospy.get_param('~base_frame', '/base_link') self.odom_frame = rospy.get_param('~odom_frame', '/odom') self.tf_listener = tf.TransformListener() rospy.sleep(1) self.tf_listener.waitForTransform(self.odom_frame,self.base_frame,rospy.Time(),rospy.Duration(60.0)) #define a bianliang self.flag = rospy.get_param('~flag', True) rospy.loginfo(self.test_distance) #tf get position #publish cmd_vel
def __init__(self,angle=0): State.__init__(self, outcomes=['succeeded','aborted','preempted'],input_keys=["angle_in"]) self.speed = rospy.get_param('~speed', 0.10) self.secretindigal = 1.0 self.tolerance = rospy.get_param('tolerance', math.radians(5)) self.start = True self.cmd_vel = rospy.Publisher('mobile_base_controller/smooth_cmd_vel', Twist, queue_size=5) self.base_frame = rospy.get_param('~base_frame', '/base_link') self.odom_frame = rospy.get_param('~odom_frame', '/odom') rospy.on_shutdown(self.shutdown) self.rate = 30 self.start_test = True self.r = rospy.Rate(self.rate) self.tf_listener = tf.TransformListener() rospy.sleep(0.5) self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60))
def __init__(self, angle=0): State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'], input_keys=["angle_in"]) self.speed = rospy.get_param('~speed', 0.03) self.secretindigal = 1.0 self.tolerance = rospy.get_param('tolerance', math.radians(5)) self.start = True self.cmd_vel = rospy.Publisher('mobile_base_controller/smooth_cmd_vel', Twist, queue_size=5) self.base_frame = rospy.get_param('~base_frame', '/base_link') self.odom_frame = rospy.get_param('~odom_frame', '/odom') rospy.on_shutdown(self.shutdown) self.rate = 30 self.start_test = True self.r = rospy.Rate(self.rate) self.angle=angle self.tf_listener = tf.TransformListener() rospy.sleep(0.5) self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60))
def __init__(self,dis=0): State.__init__(self, outcomes=['succeeded','preempted','aborted']) rospy.on_shutdown(self.shutdown) self.test_distance = dis self.rate = 100 self.r = rospy.Rate(self.rate) self.speed = rospy.get_param('~speed',0.08) self.tolerance = rospy.get_param('~tolerance', 0.01) self.cmd_vel = rospy.Publisher('/mobile_base_controller/smooth_cmd_vel',Twist,queue_size=5) self.base_frame = rospy.get_param('~base_frame', '/base_link') self.odom_frame = rospy.get_param('~odom_frame', '/odom') self.tf_listener = tf.TransformListener() rospy.sleep(1) self.tf_listener.waitForTransform(self.odom_frame,self.base_frame,rospy.Time(),rospy.Duration(60.0)) self.flag = rospy.get_param('~flag', True) #tf get position self.position = Point() self.position = self.get_position() self.y_start = self.position.y self.x_start = self.position.x #publish cmd_vel
def __init__(self,dis=0.0): State.__init__(self, outcomes=['succeeded','preempted','aborted']) rospy.on_shutdown(self.shutdown) self.test_distance = target.y-0.0 self.rate = 100 self.r = rospy.Rate(self.rate) self.speed = rospy.get_param('~speed',0.08) self.tolerance = rospy.get_param('~tolerance', 0.01) self.cmd_vel = rospy.Publisher('/mobile_base_controller/smooth_cmd_vel',Twist,queue_size=5) self.base_frame = rospy.get_param('~base_frame', '/base_link') self.odom_frame = rospy.get_param('~odom_frame', '/odom') self.tf_listener = tf.TransformListener() rospy.sleep(1) self.tf_listener.waitForTransform(self.odom_frame,self.base_frame,rospy.Time(),rospy.Duration(60.0)) #define a bianliang self.flag = rospy.get_param('~flag', True) rospy.loginfo(self.test_distance) #tf get position #publish cmd_vel
def __init__(self,angle=0): State.__init__(self, outcomes=['succeeded','aborted','preempted']) self.speed = rospy.get_param('~speed', 0.20) self.secretindigal = 1.0 self.tolerance = rospy.get_param('tolerance', math.radians(5)) self.start = True self.cmd_vel = rospy.Publisher('mobile_base_controller/smooth_cmd_vel', Twist, queue_size=5) self.base_frame = rospy.get_param('~base_frame', '/base_link') self.odom_frame = rospy.get_param('~odom_frame', '/odom') rospy.on_shutdown(self.shutdown) self.rate = 30 self.start_test = True self.r = rospy.Rate(self.rate) self.angle = angle self.tf_listener = tf.TransformListener() rospy.sleep(0.5) self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60))
def __init__(self): # Holds the current drone status self.status = -1 # Subscribe to the /ardrone/navdata topic, of message type navdata, and call self.ReceiveNavdata when a message is received self.subNavdata = rospy.Subscriber('/ardrone/navdata',Navdata,self.ReceiveNavdata) # Allow the controller to publish to the /ardrone/takeoff, land and reset topics self.pubLand = rospy.Publisher('/ardrone/land',Empty) self.pubTakeoff = rospy.Publisher('/ardrone/takeoff',Empty) self.pubReset = rospy.Publisher('/ardrone/reset',Empty) # Allow the controller to publish to the /cmd_vel topic and thus control the drone self.pubCommand = rospy.Publisher('/cmd_vel',Twist) # Setup regular publishing of control packets self.command = Twist() self.commandTimer = rospy.Timer(rospy.Duration(COMMAND_PERIOD/1000.0),self.SendCommand) # Land the drone if we are shutting down rospy.on_shutdown(self.SendLand)
def __init__(self): #constants for racecar speed and angle calculations self.pSpeed = 0.3 #tweak self.pAngle = 1 #positive charge behind racecar to give it a "kick" (forward vector) self.propelling_charge = 4.5 #more constants self.charge = 0.01 self.safety_threshold = 0.3 self.speeds = [1] #Creates a list of speeds self.stuck_time = 0 self.stuck = False self.stuck_threshold = 2 rospy.init_node("explorer") self.sub = rospy.Subscriber('scan', LaserScan, self.laser_callback) self.pub = rospy.Publisher('/vesc/ackermann_cmd_mux/input/navigation', AckermannDriveStamped) rospy.on_shutdown(self.shutdown)
def __init__(self): rospy.init_node('nav_asr', anonymous = True) rospy.Subscriber("/recognizer/output", String, self.SpeechUpdateGoal) rospy.Subscriber("/WPsOK", String, self.GetWayPoints) self.waypoint_list = dict() self.marker_list = list() self.makerArray = MarkerArray() self.makerArray_pub = rospy.Publisher("Waypoint_Array",MarkerArray,queue_size=5) rospy.on_shutdown(self.shutdown) # @@@@ # How long in seconds should the robot pause at each location? self.rest_time = rospy.get_param("~rest_time", 2) # Are we running in the fake simulator? self.fake_test = rospy.get_param("~fake_test", True) # Publisher to manually control the robot (e.g. to stop it, queue_size=5) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) # 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") # Create the waypoints list from txt
def main(): """SDK Joint Position Waypoints Example Records joint positions each time the navigator 'OK/wheel' button is pressed. Upon pressing the navigator 'Rethink' button, the recorded joint positions will begin playing back in a loop. """ arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) parser.add_argument( '-s', '--speed', default=0.3, type=float, help='joint position motion speed ratio [0.0-1.0] (default:= 0.3)' ) parser.add_argument( '-a', '--accuracy', default=intera_interface.settings.JOINT_ANGLE_TOLERANCE, type=float, help='joint position accuracy (rad) at which waypoints must achieve' ) args = parser.parse_args(rospy.myargv()[1:]) print("Initializing node... ") rospy.init_node("sdk_joint_position_waypoints", anonymous=True) waypoints = Waypoints(args.speed, args.accuracy) # Register clean shutdown rospy.on_shutdown(waypoints.clean_shutdown) # Begin example program waypoints.record() waypoints.playback()
def __init__(self, name): """ :param name: the name of the ROS node """ super(NaoqiNode, self).__init__() # A distutils.version.LooseVersion that contains the current verssion of NAOqi we're connected to self.__naoqi_version = None self.__name = name ## NAOqi stuff # dict from a modulename to a proxy self.__proxies = {} # Initialize ros, before getting parameters. rospy.init_node(self.__name) # If user has set parameters for ip and port use them as default default_ip = rospy.get_param("~pip", "127.0.0.1") default_port = rospy.get_param("~pport", 9559) # get connection from command line: from argparse import ArgumentParser parser = ArgumentParser() parser.add_argument("--pip", dest="pip", default=default_ip, help="IP/hostname of parent broker. Default is 127.0.0.1.", metavar="IP") parser.add_argument("--pport", dest="pport", default=default_port, type=int, help="port of parent broker. Default is 9559.", metavar="PORT") import sys args, unknown = parser.parse_known_args(args=rospy.myargv(argv=sys.argv)[1:]) self.pip = args.pip self.pport = args.pport ## ROS stuff self.__stop_thread = False # make sure that we unregister from everything when the module dies rospy.on_shutdown(self.__on_ros_shutdown)
def run(self): """ This function is the main run loop.""" rospy.on_shutdown(self.stop) while not rospy.is_shutdown(): if self.twist: self.publisher.publish(self.twist) self.r.sleep()
def __init__(self): self.publishers = {} rospy.on_shutdown(self.shutdown) # Appends a value to a rosparam list
def start_turn(self , goal_angular = 0.0): rospy.loginfo('[robot_move_pkg]->move_an_angular will turn %s'%goal_angular) rospy.on_shutdown(self.brake) #??????????? current_angular = start_angular = self.robot_state.get_robot_current_w()#?????????? is_arrive_goal = False r = rospy.Rate(100) delta_angular = current_angular - start_angular delta_upper_limit = abs(goal_angular) + self.stop_tolerance #???? delta_lower_limit = abs(goal_angular) - self.stop_tolerance #???? move_velocity = g_msgs.Twist() while not rospy.is_shutdown() and not is_arrive_goal: if abs(delta_angular)<=delta_upper_limit and abs(delta_angular) >= delta_lower_limit: #???? self.brake() is_arrive_goal = True break current_angular = self.robot_state.get_robot_current_w() if goal_angular > 0: move_velocity.angular.z = self.speed else: move_velocity.angular.z = -self.speed delta_angular += abs(abs(current_angular) - abs(start_angular) ) start_angular = current_angular self.cmd_vel_pub.publish(move_velocity) #??????????? r.sleep() self.brake() #???????. ?????
def move_to(self, x = 0.0, y = 0.0, yaw = 0.0): ''' ???????? ''' rospy.on_shutdown(self.brake) #??????????? rospy.loginfo('[robot_move_pkg]->move_in_robot will move to x_distance = %s' 'y_distance = %s, angular = %s'%(x, y, yaw)) if x == 0.0 and y == 0: self.turn(self.normalize_angle(yaw)) else: self.turn(self.normalize_angle(yaw)) self.start_run(x, y)
def main(): """RSDK Joint Torque Example: Joint Springs Moves the default limb to a neutral location and enters torque control mode, attaching virtual springs (Hooke's Law) to each joint maintaining the start position. Run this example and interact by grabbing, pushing, and rotating each joint to feel the torques applied that represent the virtual springs attached. You can adjust the spring constant and damping coefficient for each joint using dynamic_reconfigure. """ # Querying the parameter server to determine Robot model and limb name(s) rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize() # Parsing Input Arguments arg_fmt = argparse.ArgumentDefaultsHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt) parser.add_argument( "-l", "--limb", dest="limb", default=valid_limbs[0], choices=valid_limbs, help='limb on which to attach joint springs' ) args = parser.parse_args(rospy.myargv()[1:]) # Grabbing Robot-specific parameters for Dynamic Reconfigure config_name = ''.join([robot_name,"JointSpringsExampleConfig"]) config_module = "intera_examples.cfg" cfg = importlib.import_module('.'.join([config_module,config_name])) # Starting node connection to ROS print("Initializing node... ") rospy.init_node("sdk_joint_torque_springs_{0}".format(args.limb)) dynamic_cfg_srv = Server(cfg, lambda config, level: config) js = JointSprings(limb=args.limb) # register shutdown callback rospy.on_shutdown(js.clean_shutdown) js.attach_springs()
def __init__(self): """Initializes a controller for the robot""" print("Initializing node... ") rospy.init_node("sawyer_custom_controller") rospy.on_shutdown(self.clean_shutdown) rs = intera_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled print("Robot enabled...") self.limb = intera_interface.Limb("right") try: self.gripper = intera_interface.Gripper("right") except: self.has_gripper = False rospy.logerr("Could not initalize the gripper.") else: self.has_gripper = True self.joint_names = self.limb.joint_names() print("Done initializing controller.") # set gripper try: self.gripper = intera_interface.Gripper("right") except ValueError: rospy.logerr("Could not detect a gripper attached to the robot.") return self.gripper.calibrate() self.gripper.set_velocity(self.gripper.MAX_VELOCITY) #"set 100% velocity"), self.gripper.open()
def main(): """Joint Torque Example: Joint Springs Moves the specified limb in torque control mode, attaching virtual springs (Hooke's Law) to each joint maintaining the start position. """ arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) parser.add_argument( '-l', '--limb', dest='limb', required=True, choices=['left', 'right'], help='limb on which to attach joint springs' ) args = parser.parse_args(rospy.myargv()[1:]) print("Initializing node... ") rospy.init_node("rsdk_joint_torque_springs_%s" % (args.limb,)) dynamic_cfg_srv = Server(JointSpringsExampleConfig, lambda config, level: config) js = JointSprings(args.limb, dynamic_cfg_srv) # register shutdown callback rospy.on_shutdown(js.clean_shutdown) js._get_data() # get data from file js.move_to_neutral() #while not rospy.is_shutdown() and js.i<len(js.data): while len(js.data) and not rospy.is_shutdown(): js.attach_springs()
def __init__(self): self.node_name = "move_tbot" rospy.init_node(self.node_name) rospy.on_shutdown(self._shutdown) self.bridge = CvBridge() self.turn = Twist() self.move = GoToPose() # self.face_recog_file = FaceRecognition() # self.get_person_data = GetPersonData() self.qr_data = [] self.all_face_names = [] self.face_names = [] self.counter = 0 self.qr_sub = rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.qr_callback) self.odom_sub = rospy.Subscriber('odom', Odometry, self.odom_callback) self.num_fingers_sub = rospy.Subscriber('num_fingers', Int32, self.num_fingers_callback) # self.hand_img_sub = rospy.Subscriber('hand_img', Image, self.hand_img_callback) # self.face_img_sub = rospy.Subscriber('face_img', Image, self.face_img_callback) self.face_name_sub = rospy.Subscriber('face_names', StringArray, self.face_names_callback) self.all_face_names_sub = rospy.Subscriber('all_face_names', StringArray, self.all_face_names_callback) self.turn_pub = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) self.rate = rospy.Rate(10) while not rospy.is_shutdown(): self.run_tbot_routine()
def __init__(self, topic_name, data_type, callback=None): self.data_type = data_type self.topic = UDPSetup(topic_name, data_type) if callback is None: self.local_pub = rospy.Publisher(topic_name, data_type, queue_size=10) else: self.callback = callback self.server = UDPHandlerServer(self.__handle_callback, ("0.0.0.0", self.topic.port), socketserver.BaseRequestHandler) self.t = threading.Thread(target = self.server.serve_forever) self.t.start() rospy.on_shutdown(self.shutdown)
def __init__(self): rospy.init_node("speech") rospy.on_shutdown(self.shutdown) self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait up to 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'base_link' nav_goal.target_pose.header.stamp = rospy.Time.now() q_angle = quaternion_from_euler(0, 0,0, axes='sxyz') q = Quaternion(*q_angle) nav_goal.target_pose.pose =Pose( Point(0.3,0,0),q) move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(60.0), server_wait_timeout=rospy.Duration(60.0)) smach_top=StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"]) with smach_top: StateMachine.add("Wait_4_Statr", MonitorState("task_comming", xm_Task, self.start_cb), transitions={'invalid':'NAV', 'valid':'Wait_4_Statr', 'preempted':'NAV'}) StateMachine.add("NAV", move_base_state, transitions={"succeeded":"Wait_4_Stop","aborted":"NAV","preempted":"Wait_4_Stop"}) StateMachine.add("Wait_4_Stop",MonitorState("task_comming", xm_Task, self.stop_cb), transitions={'invalid':'', 'valid':'Wait_4_Stop', 'preempted':''}) smach_top.execute()
def __init__(self): global target rospy.init_node('object', anonymous=False) rospy.on_shutdown(self.shutdown) grasp_sm=StateMachine(outcomes=['succeeded','aborted','preempted']) self.target=Point gripper_goal = xm_GripperCommandGoal() gripper_goal.command.position = 0.10 gripper_goal.command.torque = 0.5 # gripper_goal.header.frame gripper_state = SimpleActionState('xm_move_gripper', xm_GripperCommandAction, goal=gripper_goal, result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(10), server_wait_timeout=rospy.Duration(10)) # # self.lift=actionlib.SimpleActionClient('xm_move_arm_height', xm_ArmHeightAction) # arm_goal = xm_ArmHeightGoal() # arm_goal.distance_x = 0.5 # arm_goal.distance_z = 0.1 # self.arm_state=SimpleActionState('xm_move_gripper',xm_ArmHeightAction,goal=arm_goal, # result_cb=self.arm_state_cb, exec_timeout=rospy.Duration(10), # server_wait_timeout=rospy.Duration(10)) change2arm=ChangeMode("arm") change2base=ChangeMode("base") rospy.loginfo("ready") with grasp_sm: # StateMachine.add("INIT",ServiceState('xm_move_arm_position/move_position', xm_ArmPosition,"init",response_cb=self.responce_cb),transitions={'succeeded':'FIND_OBJECT'}) StateMachine.add("FIND_OBJECT", ServiceState("Find_Object", xm_ObjectDetect,1, response_cb=self.find_object_cb), transitions={"succeeded":"BACK",'aborted':'FIND_OBJECT'}) StateMachine.add("BACK", Forword_BacK(dis= -0.5), transitions={'succeeded':'CHANGE_2_ARM1'}) StateMachine.add("CHANGE_2_ARM1",change2arm,transitions={'succeeded':"READY"}) StateMachine.add("READY",ServiceState('xm_move_arm_position/move_position', xm_ArmPosition,"prepare",response_cb=self.responce_cb),transitions={'succeeded':'CHANGE_2_BASE1'}) StateMachine.add("CHANGE_2_BASE1",change2base,transitions={"succeeded":"ALIGN_Y"}) StateMachine.add("ALIGN_Y",Left_Right(dis=target.y-0),transitions= {"succeeded" :"CHANGE_2_ARM2"}) StateMachine.add("CHANGE_2_ARM2",change2arm,transitions={'succeeded':"OPEN_GRIPPER"}) StateMachine.add("OPEN_GRIPPER",gripper_state,transitions={'succeeded':'CHANGE_2_BASE2'}) StateMachine.add("CHANGE_2_BASE2",change2base,transitions={'succeeded':""}) outcome=grasp_sm.execute("FIND_OBJECT")
def main(): print("Initializing node... ") rospy.init_node("baxter_dqn_ros") baxter_manipulator = BaxterManipulator() # Load Gazebo Models via Spawning Services # Note that the models reference is the /world frame baxter_manipulator._reset() load_gazebo_models() baxter_manipulator.listener() # Remove models from the scene on shutdown rospy.on_shutdown(delete_gazebo_models()) rospy.on_shutdown(baxter_manipulator.clean_shutdown())
def planar_grasp(arm, gripper, target_x, target_y, initial_z, target_z, target_theta, grasp_range_threshold=0.13, n_steps=20): # build trajectory traj = Trajectory('left') rospy.on_shutdown(traj.stop) current_angles = [arm.joint_angle(joint) for joint in arm.joint_names()] traj.add_point(current_angles, 0.0) for i, joint in enumerate(get_ik_joints(target_x, target_y, initial_z, target_z, target_theta, n_steps)): traj.add_point(joint.position, 1. + 0.1 * i) global sub def near_object(msg): # print 'range', msg.range if msg.range < grasp_range_threshold: global sub sub.unregister() print 'near object' traj.stop() gripper.close() # wait for the gripper to close rospy.sleep(1.) # lift arm traj2 = Trajectory('left') s = arm.joint_angles() current_z = arm.endpoint_pose()['position'].z ss = [s[x] for x in traj2._goal.trajectory.joint_names] traj2.add_point(ss, 0.) for j, joint in enumerate(get_ik_joints(target_x, target_y, current_z, initial_z, target_theta, 10)): traj2.add_point(joint.position, 0.2 * j + 1.) traj2.start() sub = rospy.Subscriber('/robot/range/left_hand_range/state', Range, near_object) # move arm traj.start() return traj
def __init__(self,head_controller=None): self.hc = head_controller self.untucker = Tuck(False) self.tucker = Tuck(True) # rospy.on_shutdown(self.tucker.clean_shutdown)
def __init__(self): self.save_path = os.path.join(rospy.get_param("~save_path"), time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") self.trajectory = LineTrajectory("/built_trajectory") # receive either goal points or clicked points self.publish_point_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_point_callback, queue_size=1) self.nav_goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.clicked_point_callback, queue_size=1) # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory)
def __init__(self): rospy.init_node('final_project_kl', anonymous=False) rospy.on_shutdown(self.shutdown) self.keyPointManager = KeyPointManager() # Create the nav_patrol state machine using a Concurrence container self.nav_patrol = Concurrence(outcomes=['succeeded', 'key_points', 'stop'], default_outcome='succeeded', outcome_map = {'key_points' : {'MONITOR_AR':'invalid'}}, child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add the sm_nav machine and a AR Tag MonitorState to the nav_patrol machine with self.nav_patrol: Concurrence.add('SM_NAV', Patrol().getSM()) Concurrence.add('MONITOR_AR',MonitorState('/ar_pose_marker', AlvarMarkers, self.ar_cb)) # Create the top level state machine self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) self.sm_top.userdata.sm_ar_tag = None with self.sm_top: StateMachine.add('PATROL', self.nav_patrol, transitions={'succeeded':'PATROL', 'key_points':'PATROL_KEYPOINTS', 'stop':'STOP'}) StateMachine.add('PATROL_KEYPOINTS', PatrolThroughKeyPoints(self.keyPointManager).getSM(), transitions={'succeeded':'STOP'}) StateMachine.add('STOP', Stop(), transitions={'succeeded':''}) intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop()
def __init__(self): #constants for racecar speed and angle calculations self.pSpeed = 2 #tweak self.pAngle = .7 #positive charge behind racecar to give it a "kick" (forward vector) self.propelling_charge = 5 #more constants self.charge = 0.01 self.safety_threshold = 0.3 self.speeds = [1] #Creates a list of speeds self.stuck_time = 0 self.stuck = False self.stuck_threshold = 2 self.e1=0 rospy.init_node("explorer") self.sub = rospy.Subscriber('scan', LaserScan, self.laser_callback) self.pub = rospy.Publisher('/vesc/ackermann_cmd_mux/input/navigation', AckermannDriveStamped) rospy.on_shutdown(self.shutdown)
def __init__(self): global scan rospy.init_node('accel', anonymous=False) rospy.on_shutdown(self.shutdown) # node specific topics (remap on command line or in launch file) self.drive = rospy.Publisher('/vesc/ackermann_cmd_mux/input/navigation', AckermannDriveStamped, queue_size=5) rate = 10 # messages/sec (also determines latency) r = rospy.Rate(rate) # determine duration to run based on desired speed and distance speed = rospy.get_param('~speed', 2.0) # meters/sec accel = rospy.get_param('~accel', 2.0) # meters/sec^2 delta_speed = accel / rate # maximum speed increment drive_time = 5.0 # seconds drive_cmd = AckermannDriveStamped() drive_cmd.drive.speed = 0.0 last_speed = 0.0 ticks = int(drive_time * rate) # convert drive time to ticks for t in range(ticks): limit_speed = speed if limit_speed - last_speed > delta_speed: limit_speed = last_speed + delta_speed drive_cmd.drive.speed = limit_speed drive_cmd.drive.steering_angle = steering_bias last_speed = limit_speed self.drive.publish(drive_cmd) # post this message r.sleep() # chill for a while # always make sure to leave the robot stopped self.drive.publish(AckermannDriveStamped())
def listener(): rospy.init_node('image-publisher', anonymous = True) rospy.on_shutdown(cleanup) rospy.Subscriber('/images', String, captureImages) rospy.spin()
def main(): print("Initializing node... ") rospy.init_node("throwing") traj = Trajectory('right') traj._t_delay = 5.0 rospy.on_shutdown(traj.stop) rospy.spin()
def __init__(self): self.lock = threading.Lock() rospy.init_node("roboclaw_node",log_level=rospy.DEBUG) rospy.on_shutdown(self.shutdown) rospy.loginfo("Connecting to roboclaw") self.wheels_speeds_pub = rospy.Publisher('/motors/commanded_speeds', Wheels_speeds, queue_size=1) self.motors_currents_pub = rospy.Publisher('/motors/read_currents', Motors_currents, queue_size=1) self.address = int(rospy.get_param("~address", "128")) if self.address > 0x87 or self.address < 0x80: rospy.logfatal("Address out of range") rospy.signal_shutdown("Address out of range") # TODO need someway to check if address is correct self.MAX_ABS_LINEAR_SPEED = float(rospy.get_param("~max_abs_linear_speed", "1.0")) self.MAX_ABS_ANGULAR_SPEED = float(rospy.get_param("~max_abs_angular_speed", "1.0")) self.TICKS_PER_METER = float(rospy.get_param("~ticks_per_meter", "4342.2")) self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.315")) self.ACC_LIM = float(rospy.get_param("~acc_lim", "0.1")) self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH) self.last_set_speed_time = rospy.get_rostime() rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=1) rospy.sleep(1) rospy.logdebug("address %d", self.address) rospy.logdebug("max_abs_linear_speed %f", self.MAX_ABS_LINEAR_SPEED) rospy.logdebug("max_abs_angular_speed %f", self.MAX_ABS_ANGULAR_SPEED) rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER) rospy.logdebug("base_width %f", self.BASE_WIDTH)
def main(): rospy.init_node("DREAM_environment_demo") env = Environment() try: env.init() rospy.on_shutdown(env.del_objects) print("Running. Ctrl-c to quit") env.run() except Exception: env.del_objects() raise
