def get_data_from_controller(): while not done: rospy.init_node('get_data_from_controller') rospy.Subscriber("position_correction", String, callback) rospy.spin() rospy.signal_shutdown("Shutting Down")
def main(): global client try: rospy.init_node("test_move", anonymous=True, disable_signals=True) client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction) print "Waiting for server..." client.wait_for_server() print "Connected to server" while 1: #move1() #move2() #print 'Enter Tra Array:' #s = raw_input() #split1 = s.split(" ") #for i in split1: # Q.append(float(i)) #move1() #del Q[0:len(Q)] #move1() #move_repeated() #move_disordered() #move_interrupt() #move1() #move_repeated() #move_disordered() #move_interrupt() except KeyboardInterrupt: rospy.signal_shutdown("KeyboardInterrupt") raise
def transition(time, next_state): """Transition function to help state machine.""" #Check if wheels have dropped if D.data.wheeldropCaster: print "Wheel drop!" print "Shutting down..." Robo.move(0,0) Robo.mode = "ambivalent" Robo.status = "shutdown" rospy.signal_shutdown("robot picked up... so we're shutting down") #Check for incoming messages from the hive hiveCommand = check_hive_updates() Robo.hiveCommand = hiveCommand #if Robo.hiveCommmand == "stop": # rospy.Timer(rospy.Duration(time), state_wait, oneshot=True) print Robo.curState #Publish robot status updates update = Robo.status rospy.loginfo(update) status_updates.publish(String(update)) rospy.Timer(rospy.Duration(time), next_state, oneshot=True)
def _shutdown_by_reactor(self): """ Reactor shutdown callback. Sends a signal to rospy to shutdown the ROS interfaces """ rospy.signal_shutdown("Reactor shutting down.")
def __init__(self): # ROS initialization: rospy.init_node('pose_manager') self.poseLibrary = dict() self.readInPoses() self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction, execute_cb=self.executeBodyPose, auto_start=False) self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction) if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)): try: rospy.wait_for_service("stop_walk_srv", timeout=2.0) self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty) except: rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. " +"This is normal if there is no nao_walker running.") self.stopWalkSrv = None self.poseServer.start() rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys()); else: rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?"); rospy.signal_shutdown("Required component missing");
def on_key(event): '''Quit if the q key was pressed''' global plt if(event.key == 'q'): plt.close('all') # Close all figures rospy.signal_shutdown('Quitting...') # Ask ROS to shutdown
def __init__(self, port=None, baud=57600, timeout=5.0): """ Initialize node, connect to bus, attempt to negotiate topics. """ self.mutex = thread.allocate_lock() self.lastsync = rospy.Time(0) self.lastsync_lost = rospy.Time(0) self.timeout = timeout self.synced = False self.pub_diagnostics = rospy.Publisher('/diagnostics', diagnostic_msgs.msg.DiagnosticArray) if port== None: # no port specified, listen for any new port? pass elif hasattr(port, 'read'): #assume its a filelike object self.port=port else: # open a specific port try: self.port = Serial(port, baud, timeout=self.timeout*0.5) except SerialException as e: rospy.logerr("Error opening serial: %s", e) rospy.signal_shutdown("Error opening serial: %s" % e) raise SystemExit self.port.timeout = 0.01 # Edit the port timeout time.sleep(0.1) # Wait for ready (patch for Uno) # hydro introduces protocol ver2 which must match node_handle.h # The protocol version is sent as the 2nd sync byte emitted by each end self.protocol_ver1 = '\xff' self.protocol_ver2 = '\xfe' self.protocol_ver = self.protocol_ver2 self.publishers = dict() # id:Publishers self.subscribers = dict() # topic:Subscriber self.services = dict() # topic:Service self.buffer_out = -1 self.buffer_in = -1 self.callbacks = dict() # endpoints for creating new pubs/subs self.callbacks[TopicInfo.ID_PUBLISHER] = self.setupPublisher self.callbacks[TopicInfo.ID_SUBSCRIBER] = self.setupSubscriber # service client/servers have 2 creation endpoints (a publisher and a subscriber) self.callbacks[TopicInfo.ID_SERVICE_SERVER+TopicInfo.ID_PUBLISHER] = self.setupServiceServerPublisher self.callbacks[TopicInfo.ID_SERVICE_SERVER+TopicInfo.ID_SUBSCRIBER] = self.setupServiceServerSubscriber self.callbacks[TopicInfo.ID_SERVICE_CLIENT+TopicInfo.ID_PUBLISHER] = self.setupServiceClientPublisher self.callbacks[TopicInfo.ID_SERVICE_CLIENT+TopicInfo.ID_SUBSCRIBER] = self.setupServiceClientSubscriber # custom endpoints self.callbacks[TopicInfo.ID_PARAMETER_REQUEST] = self.handleParameterRequest self.callbacks[TopicInfo.ID_LOG] = self.handleLoggingRequest self.callbacks[TopicInfo.ID_TIME] = self.handleTimeRequest rospy.sleep(2.0) # TODO self.requestTopics() self.lastsync = rospy.Time.now()
def get_param_duration(param): """Calls rospy.get_param and logs errors. Logs if the param does not exist or is not parsable to rospy.Durotation. And calls rospy.signal_shutdown if the value is invalid/not existing. :return: The Param param from the parameter server. :rtype: rospy.Duration """ # dummy value value = rospy.Duration(1) try: # only a default value in case the param gets fuzzed. value = rospy.Duration(get_param_num(param)) except ValueError: err_msg = ( "Param %s has the invalid value '%s'." % (param, rospy.get_param(param))) rospy.logerr(err_msg) rospy.signal_shutdown(err_msg) value = rospy.Duration(1) return value
def checkImage(self): path2 = "test" test_images = [os.path.join(path2, item) for item in os.listdir(path2)] for im_path in test_images: curr_image_gray = PIL.Image.open(im_path).convert('L') np_array = np.array(curr_image_gray , 'uint8') all_faces = self.fC.detectMultiScale(np_array) #print all_faces for (x , y , width , height) in all_faces: pred_value, confidence = self.predictor.predict(np_array[y: y + height, x: x + width]) if self.unlocked == True: break if confidence >= 10: self.unlocked = True print "UNLOCKED *****************************************************************************" self.connection.write(self.beep_command) break if self.unlocked == False: print "LOCKED ******************************** Shutting Down" rospy.signal_shutdown(1)
def main(argv): app = wx.App() KeyEventFrame() print(__doc__) app.MainLoop() rospy.signal_shutdown("MainLoop") return 0
def window_shutdown(self, widget): # kill gtk thread gtk.main_quit() # kill roscomms thread self.roscommsThread.join(0) # I think it is dead! # kill ros thread rospy.signal_shutdown("Because I said so!")
def listener(): # In ROS, nodes are uniquely named. If two nodes with the same # node are launched, the previous one is kicked off. The # anonymous=True flag means that rospy will choose a unique # name for our 'listener' node so that multiple listeners can # run simultaneously. rospy.init_node('listener', anonymous=True) global client try: client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction) print "Waiting for server..." client.wait_for_server() print "Connected to server" except KeyboardInterrupt: rospy.signal_shutdown("KeyboardInterrupt") raise receive() #global sub_stp #sub_stp = rospy.Subscriber("chatter", String, callback) # spin() simply keeps python from exiting until this node is stopped rospy.spin()
def main(): """ the main function that sets everything up """ # Initialize this ROS node rospy.init_node("kbd_publisher") # any name will do # create our window - be sure the window has the focus! windows_init() # create a String publisher (pub) pub = rospy.Publisher("text_data", String) # main loop while rospy.is_shutdown() == False: # Get incoming key events using cv.WaitKey (lowest 8 bits) key_code = cv.WaitKey(10) & 255 # gets the window's next key_code if key_code == 255: continue # 255 means "no key pressed" key_press = chr(key_code) # convert key_code to a character if " " <= key_press <= "z": # if it's in our valid range print "Publishing ", str(key_press) pub.publish(String(str(key_press))) # publish! if key_code == 27 or key_press == "q": # if ESC or 'q' was pressed pub.publish(String("q")) # publish the string 'q' either way rospy.signal_shutdown("Quitting...") print "Bye!"
def __init__(self): self.config = read_config() self.finished_pub = rospy.Publisher("/map_node/sim_complete", Bool ,queue_size=20) self.path_list = rospy.Publisher("/results/path_list", AStarPath ,queue_size=30) self.policy_list = rospy.Publisher("/results/policy_list", PolicyList ,queue_size=100) rospy.init_node('robot',anonymous=True) self.rate = rospy.Rate(1) rospy.sleep(1) # move_list = self.config["move_list"] # map_size = self.config["map_size"] # start = self.config["start"] # goal = self.config["goal"] # walls = self.config["walls"] # pits = self.config["pits"] # cost = self.config["reward_for_each_step"] # a_star = astar(move_list,map_size,start,goal,walls,pits,cost) # mdp_policies = mdp(self.config) reinforcement = ReinforcementLearning(self.config) # # for move in a_star: # # self.path_list.publish(move) # rospy.sleep(1) # for policy in mdp_policies: # self.policy_list.publish(mdp_policies[len(mdp_policies)-1]) for i in range(1000): self.policy_list.publish(reinforcement.allPolicies[999]) # rospy.sleep(1) rospy.sleep(1) self.finished_pub.publish(True) rospy.sleep(1) rospy.signal_shutdown("finished moving")
def main(): rospy.init_node('axclient', anonymous=True) parser = OptionParser(__doc__.strip()) # parser.add_option("-t","--test",action="store_true", dest="test",default=False, # help="A testing flag") # parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah") (options, args) = parser.parse_args(rospy.myargv()) if (len(args) != 2): parser.error("You must specify the action topic name Eg: ./axclient.py my_action_topic") # get action type from topic topic_type = rostopic._get_topic_type("%s/goal"%args[1])[0] # remove "Goal" string from action type assert("Goal" in topic_type) topic_type = topic_type[0:len(topic_type)-4] action = DynamicAction(topic_type) app = AXClientApp(action, args[1]) app.MainLoop() app.OnQuit() rospy.signal_shutdown('GUI shutdown')
def tear_down(self, keep_alive=False): # forcefully shut down service before killing node if self.dserver: self.dserver.set_service.shutdown("User deleted template.") # for rostest (and potentially other cases), we want to clean up but keep the node alive if not keep_alive: rospy.signal_shutdown("User deleted template.")
def run(self, _): try: for container in self._containers: self._conn.createContainer(**container) for node in self._nodes: self._conn.addNode(**node) for parameter in self._parameters: self._conn.addParameter(**parameter) for interface in self._interfaces: self._conn.addInterface(**interface) for connection in self._connections: self._conn.addConnection(**connection) for ros in self._interfaces: iType = ros['iType'] if iType in _MAP: self._ifs.append( getattr(self._conn, _MAP[iType])(ros['iTag'], ros['iCls'], ros['addr']) ) except Exception as e: import traceback print(''.join(traceback.format_exception_only(type(e), e))) rospy.signal_shutdown('Error')
def go(self, *args, **kwargs): r = rospy.Rate(20) count = 0 while not rospy.is_shutdown(): c = self.stdscr.getch() if c == ord('q'): self.curses_msg("Shutting down...") self.restore_screen() rospy.signal_shutdown("Quitting..") elif c == ord('r'): self.reset() elif c == curses.KEY_RIGHT: self.cur_page = (self.cur_page + 1) % self.num_pages elif c == curses.KEY_LEFT: self.cur_page = (self.cur_page - 1) % self.num_pages else: # Page-specific keys: if self.cur_page == self.page_names.index("Control"): if c >= ord('1') and c <= ord('9'): self.mode_to_active(c - ord('1')) if self.cur_page == self.page_names.index('Mode'): cur_mode = self.latest.controller_status.active_mode if cur_mode == "hover": if c == ord('m'): self.toggle_modify_hover() elif c == ord('y'): self.toggle_modify_yaw_in_hover() # Refresh display: count = (count + 1) % 4 if count == 0 or (self.cur_page != self.prev_page): self.update() r.sleep()
def adjust_yn(): def yes(): done = True rospy.signal_shutdown("Shutting Down") def no(): main_menu() bindings = { # key: (function, args, description) 'y': (yes,[], "Let Robot Continue"), 'n': (no, [], "Enter Control Mode"), } done = False while not done and not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Shutting Down.") elif c in bindings: cmd = bindings[c] cmd[0](*cmd[1]) else: print("key bindings: ") for key, val in sorted(bindings.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2]))
def locator(self): while not rospy.is_shutdown(): "Waiting" try: image = np.asarray(cv2.cv.CloneMat(self.image)) except AttributeError: rospy.sleep(1) continue image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) image = cv2.GaussianBlur(image, (3,3), 2) edges = cv2.Canny(image, 50, 20) #c = edges #c = cv2.HoughCircles(edges, cv2. , 2, 100) circles = cv2.HoughCircles(image, cv2.cv.CV_HOUGH_GRADIENT, 2, 20, maxRadius=100) print circles if circles is not None: for c in circles[0]: cv2.circle(image, (c[0],c[1]), c[2], (0,255,0),2) #convert to HSV space #image = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) #Filter for yellow balls #chan = cv.CloneMat(image) #chan = cv2.split(np.asarray(image)) #cv.Threshold(image,image,228,255,cv.CV_THRESH_BINARY) #cv.Dilate(image,image,None,2) #cv.Erode(image,image,None,2) cv2.imshow("Image window", image) cv2.imshow("Edges", edges) if cv2.waitKey(2) == 1048603: rospy.signal_shutdown("You asked me to you bastard") rospy.sleep(0.05)# sleeping for 50 ms
def process_depth(depth): #depth_image = D.bridge.imgmsg_to_cv2(data, "32FC1").astype(np.uint8) #cv2.imshow('im', depth) #try: # depth_image = cv2.warpPerspective(depth_image, H, (config.GAME_WIDTH,config.GAME_HEIGHT)) #except NameError: # pass center = findSphero_depth(depth) if not center: print 'None' D.pub.publish(str(center)) #depth_image = depth.astype(np.uint8) #cv2.circle(depth_image, center, 5, (255, 0, 0), 2) #cv2.imshow('im', depth_image) key_press = cv2.waitKey(5) & 0xff if key_press == 27 or key_press == ord('q'): rospy.signal_shutdown(0)
def command_callback(self, command): self.command = command.data if (self.command == 'exit'): self.pubPanelsCommand.publish(MsgPanelsCommand(command='stop', arg1=0, arg2=0, arg3=0, arg4=0, arg5=0, arg6=0)) self.bRunning = False rospy.signal_shutdown('User requested exit.') if (self.command == 'stop'): self.pubPanelsCommand.publish(MsgPanelsCommand(command='stop', arg1=0, arg2=0, arg3=0, arg4=0, arg5=0, arg6=0)) self.pubPanelsCommand.publish(MsgPanelsCommand(command='all_off', arg1=0, arg2=0, arg3=0, arg4=0, arg5=0, arg6=0)) self.bRunning = False if (self.command == 'start'): self.pubPanelsCommand.publish(MsgPanelsCommand(command='set_pattern_id', arg1=self.params['pattern_id'], arg2=0, arg3=0, arg4=0, arg5=0, arg6=0)) self.pubPanelsCommand.publish(MsgPanelsCommand(command='start', arg1=0, arg2=0, arg3=0, arg4=0, arg5=0, arg6=0)) self.bRunning = True if (self.command == 'help'): rospy.logwarn('The %s/command topic accepts the following string commands:' % self.nodename.rstrip('/')) rospy.logwarn(' help This message.') rospy.logwarn(' stop Stop the ledpanels.') rospy.logwarn(' start Start the ledpanels.') rospy.logwarn(' exit Exit the program.') rospy.logwarn('') rospy.logwarn('You can send the above commands at the shell prompt via:') rospy.logwarn('rostopic pub -1 %s/command std_msgs/String commandtext' % self.nodename.rstrip('/')) rospy.logwarn('') rospy.logwarn('Parameters are settable as launch-time parameters.') rospy.logwarn('')
def main(): pub = rospy.Publisher("/vesselness/settings", vesselness_params, queue_size=10) rospy.init_node("vesselness_filter_parameter_gui", anonymous=True) rate = rospy.Rate(1) # 10hz # Initialize parameters params = vesselness_params() # Setup GUI window_name = "Vesselness Parameter GUI" image = np.zeros((1, 512)) cv2.namedWindow(window_name) cv2.createTrackbar("Hessian Side", window_name, 1, 4, do_nothing) cv2.createTrackbar("Hessian Variance", window_name, 0, 255, do_nothing) cv2.createTrackbar("Post Process Side", window_name, 1, 4, do_nothing) cv2.createTrackbar("Post Process Variance", window_name, 0, 255, do_nothing) cv2.createTrackbar("c Parameter", window_name, 0, 255, do_nothing) cv2.createTrackbar("beta Parameter", window_name, 0, 255, do_nothing) while not rospy.is_shutdown(): cv2.imshow(window_name, image) k = cv2.waitKey(1) & 0xFF if k in [27, ord('q')]: rospy.signal_shutdown('Quit') params.hessianSide = cv2.getTrackbarPos("Hessian Side", window_name) params.hessianVariance = cv2.getTrackbarPos("Hessian Variance", window_name) / 10.0 params.postProcessSide = cv2.getTrackbarPos("Post Process Side", window_name) params.postProcessVariance = cv2.getTrackbarPos("Post Process Variance", window_name) / 10.0 params.cParameter = 0.001 + 0.999 / 255 * cv2.getTrackbarPos("c Parameter", window_name) params.betaParameter = 0.001 + 0.999 / 255 * cv2.getTrackbarPos("beta Parameter", window_name) rospy.loginfo(params) pub.publish(params) rate.sleep()
def set_path(self, ogrid): #group contam_points into boxes, prioritizing less dirty areas first points = self.extract_points(ogrid) if points.size==0: rospy.signal_shutdown("Cleaning complete. Program will now exit.") return else: n_clusters = int(round(points.size/2*(self.resolution**2))) #1 cluster ~= 1 sq. meter print n_clusters cluster_data = [] if n_clusters >= 1: kmeans = KMeans(n_clusters).fit(points) for n in xrange(n_clusters): xy = points[kmeans.labels_==n] #print "point {0}".format(n) cluster_data.append(self.bounding_box(xy)) order = self._get_order([x[4] for x in cluster_data]) self.seq += 1 self.goals.extend(self._set_cluster_goals(points[kmeans.labels_==order[0][0]].tolist(), cluster_data[order[0][0]], order[0][1])) # for o in order: # self.goals.extend(self._set_cluster_goals(cluster_data[o[0]], o[1])) # self.seq += 1 # elif n_clusters == 0 and -1 in db.labels_: # xy = points[db.labels_== -1] # for point in xy: # header = Header(self.seq, rospy.Time.now(), '/map') # self.seq += 1 # pose = PoseStamped(header, Pose(Point(point[0], point[1], 0), RIGHT)) # goal = MoveBaseGoal() # goal.target_pose = pose # self.goals.append(goal) else: rospy.signal_shutdown("Cleaning complete. Program will now exit.")
def callback(data): """ This function is called for each published message """ message = data.data print "I received the string", message if message in 'xqs': D.STOP = True D.tank(0,0) if message in 'xq': time.sleep(.5) # wait a bit... print "Goodbye!" rospy.signal_shutdown("Quit requested.") elif message == ' ': D.tank(0,0) elif message == 'F': transition( 0.1, state_start ) elif message == 'R': D.tank(200,200) elif message == 'Q': D.tank(-200,200) elif message == 'S': D.tank(200,-200) elif message == 'T': D.tank(-200,-200)
def talker(self): # Main publisher logic- app branches out from here self.pub = rospy.Publisher('hamr_command', HamrCommand, queue_size=10) rospy.init_node('hamr_tester', anonymous=True) rate = rospy.Rate(10) # 10hz msg = HamrCommand() while not rospy.is_shutdown(): drive_or_pid = raw_input("Drive, PID, Kill all Motors (kill), DD Set (DDS), Holonomic (holo), tests or Quit?\n") drive_or_pid = str.strip(drive_or_pid.lower()) if drive_or_pid == 'drive': self.drive_control(msg) elif drive_or_pid == 'pid': self.pid_control(msg) self.get_val(msg) elif drive_or_pid == 'kill': print 'Killing all motors...' self.kill_motors() elif drive_or_pid == 'quit': rospy.signal_shutdown('Node shut down') elif drive_or_pid == 'dds': self.dd_set(msg) elif drive_or_pid == 'holo': self.holonomic_control(msg) elif drive_or_pid == 'test': self.call_tests(msg) else: print "That's not a valid option" rate.sleep()
def run(self): move_thread = None if not is_moveit_running(): rospy.loginfo("starting moveit") move_thread = MoveItThread() rospy.loginfo("Waiting for MoveIt...") self.client = MoveGroupInterface("arm_with_torso", "base_link") rospy.loginfo("...connected") # Padding does not work (especially for self collisions) # So we are adding a box above the base of the robot scene = PlanningSceneInterface("base_link") scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375) joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.client.moveToJointPosition(joints, pose, 0.0, max_velocity_scaling_factor=0.5) if result and result.error_code.val == MoveItErrorCodes.SUCCESS: scene.removeCollisionObject("keepout") if move_thread: move_thread.stop() # On success quit # Stopping the MoveIt thread works, however, the action client # does not get shut down, and future tucks will not work. # As a work-around, we die and roslaunch will respawn us. rospy.signal_shutdown("done") sys.exit(0) return
def detect_people(self, img): self.frame_width = img.width cv_image = CvBridge().imgmsg_to_cv2(img, desired_encoding="bgr8") print "what the fuk" found, w = self.hog.detectMultiScale(cv_image, winStride=(8,8), padding=(32,32), scale=1.05) found_filtered = [] for ri, r in enumerate(found): for qi, q in enumerate(found): if ri != qi and inside(r, q): break else: found_filtered.append(r) x, y, w, _ = found_filtered[0] self.calculate_distance(x, w) if self.cur_distance <= 1.0: rospy.sleep(1) rospy.signal_shutdown("Done.") self.calculate_goal() self.markov = mdp(self.goal) self.pl = PolicyList() self.pl.data = markov.policy_list self.make_move()
def main(): global client try: rospy.init_node("test_move", anonymous=True, disable_signals=True) client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction) print "Waiting for server..." client.wait_for_server() print "Connected to server" parameters = rospy.get_param(None) index = str(parameters).find('prefix') if (index > 0): prefix = str(parameters)[index+len("prefix': '"):(index+len("prefix': '")+str(parameters)[index+len("prefix': '"):-1].find("'"))] for i, name in enumerate(JOINT_NAMES): JOINT_NAMES[i] = prefix + name print "This program makes the robot move between the following three poses:" print str([Q1[i]*180./pi for i in xrange(0,6)]) print str([Q2[i]*180./pi for i in xrange(0,6)]) print str([Q3[i]*180./pi for i in xrange(0,6)]) print "Please make sure that your robot can move freely between these poses before proceeding!" inp = raw_input("Continue? y/n: ")[0] if (inp == 'y'): #move1() move_repeated() #move_disordered() #move_interrupt() else: print "Halting program" except KeyboardInterrupt: rospy.signal_shutdown("KeyboardInterrupt") raise
def spin(self): self.diagnostic_pub = rospy.Publisher("/diagnostics", DiagnosticArray) try: reset_count = 0 rospy.loginfo("Camera synchronizer is running...") controller_update_count = 0 while not rospy.is_shutdown(): if self.config['camera_reset'] == True: reset_count = reset_count + 1 if reset_count > 3: self.server.update_configuration({'camera_reset' : False}) else: reset_count = 0 self.update_diagnostics() # In case the controllers got restarted, refresh their state. controller_update_count += 1 if controller_update_count >= 10: controller_update_count = 0 for controller in self.controllers: controller.update(); rospy.sleep(1) finally: rospy.signal_shutdown("Main thread exiting") self.kill() print "Main thread exiting"
def __init__(self): #create our action server clients self._left_client = actionlib.SimpleActionClient( 'robot/limb/left/follow_joint_trajectory', FollowJointTrajectoryAction, ) self._right_client = actionlib.SimpleActionClient( 'robot/limb/right/follow_joint_trajectory', FollowJointTrajectoryAction, ) #verify joint trajectory action servers are available l_server_up = self._left_client.wait_for_server(rospy.Duration(10.0)) r_server_up = self._right_client.wait_for_server(rospy.Duration(10.0)) if not l_server_up or not r_server_up: msg = ("Action server not available." " Verify action server availability.") rospy.logerr(msg) rospy.signal_shutdown(msg) sys.exit(1) #create our goal request self._l_goal = FollowJointTrajectoryGoal() self._r_goal = FollowJointTrajectoryGoal() #limb interface - current angles needed for start move self._l_arm = baxter_interface.Limb('left') self._r_arm = baxter_interface.Limb('right') #gripper interface - for gripper command playback self._l_gripper = baxter_interface.Gripper('left', CHECK_VERSION) self._r_gripper = baxter_interface.Gripper('right', CHECK_VERSION) #flag to signify the arm trajectories have begun executing self._arm_trajectory_started = False #reentrant lock to prevent same-thread lockout self._lock = threading.RLock() # Verify Grippers Have No Errors and are Calibrated if self._l_gripper.error(): self._l_gripper.reset() if self._r_gripper.error(): self._r_gripper.reset() if (not self._l_gripper.calibrated() and self._l_gripper.type() != 'custom'): self._l_gripper.calibrate() if (not self._r_gripper.calibrated() and self._r_gripper.type() != 'custom'): self._r_gripper.calibrate() #gripper goal trajectories self._l_grip = FollowJointTrajectoryGoal() self._r_grip = FollowJointTrajectoryGoal() # Timing offset to prevent gripper playback before trajectory has started self._slow_move_offset = 0.0 self._trajectory_start_offset = rospy.Duration(0.0) self._trajectory_actual_offset = rospy.Duration(0.0) #param namespace self._param_ns = '/rsdk_joint_trajectory_action_server/' #gripper control rate self._gripper_rate = 20.0 # Hz #publish timing topics self.timing_pub_state = rospy.Publisher('/board_pose/cycle_on', std_msgs.Bool, queue_size=0) self.timing_msg_state = std_msgs.Bool() self.timing_pub_time = rospy.Publisher('/board_pose/cycle_time', std_msgs.Time, queue_size=1) self.timing_msg_time = std_msgs.Time()
def follow(self): if not (self.target_found and self.object_location != None): return pid = self.camera_pid pid.kp = -0.0005 pid.kd = 0.005 target_value = 0.0 current_value = self.object_location print current_value ang = pid.compute(current_value, target_value) keep_distance = 0.3 spectrum = 20 mid = np.amin(np.concatenate((self.laser_values[-spectrum:], self.laser_values[:spectrum]), axis=0) ) #right = np.amin(self.laser_values[-2*spectrum:-spectrum]) #left = np.amin(self.laser_values[spectrum:spectrum*2]) #print 'left', left, 'mid', mid, 'right', right, ' ang', ang if self.target_found: if abs(current_value)<2: distance = min(self.laser_values[0],3) if distance < 0.1: return client = actionlib.SimpleActionClient('move_base',MoveBaseAction) client.wait_for_server() goal = MoveBaseGoal() goal.target_pose.header.frame_id = "base_link" goal.target_pose.header.stamp = rospy.Time.now() # Move 0.5 meters forward along the x axis of the "map" coordinate frame goal.target_pose.pose.position.x = distance - 0.4 # No rotation of the mobile base frame w.r.t. map frame goal.target_pose.pose.orientation.w = 1.0 print 'going on goal', goal time.sleep(3.0) SimpleActionClient = client.send_goal(goal) time.sleep(2.0) client.cancel_goal() # Waits for the server to finish performing the action. wait = client.wait_for_result() if not wait: rospy.logerr("Action server not available!") rospy.signal_shutdown("Action server not available!") else: # Result of executing the action print 'result', client.get_result() br = tf.TransformBroadcaster() br.sendTransform((distance, 0.0, 0.0),(0.0, 0.0, 0.0, 1.0),rospy.Time.now(),"/carrot1","/base_link") ls = tf.TransformListener() #print ls.allFramesAsString() #print 'carrot', ls.frameExists("/carrot1"), 'map', ls.frameExists("map"), 'base_link', ls.frameExists("base_link") if ls.frameExists("carrot1") and ls.frameExists("map"): t = ls.getLatestCommonTime("carrot1", "map") position, quaternion = ls.lookupTransform("carrot1", "map", t) print 'pos', position return if mid<1.5: #drive(mid - keep_distance ,ang) vel = mid - keep_distance else: #drive(1,ang) vel = 1 else: #drive(0,0) vel = 0 ang = 0 vel=0 self.drive(vel,-ang)
freeze = False if state == 2: freeze = False if state == 1: freeze = False if state == 4 and state_transition_time_s > 30: print("Shutting down because in state 4 for 30+ s") #unix('sudo shutdown -h now') if time_step.check(): print( d2s("In state", state, "for", state_transition_time_s, "seconds, previous_state =", previous_state)) time_step.reset() if not folder_display_timer.check(): print("*** Data foldername = " + rp.foldername + '***') if reload_timer.check(): reload(rp) model_name_pub.publish(std_msgs.msg.String(rp.weights_file_path)) reload_timer.reset() if git_pull_timer.check(): #unix(opjh('kzpy3/kzpy3_git_pull.sh')) unix(opjh('model_car/model_car_git_pull.sh')) git_pull_timer.reset() except Exception as e: print("********** Exception ***********************", 'red') traceback.print_exc(file=sys.stdout) rospy.signal_shutdown(d2s(e.message, e.args))
def main(): rospy.init_node('assembly_task_manager') topic_name = '/assembly/state_transition' planner = MoveGroupPlanner() @smach.cb_interface(input_keys=['target_pose']) def get_trajectory(ud, goal): trajectory = planner.plan(ud.target_pose) # Good! Checked return_goal = FollowJointTrajectoryGoal() return_goal.trajectory = trajectory.joint_trajectory #print(trajectory) return return_goal assembly_sm = StateMachine(outcomes=['finished', 'aborted', 'preempted']) ## predefined targets # init position joint_init_goal = [None] * 7 joint_init_goal[0] = 0 joint_init_goal[1] = -pi / 4 joint_init_goal[2] = 0 joint_init_goal[3] = -pi / 2 joint_init_goal[4] = 0 joint_init_goal[5] = pi / 3 joint_init_goal[6] = pi / 4 pose_goal = geometry_msgs.msg.Pose() pose_goal.orientation.x = 0.9238795 pose_goal.orientation.y = -0.3826834 pose_goal.orientation.z = 0. pose_goal.orientation.w = 0. pose_goal.position.x = 0.4 pose_goal.position.y = 0.0 pose_goal.position.z = 0.12 assembly_sm.userdata.joint_init_goal = joint_init_goal assembly_sm.userdata.pose1_goal = pose_goal peg_goal1 = AssemblePegInHoleGoal() peg_goal1.pitch = 0.0025 peg_goal1.linear_velocity = 0.005 peg_goal1.z_force = -6.0 peg_goal1.insert_force = -20.0 contact_force_goal = AssembleApproachGoal() contact_force_goal.contact_force = 6 epsilon_inner = 0.002 epsilon_outer = 0.002 epsilon = franka_gripper.msg.GraspEpsilon(inner=epsilon_inner, outer=epsilon_outer) grasp_goal1 = franka_gripper.msg.GraspGoal(width=0.007, epsilon=epsilon, speed=0.1, force=60) gripper_open_goal = franka_gripper.msg.MoveGoal(width=0.1, speed=0.1) ## defining state machine structure with assembly_sm: StateMachine.add('READY', StringTransitionState(topic_name, outcomes=['initialize']), transitions={'initialize': 'INITIALIZE1'}) StateMachine.add('INITIALIZE1', SimpleActionState('/franka_gripper/move', franka_gripper.msg.MoveAction, goal=gripper_open_goal), transitions={'succeeded': 'INITIALIZE2'}) StateMachine.add('INITIALIZE2', SimpleActionState( '/assembly_controller/joint_trajectory_control', FollowJointTrajectoryAction, goal_cb=get_trajectory), transitions={'succeeded': 'READY_TO_MOVE'}, remapping={'target_pose': 'joint_init_goal'}) StateMachine.add('READY_TO_MOVE', StringTransitionState(topic_name, outcomes=['approach1']), transitions={'approach1': 'APPROACH1'}) StateMachine.add('APPROACH1', SimpleActionState( '/assembly_controller/joint_trajectory_control', FollowJointTrajectoryAction, goal_cb=get_trajectory), transitions={'succeeded': 'GRASP1'}, remapping={'target_pose': 'pose1_goal'}) StateMachine.add('GRASP1', SimpleActionState('/franka_gripper/grasp', franka_gripper.msg.GraspAction, goal=grasp_goal1), transitions={'succeeded': 'APPROACH_TO_COMPONENT'}) StateMachine.add('APPROACH_TO_COMPONENT', SimpleActionState( '/assembly_controller/assemble_approach_control', AssembleApproachAction, goal=contact_force_goal), transitions={'succeeded': 'ASSEMBLE1'}) StateMachine.add('ASSEMBLE1', SimpleActionState( '/assembly_controller/assemble_peginhole_control', AssemblePegInHoleAction, goal=peg_goal1), transitions={'succeeded': 'finished'}) #StateMachine.add('APPROACH1', # SimpleActionState('/assembly_controller/joint_trajectory_control', JointControlAction, goal=joint_init_goal), # transitions={'succeeded':'READY_TO_MOVE'}) # Run state machine introspection server intro_server = smach_ros.IntrospectionServer('assembly', assembly_sm, '/ASSEMBLY_1') intro_server.start() assembly_sm.execute() rospy.spin() intro_server.stop() rospy.signal_shutdown('All done.')
while not rospy.is_shutdown(): msg.position[0:6] = ic.jointsPos msg.position[6:9] = [0.0, 0.0, 0.0] jointPub.publish(msg) # for i in range(0, 5): # x = raw_input ("Press to move arm") # ic.moveArm(position_list[i]) # x = raw_input ("Press to get data") # ic.jointReadList[i] = True # x = raw_input ("Press to stop data") # ic.jointReadList[i] = False if (ic.finishedCalibrating): for i in range(0,6): print(len(ic.jointDataList[i])) for i in range(0,6): averages.append(np.average(ic.jointDataList[i])) print(averages) with open('imuRangePositions.csv', 'wb') as csvfile: writer = csv.writer(csvfile, delimiter=',', quotechar='|', quoting=csv.QUOTE_MINIMAL) writer.writerow(averages) rospy.signal_shutdown("Done Calibrating") rate.sleep()
def get_ctrl_output(self): # get the location of the robot try: (translation, rotation) = self.trans_listener.lookupTransform( "/map", "base_footprint", rospy.Time(0)) euler = tf.transformations.euler_from_quaternion(rotation) self.x = translation[0] self.y = translation[1] self.th = euler[2] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): translation = (0, 0, 0) rotation = (0, 0, 0, 1) euler = tf.transformations.euler_from_quaternion(rotation) self.x = translation[0] self.y = translation[1] self.th = euler[2] # Define Controller Gains k1 = 0.5 k2 = 0.5 k3 = 0.5 # Distance to final point in current path rho = np.linalg.norm( np.asarray(self.x_goal[0:2]) - np.asarray([self.x, self.y])) # Define relevant control parameters alpha = wrapToPi( np.arctan2(self.y_g - self.y, self.x_g - self.x) - self.th) delta = wrapToPi(alpha + self.th - self.th_g) #Define control inputs (V,om) - without saturation constraints if self.Mode == 0: #'velocity_control' rospy.loginfo("Velocity Mode") V = self.V om = self.om else: rospy.loginfo("Position Mode") V = k1 * rho * np.cos(alpha) om = k2 * alpha + k1 * np.sinc( alpha / np.pi) * np.cos(alpha) * (alpha + k3 * delta) # Check whether we're close to the goal, and override control inputs to force a stop # at the target position rospy.logwarn('Mission State is {}'.format(self.missionState)) if rho < self.STOP_THRESHOLD or self.missionState == self.MissionStates.END_OF_MISSION: rospy.logwarn('Stopping Robot') if self.missionState == self.MissionStates.END_OF_MISSION: rospy.signal_shutdown( 'End of Mission. Shutting down controller.') V = 0 om = 0 # Apply saturation limits V = np.sign(V) * min(0.3, np.abs(V)) om = np.sign(om) * min(1.0, np.abs(om)) cmd_x_dot = V # forward velocity cmd_theta_dot = om cmd = Twist() cmd.linear.x = cmd_x_dot cmd.angular.z = cmd_theta_dot return cmd
tf_listener = tf.TransformListener() # parent frame for the listener parent_frame = 'odom' # child frame for the listener child_frame = 'base_footprint' # gains for the proportional controllers. These values can be tuned. k_h_gain = 1 k_v_gain = 1 try: tf_listener.waitForTransform(parent_frame, child_frame, rospy.Time(), rospy.Duration(1.0)) except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("Cannot find transform between {p} and {c}".format( p=parent_frame, c=child_frame)) rospy.signal_shutdown("tf Exception") # Class for storing node position, cost to come, parent index, and prev_orientation. class Node: def __init__(self, x, y, cost, parent_index, prev_orientation, curr_orientation, UL, RL): self.x = x self.y = y self.cost = cost self.parent_index = parent_index self.prev_orientation = prev_orientation self.curr_orientation = curr_orientation self.UL = UL self.RL = RL
if __name__ == '__main__': rospy.init_node('new_pose', argv=sys.argv, disable_signals=True) listener = tf.TransformListener() while True: try: translation, rotation = listener.lookupTransform('j2s7s300_link_base', 'ee_frame_camera_flipped', rospy.Time()) break # once the transform is obtained move on except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue # if it fails try again pose = translation + rotation print(pose) updated_pose = rospy.ServiceProxy('new_pose', New_pose) try: answer = updated_pose(pose) except rospy.ServiceException as e: rospy.logwarn('Service call failed for: {0}'.format(e)) rospy.loginfo( '{0}'.format(answer.success)) rospy.signal_shutdown('{0}'.format(answer.success))
def __init__(self): # params self.dt = 0.01 # timestep of the simulation t_final = rospy.get_param('/t_final') do_liveplot = rospy.get_param('/do_liveplot') save_logfile = rospy.get_param('/save_logfile') # init node subs pubs rospy.init_node('experiment_manager', anonymous=True) #rospy.init_node('experiment_manager', anonymous=True, disable_signals=True) self.clockpub = rospy.Publisher('/clock', Clock, queue_size=10) self.pathlocalsub = rospy.Subscriber("pathlocal", Path, self.pathlocal_callback) self.obstaclesub = rospy.Subscriber("obstacles", Obstacles, self.obstacles_callback) self.trajhatsub = rospy.Subscriber("trajhat", Trajectory, self.trajhat_callback) self.trajstarsub = rospy.Subscriber("trajstar", Trajectory, self.trajstar_callback) self.statesub = rospy.Subscriber("state", State, self.state_callback) self.dynamicparamsub = rospy.Subscriber("dynamic_vehicle_params", DynamicVehicleParams, self.dynamicparams_callback) # load global path self.loadPathGlobalFromFile() # set static vehicle params self.setStaticParams() # init internal variables self.counter = 0 # use this to reduce plot update rate self.pathlocal = Path() self.obstacles = Obstacles() self.trajhat = Trajectory() self.trajstar = Trajectory() self.state = State() self.dp = DynamicVehicleParams() # init lists for logging self.tvec = [] self.states = [] if (do_liveplot): # init plot window plt.ion() self.f, (self.a0, self.a1) = plt.subplots( 1, 2, gridspec_kw={'width_ratios': [3, 1]}) self.a0.axis("equal") self.a1.axis("equal") # Main loop self.t = 0 while (not rospy.is_shutdown()) and self.t < t_final: # store data from subscribed topics if (save_logfile): self.stack_data() # handle simtime self.t += self.dt msg = Clock() t_rostime = rospy.Time(self.t) msg.clock = t_rostime self.clockpub.publish(msg) if (do_liveplot): self.liveplot() slowdown_factor = 1 else: slowdown_factor = 1 print 'simtime t =', t_rostime.to_sec() time.sleep(self.dt * slowdown_factor) print 'simulation finished' if (save_logfile): self.save_log() # send shutdown signal message = 'run finished, shutting down' print message rospy.signal_shutdown(message)
def __init__(self): # Give the node a name rospy.init_node('out_and_back', anonymous=False) tf_prefix = rospy.get_param('tf_prefix', 'robot1') # 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', Twist, queue_size=5) # How fast will we update the robot's movement? rate = 200 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Set the forward linear speed to 0.2 meters per second linear_speed = 20 # Set the travel points in meters goal_x = [ 0.0, 0.0, 42.0, 42.0, -12.0, -12.0, -32.0, -32.0, -8.0, -8.0, -8.0, 38.0, 38.0, 0.0, 0.0 ] goal_y = [ 0.0, -2.0, -2.0, 52.0, 52.0, 18.0, 18.0, 22.0, 22.0, 18.0, 48.0, 48.0, 2.0, 2.0, 0.0 ] #goal_x = [ 0.0, 0.0, 4.0, 4.0, 5.0, 0.0, 0.0, -5.0,-4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #goal_y = [ 0.0 , 1.0, 1.0, 2.0, 2.0, 2.0,-2.0, -2.0,-1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Set the rotation speed in radians per second angular_speed = 0.1 # Set the angular tolerance in degrees converted to radians angular_tolerance = radians(5) distance_tolerance = 0.01 # 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 = '/' + tf_prefix + '/odom' # Find out if the robot uses /base_link or /base_footprint try: self.tf_listener.waitForTransform( self.odom_frame, '/' + tf_prefix + '/base_footprint', rospy.Time(), rospy.Duration(1)) self.base_frame = '/' + tf_prefix + '/base_footprint' except (tf.Exception, tf.ConnectivityException, tf.LookupException): try: self.tf_listener.waitForTransform( self.odom_frame, '/' + tf_prefix + '/base_link', rospy.Time(), rospy.Duration(1)) self.base_frame = '/' + tf_prefix + '/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() for i in range(len(goal_x)): print("goal: x=%f, y=%f" % (goal_x[i], goal_y[i])) move_cmd = Twist() (position, rotation) = self.get_odom() angle_to_goal = 0 while not_done(position.x, position.y, goal_x[i], goal_y[i]): inc_x = goal_x[i] - position.x inc_y = goal_y[i] - position.y last_goal = angle_to_goal angle_to_goal = atan2(inc_y, inc_x) alpha = angle_to_goal - last_goal if abs(alpha) > 1.5 * pi: if alpha > 0: angle_to_goal -= 2 * pi else: angle_to_goal += 2 * pi # print("goal:") # print(angle_to_goal) # print(rotation) if abs(angle_to_goal - rotation) > abs(angular_tolerance) or abs( angle_to_goal - rotation) > pi - abs(angular_tolerance): move_cmd.linear.x = 0 if angle_to_goal > rotation: move_cmd.angular.z = 0.4 else: move_cmd.angular.z = -0.4 else: move_cmd.linear.x = 0.1 move_cmd.angular.z = 0.0 self.cmd_vel.publish(move_cmd) r.sleep() last_rotation = rotation (position, rotation) = self.get_odom() delta = rotation - last_rotation if abs(delta) > pi: if delta > 0: rotation -= 2 * pi else: rotation += 2 * pi # rotation = normalize_angle(rotation) # Stop the robot before the next leg move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(2) print("All done!") os.system( 'cd $(rospack find omtb_map_extension_plugin)/scrips && sh save_map_2000.sh' ) os.system( 'cd $(rospack find omtb_map_extension_plugin)/scrips && sh kill_bag.sh' ) # Stop the robot for good self.cmd_vel.publish(Twist())
def state_callback(msg): global state state = msg.data if state == 7: rospy.signal_shutdown('killed by selfdrive manager') sys.exit()
#verify result if all([l_finish, r_finish, l_result, r_result]): return True else: msg = ("Trajectory action failed or did not finish before " "timeout/interrupt.") rospy.logwarn(msg) return False if __name__ == "__main__": a = BaxterActuatorHandler(None, None, 'both', 100, 'position_w_id') a.play_traj('lib/handlers/baxter/actions/pickup.rec', 'pickup', 1, 0, initial=True) a.play_traj('lib/handlers/baxter/actions/drop.rec', 'drop', 1, 0, initial=True) a.play_traj('lib/handlers/baxter/actions/pickup.rec', 'pickup', 1, 1, initial=False) rospy.signal_shutdown("Action completed.")
default='relaxedIK.config') relaxedIK = RelaxedIK.init_from_config(config_file_name) num_chains = relaxedIK.vars.robot.numChains while eepg == None: continue rate = rospy.Rate(100) while not rospy.is_shutdown(): pose_goals = eepg.ee_poses header = eepg.header num_poses = len(pose_goals) if not num_poses == num_chains: print bcolors.FAIL + 'ERROR: Number of pose goals ({}) ' \ 'not equal to the number of kinematic chains ({}). Exiting relaxed_ik_node'.format(num_poses, num_chains) rospy.signal_shutdown() pos_goals = [] quat_goals = [] for p in pose_goals: pos_x = p.position.x pos_y = p.position.y pos_z = p.position.z quat_w = p.orientation.w quat_x = p.orientation.x quat_y = p.orientation.y quat_z = p.orientation.z pos_goals.append([pos_x, pos_y, pos_z])
self.targetSize = 1000 self.detection = False self.controller.SetCoord(self.targetX, self.targetY, self.targetSize, self.detection) finally: self.imageLock.release() def ReceiveNavdata(self,navdata): # Update the message to be displayed msg = self.StatusMessages[navdata.state] if navdata.state in self.StatusMessages else self.UnknownMessage self.statusMessage = 'Connected:{}||{}||Battery:{}%||AutoTracking:{}'.format(self.connected,msg,int(navdata.batteryPercent),self.auto) self.tagLock.acquire() try: if navdata.tags_count > 0: self.tags = [(navdata.tags_xc[i],navdata.tags_yc[i],navdata.tags_distance[i]) for i in range(0,navdata.tags_count)] else: self.tags = [] finally: self.tagLock.release() if __name__=='__main__': import sys rospy.init_node('Tracking') app = QtGui.QApplication(sys.argv) #controller = BasicDroneController() display = VideoProcessing() display.show() status = app.exec_() rospy.signal_shutdown('Great Flying!') sys.exit(status)
def callback(data): rospy.on_shutdown(offhook) global started global max_speed global max_angle global lmt_speed global lmt_angle global cur_speed global cur_angle lefttrg = data.axes[2] ritetrg = data.axes[5] leftbump = data.buttons[4] ritebump = data.buttons[5] dpadx = data.axes[6] dpady = data.axes[7] leftanlgx = data.axes[0] leftanlgy = data.axes[1] start = data.buttons[0] bigX = data.buttons[8] if bigX == 1: rospy.signal_shutdown("X pressed") if start == 1: started = not started print ("ENABLED" if started else "DISABLED") if started: lmt_speed += dpady*2 lmt_angle += -1*dpadx*5 cur_angle = range_map(leftanlgx, 1, -1, -1*lmt_angle, lmt_angle) if lefttrg == 1 or ritetrg == 1: if lefttrg != 1: cur_speed = range_map(lefttrg, -1, 1, -1*lmt_speed, 0) elif ritetrg != 1: cur_speed = range_map(ritetrg, 1, -1, 0, lmt_speed) else: cur_speed = 0 if lefttrg != 1 and ritetrg != 1: cur_speed = 0 if leftbump == 1: cur_angle = 0 if ritebump == 1: cur_speed = 0 else: cur_speed = 0 cur_angle = 0 msg = drive_param() msg.velocity = cur_speed msg.angle = cur_speed print ('Limits: speed: %f, angle: %f' % (lmt_speed, lmt_angle)) print ('Current: speed: %f, angle: %f' % (cur_speed, cur_angle)) pub.publish(msg)
def main(): global client, MODE_DEBUG, MODE_REDUCE_FULL try: if (len(sys.argv) > 1): if (sys.argv[1] == 'debug'): MODE_DEBUG = True print "MODE_DEBUG=" + str(MODE_DEBUG) if (len(sys.argv) > 2): if (sys.argv[2] == 'full'): MODE_REDUCE_FULL = False print "Mode Debug Full ? : " + str(MODE_REDUCE_FULL) c_print("Initialisation du Publisher /ur_driver/URScript", MODE_REDUCE_FULL) pub = rospy.Publisher('/ur_driver/URScript', String, queue_size=10) c_print("Initialisation du Publisher /wsg_50_driver/goal_position", MODE_REDUCE_FULL) pubGripper = rospy.Publisher('/wsg_50_driver/goal_position', Cmd, queue_size=10) c_print(str("Initialisation du noeud :" + NODE_NAME), MODE_REDUCE_FULL) rospy.init_node(NODE_NAME, anonymous=True, disable_signals=True) print "This program makes the robot move, choose a mode:" print " - Mode ROS (ROS FollowJointTrajectoryAction) : R" print " - Mode URSCRIPT (movej, servoj) : U" print " - Demo : D" print " - Kinematics : K" inp = raw_input("Mode? R/U/D/K: ")[0] if (inp == 'R'): client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction) c_print("Waiting for server...", MODE_REDUCE_FULL) client.wait_for_server() c_print("Connected to server") #move1() move_repeated() #move_disordered() #move_interrupt() elif (inp == 'U'): c_print(str("Instanciation Kinematic UR10"), MODE_REDUCE_FULL) kin = Kinematics('ur10') c_print(str("Attente d'un message de JointState"), MODE_REDUCE_FULL) joint_states = rospy.wait_for_message("joint_states", JointState) joints_pos = joint_states.position c_print("JointState.position: " + str(tuple(joints_pos)), MODE_REDUCE_FULL) x = kin.forward(np.array(joints_pos)) c_print(str("Declaration point de depart du bras"), True) c_print("First Pose " + str(x)) #Dessine un arc de cercle pour une translation de 10cm test_incr([0.00, 0.30, 0.00], x, kin, pub, pubGripper, 300) elif (inp == 'D'): pubGripper.publish("open", 70.0, 4.0) verif_move_gripper() c_print(str("Instanciation Kinematic UR10"), MODE_REDUCE_FULL) kin = Kinematics('ur10') c_print(str("Attente d'un message de JointState"), MODE_REDUCE_FULL) joint_states = rospy.wait_for_message("joint_states", JointState) joints_pos = joint_states.position c_print("JointState.position: " + str(tuple(joints_pos)), MODE_REDUCE_FULL) x = kin.forward(np.array(joints_pos)) c_print(str("Placement pour Pion 1"), True) x = [[0.00, -1.00, 0.00, 0.00], [0.00, 0.00, 1.00, 0.38], [-1.00, 0.00, 0.00, 0.19], [0., 0., 0., 1.]] c_print("First Pose " + str(x)) #print "Modif : ",x sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("close", 34.8, 4.0) verif_move_gripper() c_print(str("Deplacement pour Pion 1"), True) move_diag_right_top(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("open", 50.0, 4.0) verif_move_gripper() c_print(str("Position temporaire"), True) move_leave(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("open", 60.0, 4.0) verif_move_gripper() c_print(str("Placement pour Pion 2"), True) move_left(x) move_left(x) move_top(x) move_top(x) move_take(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("close", 34.8, 4.0) verif_move_gripper() c_print(str("Deplacement pour Pion 2"), True) move_diag_right_down(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("open", 50.0, 4.0) verif_move_gripper() c_print(str("Position temporaire"), True) move_leave(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" print send pub.publish(send) verif_move(sol) c_print(str("Placement Pion 1 mange Pion2"), True) move_diag_right_down(x) move_take(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("close", 34.8, 4.0) verif_move_gripper() c_print(str("Position temporaire"), True) move_leave(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) c_print(str("Placement pour manger Pion 2"), True) move_left(x) move_left(x) move_top(x) move_top(x) move_take(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("open", 50.0, 4.0) verif_move_gripper() c_print(str("Position temporaire"), True) move_leave(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) c_print(str("Pion qui meurt"), True) move_diag_right_down(x) move_take(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("close", 34.8, 4.0) verif_move_gripper() c_print(str("Position temporaire"), True) move_leave(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) c_print(str("Pion 2 mort"), True) move_kill(x) move_take(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("open", 50.0, 4.0) verif_move_gripper() elif (inp == 'K'): TestKin.main() else: print "Halting program" except KeyboardInterrupt: rospy.signal_shutdown("KeyboardInterrupt") raise
def stop_ros(reason): rospy.signal_shutdown(reason) roscpp_shutdown()
cmds = cmd results = [] for cmd in cmds: result = self._execSingle(cmd, shell, silent) results.append(result) return results else: return self._execSingle(cmd, shell, silent) def _execSingle(self, cmd, shell=False, silent=False): if not silent: rospy.loginfo('Executing command "%s"' % cmd) if not shell: cmd = cmd.split(' ') p = subprocess.Popen(cmd, shell=shell, stdout=PIPE, stderr=PIPE) (stdout, stderr) = p.communicate() returncode = p.returncode result = CmdResult(cmd, returncode, stdout, stderr) if result.failed and not silent: rospy.logerr('Command "%s" failed' % cmd) rospy.logerr(str(result)) return result if __name__ == '__main__': rospy.init_node('squirrel_website_builder') repository = rospy.get_param('~repository') Builder(repository).build() rospy.signal_shutdown('Build finished')
def __init__(self): global p1, p2 self.ERRORS = { 0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"), 0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"), 0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"), 0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"), 0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"), 0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"), 0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"), 0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"), 0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"), 0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"), 0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"), 0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"), 0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"), 0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"), 0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"), 0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"), 0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home") } rospy.init_node("roboclaw_node_f") rospy.on_shutdown(self.shutdown) rospy.loginfo("Connecting to roboclaw") dev_name_front = rospy.get_param("~dev_front", "/dev/ttyACM0") dev_name_back = rospy.get_param("~dev_back", "/dev/ttyACM1") baud_rate = int(rospy.get_param("~baud", "38400")) rospy.logwarn("after dev name") self.address_front = int(rospy.get_param("~address_front", "128")) self.address_back = int(rospy.get_param("~address_back", "129")) #check wheather the addresses are in range if self.address_front > 0x87 or self.address_front < 0x80 or self.address_back > 0x87 or self.address_back < 0x80: rospy.logfatal("Address out of range") rospy.signal_shutdown("Address out of range") # TODO need someway to check if address is correct try: p1 = roboclaw.Open(dev_name_front, baud_rate) except Exception as e: rospy.logfatal("Could not connect to front Roboclaw") rospy.logdebug(e) rospy.signal_shutdown("Could not connect to front Roboclaw") try: p2 = roboclaw.Open(dev_name_back, baud_rate) except Exception as e: rospy.logfatal("Could not connect to back Roboclaw") rospy.logdebug(e) rospy.signal_shutdown("Could not connect to back Roboclaw") #run diagnostics # self.updater = diagnostic_updater.Updater() ###check later may be do it for both the motors # self.updater.setHardwareID("Roboclaw") # self.updater.add(diagnostic_updater. # FunctionDiagnosticTask("Vitals", self.check_vitals)) #check if you can get the version of the roboclaw...mosly you dont try: version = roboclaw.ReadVersion(self.address_front, p1) except Exception as e: rospy.logwarn("Problem getting front roboclaw version") rospy.logdebug(e) pass if not version[0]: rospy.logwarn("Could not get version from front roboclaw") else: rospy.logdebug(repr(version[1])) try: version = roboclaw.ReadVersion(self.address_back, p2) except Exception as e: rospy.logwarn("Problem getting back roboclaw version") rospy.logdebug(e) pass if not version[0]: rospy.logwarn("Could not get version from back roboclaw") else: rospy.logdebug(repr(version[1])) # roboclaw.SpeedM1M2(self.address_front, 0, 0,p1) roboclaw.ResetEncoders(self.address_front, p1) # roboclaw.SpeedM1M2(self.address_back, 0, 0,p2) roboclaw.ResetEncoders(self.address_back, p2) self.MAX_SPEED = float(rospy.get_param("~max_speed", "1.1")) self.TICKS_PER_METER = float( rospy.get_param("~ticks_per_meter", "35818")) self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.1762")) self.BASE_LENGTH = float(rospy.get_param("~base_length", "0.2485")) self.WHEEL_RADIUS = float(rospy.get_param("~wheel_radius", "0.0635")) self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH, self.BASE_LENGTH, self.WHEEL_RADIUS) self.last_set_speed_time = rospy.get_rostime() rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback) rospy.Subscriber("gains", Twist, self.gain_callback) # rospy.sleep(1) rospy.logdebug("dev_front %s dev_back %s", dev_name_front, dev_name_back) rospy.logdebug("baud %d", baud_rate) rospy.logdebug("address_front %d address_back %d", self.address_front, self.address_back) rospy.logdebug("max_speed %f", self.MAX_SPEED) rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER) rospy.logdebug("base_width %f base_length %f", self.BASE_WIDTH, self.BASE_LENGTH)
def __init__(self): # Give the node a name rospy.init_node('nav_square', 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? rate = 20 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Set the parameters for the target square goal_distance = rospy.get_param("~goal_distance", 1.0) # meters goal_angle = radians(rospy.get_param( "~goal_angle", 90)) # degrees converted to radians linear_speed = rospy.get_param("~linear_speed", 0.2) # meters per second angular_speed = rospy.get_param("~angular_speed", 0.7) # radians per second angular_tolerance = radians(rospy.get_param("~angular_tolerance", 2)) # degrees to radians # 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 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) # 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() # Cycle through the four sides of the square for i in range(4): # 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 rotating move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1.0) # 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 # Begin the rotation 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 lopp delta_angle = normalize_angle(rotation - last_angle) turn_angle += delta_angle last_angle = rotation move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1.0) # Stop the robot when we are done self.cmd_vel.publish(Twist())
def __init__(self): self.ERRORS = {0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"), 0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"), 0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"), 0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"), 0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"), 0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"), 0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"), 0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"), 0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"), 0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"), 0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"), 0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"), 0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"), 0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"), 0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"), 0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"), 0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")} rospy.init_node("roboclaw_node", log_level=rospy.DEBUG) #TODO: remove 2nd param when done debugging rospy.on_shutdown(self.shutdown) rospy.loginfo("Connecting to roboclaw") self.dev_name = rospy.get_param("~dev", "/dev/ttyACM0") self.baud_rate = int(rospy.get_param("~baud", "115200")) 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 (Matt): figure out why roboclaw sometimes fails to Open try: roboclaw.Open(self.dev_name, self.baud_rate) except Exception as e: rospy.logfatal("Could not connect to Roboclaw") rospy.logdebug(e) rospy.signal_shutdown("Could not connect to Roboclaw") self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("Roboclaw") self.updater.add(diagnostic_updater. FunctionDiagnosticTask("Vitals", self.check_vitals)) try: version = roboclaw.ReadVersion(self.address) except Exception as e: rospy.logwarn("Problem getting roboclaw version") rospy.logdebug(e) pass if version is None: rospy.logwarn("Could not get version from roboclaw") else: rospy.logdebug(repr(version[1])) roboclaw.SpeedM1M2(self.address, 0, 0) roboclaw.ResetEncoders(self.address) self.LINEAR_MAX_SPEED = float(rospy.get_param("linear/x/max_velocity", "2.0")) self.ANGULAR_MAX_SPEED = float(rospy.get_param("angular/z/max_velocity", "2.0")) self.TICKS_PER_METER = float(rospy.get_param("tick_per_meter", "10")) self.BASE_WIDTH = float(rospy.get_param("base_width", "0.315")) self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH) self.last_set_speed_time = rospy.get_rostime() self.sub = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=5) self.TIMEOUT = 2 rospy.sleep(1) rospy.logdebug("dev %s", self.dev_name) rospy.logdebug("baud %d", self.baud_rate) rospy.logdebug("address %d", self.address) rospy.logdebug("max_speed %f", self.LINEAR_MAX_SPEED) rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER) rospy.logdebug("base_width %f", self.BASE_WIDTH)
def map_joystick(joystick, limb): """ maps joystick input to gripper commands @param joystick: an instance of a Joystick """ print("Getting robot state... ") rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION) init_state = rs.state() gripper = None original_deadzone = None def clean_shutdown(): if gripper and original_deadzone: gripper.set_dead_zone(original_deadzone) print("Exiting example.") try: gripper = intera_interface.Gripper(limb + '_gripper') except (ValueError, OSError) as e: rospy.logerr("Could not detect an electric gripper attached to the robot.") clean_shutdown() return rospy.on_shutdown(clean_shutdown) # abbreviations jhi = lambda s: joystick.stick_value(s) > 0 jlo = lambda s: joystick.stick_value(s) < 0 bdn = joystick.button_down bup = joystick.button_up def print_help(bindings_list): print("Press Ctrl-C to quit.") for bindings in bindings_list: for (test, _cmd, doc) in bindings: if callable(doc): doc = doc() print("%s: %s" % (str(test[1]), doc)) def offset_position(offset_pos): cmd_pos = max(min(gripper.get_position() + offset_pos, gripper.MAX_POSITION), gripper.MIN_POSITION) gripper.set_position(cmd_pos) print("commanded position set to {0} m".format(cmd_pos)) def update_velocity(offset_vel): cmd_speed = max(min(gripper.get_cmd_velocity() + offset_vel, gripper.MAX_VELOCITY), gripper.MIN_VELOCITY) gripper.set_cmd_velocity(cmd_speed) print("commanded velocity set to {0} m/s".format(cmd_speed)) # decrease position dead_zone original_deadzone = gripper.get_dead_zone() # WARNING: setting the deadzone below this can cause oscillations in # the gripper position. However, setting the deadzone to this # value is required to achieve the incremental commands in this example gripper.set_dead_zone(0.001) rospy.loginfo("Gripper deadzone set to {}".format(gripper.get_dead_zone())) num_steps = 8.0 percent_delta = 1.0 / num_steps position_increment = (gripper.MAX_POSITION - gripper.MIN_POSITION) * percent_delta velocity_increment = (gripper.MAX_VELOCITY - gripper.MIN_VELOCITY) * percent_delta bindings_list = [] bindings = ( #(test, command, description) ((bdn, ['btnLeft']), (gripper.reboot, []), "reboot"), ((bdn, ['btnUp']), (gripper.calibrate, []), "calibrate"), ((bdn, ['leftTrigger']), (gripper.close, []), "close"), ((bup, ['leftTrigger']), (gripper.open, []), "open (release)"), ((bdn, ['leftBumper']), (gripper.stop, []), "stop"), ((jlo, ['leftStickVert']), (offset_position, [-position_increment]), "decrease position"), ((jhi, ['leftStickVert']), (offset_position, [position_increment]), "increase position"), ((jlo, ['rightStickVert']), (update_velocity, [-velocity_increment]), "decrease commanded velocity"), ((jhi, ['rightStickVert']), (update_velocity, [velocity_increment]), "increase commanded velocity"), ((bdn, ['function1']), (print_help, [bindings_list]), "help"), ((bdn, ['function2']), (print_help, [bindings_list]), "help"), ) bindings_list.append(bindings) rospy.loginfo("Enabling robot...") rs.enable() rate = rospy.Rate(100) print_help(bindings_list) print("Press <Start> button for help; Ctrl-C to stop...") while not rospy.is_shutdown(): # test each joystick condition and call binding cmd if true for (test, cmd, doc) in bindings: if test[0](*test[1]): print(doc) cmd[0](*cmd[1]) rate.sleep() rospy.signal_shutdown("Example finished.")
rospy.sleep(10) ## TODO: Should times for .sleep be configurable?? rospy.loginfo("Reinitializing Node") else: ## Everything is initialised and ready. Wait for requests: try: rospy.loginfo("...Image Adaptor initialized...") except KeyboardInterrupt: rospy.loginfo("Shutting down node.") else: rospy.spin() return ############################################################################### ## ***** ***** ENTRY POINT FOR STARTING THE CODE ***** ***** ############################################################################### if __name__ == '__main__': try: basename = sys.argv[0].split('/')[-1] if basename == "": basename = sys.argv[0].split('/')[-2] if basename[-3:] == ".py": basename = basename[0:-3] mainFunction(basename) except Exception as e: rospy.logerr("WHOLE-SCOPE EXCEPTION - NODE SHUTTING DOWN") rospy.logerr(("Error: %s." % e).center(80, '*')) finally: rospy.signal_shutdown("testing shutdown")
if dynamixel_io.exception: raise dynamixel_io.exception except dynamixel_io.FatalErrorCodeError, fece: rospy.logerr(fece) except dynamixel_io.NonfatalErrorCodeError, nfece: self.error_counts['non_fatal'] += 1 rospy.logdebug(nfece) except dynamixel_io.ChecksumError, cse: self.error_counts['checksum'] += 1 rospy.logdebug(cse) except dynamixel_io.DroppedPacketError, dpe: self.error_counts['dropped'] += 1 rospy.logdebug(dpe.message) except OSError, ose: if ose.errno != errno.EAGAIN: rospy.logfatal(errno.errorcode[ose.errno]) rospy.signal_shutdown(errno.errorcode[ose.errno]) if motor_states: msl = MotorStateList() msl.motor_states = motor_states self.motor_states_pub.publish(msl) self.current_state = msl # calculate actual update rate current_time = rospy.Time.now() rates.append(1.0 / (current_time - last_time).to_sec()) self.actual_rate = round(sum(rates) / num_events, 2) last_time = current_time rate.sleep()
def run(self): # ====== Setup section ===== counter = 0 counter_waypoint = 0 counter_datasave = 0 counter_total_datasaved = 0 timestamp = 0 # ====== Setup section ===== while not rospy.is_shutdown(): if self.init: data_timestamp.append(timestamp) data_temperature.append(self.currentTemperature) data_salinity.append(self.currentSalinity) data_x.append(self.vehicle_pos[0]) data_y.append(self.vehicle_pos[1]) data_z.append(self.vehicle_pos[-1]) data_lat.append(lat4) data_lon.append(lon4) if counter_datasave >= 10: save_data(datapath, data_timestamp, data_lat, data_lon, data_x, data_y, data_z, data_salinity, data_temperature) print("Data saved {:d} times".format(counter_total_datasaved)) counter_datasave = 0 counter_total_datasaved = counter_total_datasaved + 1 timestamp = timestamp + 1 counter_datasave = counter_datasave + 1 if self.auv_handler.getState() == "waiting": if counter_waypoint == 0: self.auv_handler.setWaypoint(deg2rad(lat_start), deg2rad(lon_start), depth_start) else: xcand, ycand, zcand = find_candidates_loc(xnow, ynow, znow, N1, N2, N3) t1 = time.time() xnext, ynext, znext = find_next_EIBV_1D(xcand, ycand, zcand, xnow, ynow, znow, xpre, ypre, zpre, N1, N2, N3, Sigma_cond, mu_cond, tau_sal, Threshold_S) t2 = time.time() t_elapsed.append(t2 - t1) print("It takes {:.2f} seconds to compute the next waypoint".format(t2 - t1)) print("next is ", xnext, ynext, znext) lat_next, lon_next = xy2latlon(xnext, ynext, origin, distance, alpha) depth_next = depth_obs[znext] ind_next = ravel_index([xnext, ynext, znext], N1, N2, N3) F = np.zeros([1, N]) F[0, ind_next] = True print("Arrived the starting location") save_data(datapath, data_timestamp, data_lat, data_lon, data_x, data_y, data_z, data_salinity, data_temperature) counter_total_datasaved = counter_total_datasaved + 1 print("Data saved {:02d} times".format(counter_total_datasaved)) print("Move to new way point, lat: {:.2f}, lon: {:.2f}, depth: {:.2f}".format(lat_next, lon_next, depth_next)) sal_sampled = np.mean(data_salinity[-10:]) # take the past ten samples and average mu_cond, Sigma_cond = GPupd(mu_cond, Sigma_cond, R, F, sal_sampled) xpre, ypre, zpre = xnow, ynow, znow xnow, ynow, znow = xnext, ynext, znext path.append([xnow, ynow, znow]) path_cand.append([xcand, ycand, zcand]) coords.append([lat_next, lon_next]) mu.append(mu_cond) Sigma.append(Sigma_cond) save_ESdata(datapath, path, path_cand, coords, mu, Sigma, t_elapsed) # Move to the next waypoint self.auv_handler.setWaypoint(deg2rad(lat_next), deg2rad(lon_next), depth_next) counter_waypoint = counter_waypoint + 1 print("Waypoint is ", counter_waypoint) if counter_waypoint >= N_steps: rospy.signal_shutdown("Mission completed!!!")
rate = rospy.Rate(10) while all((pub.get_num_connections() < num_subs, rospy.Time.now() < time_end, not rospy.is_shutdown())): rate.sleep() return (pub.get_num_connections() >= num_subs) if __name__ == '__main__': rospy.init_node('motoman_test') pub_traj = rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory, queue_size=1) if not WaitForSubscribers(pub_traj, 3.0): print 'WARNING: No subscribers of /joint_path_command' joint_names = [ 'joint_' + jkey for jkey in ('s', 'l', 'e', 'u', 'r', 'b', 't') ] joint_names = rospy.get_param('controller_joint_names') t_traj = [4.0, 6.0, 8.0, 12.0] q_traj = [[0.0] * 7, [0.1, -0.3, 0.15, -0.7, 0.1, -0.3, 0.15], [0.21, -0.59, 0.30, -1.46, 0.35, -0.68, 0.31], [0.0] * 7] dq_traj = [[0.0] * 7] * 4 traj = ToROSTrajectory(joint_names, q_traj, t_traj, dq_traj) pub_traj.publish(traj) rospy.signal_shutdown('Done.')
def create_window(): """ Creates a GUI window to publish a pose. """ master = Tkinter.Tk() height_min_box = Tkinter.Scale(master, command=height_min, from_=MIN_DISTANCE, to=MAX_DISTANCE, resolution=LINEAR_RESOLUTION, label="Height (Minimum)") height_min_box.grid(row=0, column=0) height_max_box = Tkinter.Scale(master, command=height_max, from_=MIN_DISTANCE, to=MAX_DISTANCE, resolution=LINEAR_RESOLUTION, label="Height (Maximum)") height_max_box.grid(row=1, column=0) zenith_min_box = Tkinter.Scale(master, command=zenith_min, from_=MIN_ORIENTATION, to=MAX_ORIENTATION, resolution=ANGULAR_RESOLUTION, label="Zenith (Minimum)") zenith_min_box.grid(row=0, column=1) height_max_box = Tkinter.Scale(master, command=zenith_max, from_=MIN_ORIENTATION, to=MAX_ORIENTATION, resolution=ANGULAR_RESOLUTION, label="Zenith (Maximum)") height_max_box.grid(row=1, column=1) azimuth_min_box = Tkinter.Scale(master, command=azimuth_min, from_=MIN_ORIENTATION, to=MAX_ORIENTATION, resolution=ANGULAR_RESOLUTION, label="Azimuth (Minimum)") azimuth_min_box.grid(row=0, column=2) azimuth_max_box = Tkinter.Scale(master, command=azimuth_max, from_=MIN_ORIENTATION, to=MAX_ORIENTATION, resolution=ANGULAR_RESOLUTION, label="Azimuth (Maximum)") azimuth_max_box.grid(row=1, column=2) yaw_min_box = Tkinter.Scale(master, command=yaw_min, from_=MIN_ORIENTATION, to=MAX_ORIENTATION, resolution=ANGULAR_RESOLUTION, label="Yaw (Minimum)") yaw_min_box.grid(row=0, column=3) yaw_max_box = Tkinter.Scale(master, command=yaw_max, from_=MIN_ORIENTATION, to=MAX_ORIENTATION, resolution=ANGULAR_RESOLUTION, label="Yaw (Maximum)") yaw_max_box.grid(row=1, column=3) radial_min_box = Tkinter.Scale(master, command=radial_min, from_=MIN_DISTANCE, to=MAX_DISTANCE, resolution=LINEAR_RESOLUTION, label="Radial distance (Minimum)") radial_min_box.grid(row=0, column=4) radial_max_box = Tkinter.Scale(master, command=radial_max, from_=MIN_DISTANCE, to=MAX_DISTANCE, resolution=LINEAR_RESOLUTION, label="Radial distance (Maximum)") radial_max_box.grid(row=1, column=4) master.title("Sampling parameters GUI") master.mainloop() rospy.signal_shutdown("GUI closed")
def __init__(self, port=None, baud=57600, timeout=5.0): """ Initialize node, connect to bus, attempt to negotiate topics. """ self.mutex = thread.allocate_lock() self.lastsync = rospy.Time(0) self.lastsync_lost = rospy.Time(0) self.timeout = timeout self.synced = False self.pub_diagnostics = rospy.Publisher( '/diagnostics', diagnostic_msgs.msg.DiagnosticArray, queue_size=10) if port == None: # no port specified, listen for any new port? pass elif hasattr(port, 'read'): #assume its a filelike object self.port = port else: # open a specific port try: self.port = Serial(port, baud, timeout=self.timeout * 0.5) except SerialException as e: rospy.logerr("Error opening serial: %s", e) rospy.signal_shutdown("Error opening serial: %s" % e) raise SystemExit self.port.timeout = 0.01 # Edit the port timeout time.sleep(0.1) # Wait for ready (patch for Uno) # hydro introduces protocol ver2 which must match node_handle.h # The protocol version is sent as the 2nd sync byte emitted by each end self.protocol_ver1 = '\xff' self.protocol_ver2 = '\xfe' self.protocol_ver = self.protocol_ver2 self.publishers = dict() # id:Publishers self.subscribers = dict() # topic:Subscriber self.services = dict() # topic:Service self.buffer_out = -1 self.buffer_in = -1 self.callbacks = dict() # endpoints for creating new pubs/subs self.callbacks[TopicInfo.ID_PUBLISHER] = self.setupPublisher self.callbacks[TopicInfo.ID_SUBSCRIBER] = self.setupSubscriber # service client/servers have 2 creation endpoints (a publisher and a subscriber) self.callbacks[ TopicInfo.ID_SERVICE_SERVER + TopicInfo.ID_PUBLISHER] = self.setupServiceServerPublisher self.callbacks[ TopicInfo.ID_SERVICE_SERVER + TopicInfo.ID_SUBSCRIBER] = self.setupServiceServerSubscriber self.callbacks[ TopicInfo.ID_SERVICE_CLIENT + TopicInfo.ID_PUBLISHER] = self.setupServiceClientPublisher self.callbacks[ TopicInfo.ID_SERVICE_CLIENT + TopicInfo.ID_SUBSCRIBER] = self.setupServiceClientSubscriber # custom endpoints self.callbacks[ TopicInfo.ID_PARAMETER_REQUEST] = self.handleParameterRequest self.callbacks[TopicInfo.ID_LOG] = self.handleLoggingRequest self.callbacks[TopicInfo.ID_TIME] = self.handleTimeRequest rospy.sleep(2.0) # TODO self.requestTopics() self.lastsync = rospy.Time.now() signal.signal(signal.SIGINT, self.txStopRequest)
def temperature_process(temperature_data): # turn ROS message into a numpy array tempData = np.asarray(temperature_data.data) # calibration routine global calib_counter global pix_calib global deadpixel_map calib_frames = 20 if calib_counter < calib_frames: # for each pixel, get the average value over x frames pix_calib = np.add(pix_calib, tempData) print "Obtained Calibration image #", calib_counter + 1 calib_counter += 1 elif calib_counter == calib_frames: pix_calib = pix_calib / (calib_frames) meanpixel = np.mean(pix_calib) for i in range(0, PIXEL_DATA_SIZE): if (pix_calib[i] > meanpixel + 240) or (pix_calib[i] < meanpixel - 240): print("dead pixel at location {0} ({1} vs {2})".format( i, pix_calib[i], meanpixel)) deadpixel_map = np.append(deadpixel_map, [i]) calib_counter += 1 print("dead pixels identified at locations:") print np.size(deadpixel_map) - 1, ' dead pixels detected' print "Calibration finished" else: # replace dead pixels with the pixel value of its neighboor (lazy, i know) for i in range(1, len(deadpixel_map)): tempData[deadpixel_map[i]] = tempData[deadpixel_map[i] - 1] calib_gain = 2 # Temperature data into human viewable image scaling_array = np.subtract(tempData, pix_calib) * calib_gain temp_min = np.ndarray.min(scaling_array) temp_max = np.ndarray.max(scaling_array) # temp_min = -100 # temp_max = 100 temp_range = np.float(temp_max - temp_min) floats = np.array((scaling_array - temp_min) / (temp_range), dtype=float) img = np.array(floats * 255, dtype=np.uint8) # now put the pixels into a opencv image compatible numpy array cvimg = np.zeros(PIXEL_DATA_SIZE, dtype=np.uint8) for i in range(0, PIXEL_DATA_SIZE): cvimg[(PIXEL_DATA_SIZE - ((i / 384) + 1) * 384 + i % 384)] = np.uint8(floats[i] * 255) cvimg = np.reshape(cvimg, (288, -1)) # display the image cv2.imshow('cvimg', cvimg) cv2.moveWindow('cvimg', 50, 50) waitkey = cv2.waitKey(1) if waitkey == 27 or waitkey == 113: # 'esc' and 'q' keys print('exit key pressed') rospy.signal_shutdown('exit key pressed') elif waitkey == 99: # 'c' key print "C key pressed - recalibrating. Keep the lens Covered." calib_counter = 0
# Creates the SimpleActionClient, passing the type of the action # (FibonacciAction) to the constructor. client = actionlib.SimpleActionClient('/arm/joint_trajectory_controller/follow_joint_trajectory', FollowJointTrajectoryAction) # Waits until the action server has started up and started # listening for goals. client.wait_for_server(rospy.Duration(5.0)) # verify joint trajectory action servers are available client_server = client.wait_for_server(rospy.Duration(10.0)) if not client_server: msg = ("Action server not available." " Verify action server availability.") rospy.logerr(msg) rospy.signal_shutdown(msg) sys.exit(1) # Creates a goal to send to the action server. goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = ['arm_elbow_joint',\ 'arm_shoulder_lift_joint', 'arm_shoulder_pan_joint',\ 'arm_wrist_1_joint', 'arm_wrist_2_joint', 'arm_wrist_3_joint'] goal.trajectory.points = [] point0 = JointTrajectoryPoint() point0.positions = [0.0,0.0,0.0,0.0,0.0,0.0] point0.velocities = [0.0,0.0,0.0,0.0,0.0,0.0] # To be reached 1 second after starting along the trajectory point0.time_from_start = rospy.Duration(1.0) goal.trajectory.points.append(point0)