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)
Beispiel #4
0
    def _shutdown_by_reactor(self):
        """
        Reactor shutdown callback.  Sends a signal to rospy to shutdown the ROS
        interfaces
        """

        rospy.signal_shutdown("Reactor shutting down.")
Beispiel #5
0
    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");
Beispiel #6
0
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()
Beispiel #8
0
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)
Beispiel #10
0
def main(argv):
    app = wx.App()
    KeyEventFrame()
    print(__doc__)
    app.MainLoop()
    rospy.signal_shutdown("MainLoop")
    return 0
Beispiel #11
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!"
Beispiel #14
0
	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")
Beispiel #15
0
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.")
Beispiel #17
0
    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]))
Beispiel #20
0
 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	
Beispiel #21
0
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)
Beispiel #22
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)
Beispiel #26
0
 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
Beispiel #28
0
    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()
Beispiel #29
0
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
Beispiel #30
0
 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()
Beispiel #32
0
	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)
Beispiel #33
0
                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()

Beispiel #36
0
    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))
Beispiel #39
0
    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()
Beispiel #42
0
        #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.")
Beispiel #43
0
                                       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)
Beispiel #46
0
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
Beispiel #47
0
def stop_ros(reason):
    rospy.signal_shutdown(reason)
    roscpp_shutdown()
Beispiel #48
0
            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')
Beispiel #49
0
    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)
Beispiel #50
0
    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())
Beispiel #51
0
    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)
Beispiel #52
0
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")
Beispiel #54
0
                        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()
Beispiel #55
0
    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!!!")
Beispiel #56
0
    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.')
Beispiel #57
0
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")
Beispiel #58
0
    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)
Beispiel #59
0
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)