示例#1
0
 def call_service(self):
     rospy.wait_for_service('/zeroconf/add_listener')
     try:
         add_listener = rospy.ServiceProxy('/zeroconf/add_listener', AddListener)
         r = add_listener('_workstation._tcp')
     except rospy.ServiceException, e:
         print "Servce call failed: %s"%e
示例#2
0
    def __init__(self):
        """
        JARVIS User Interface Node
        """
        rospy.on_shutdown(self.cleanup)
        rospy.wait_for_service('return_grips')
        self.msg = GoalID()
        self.newGrips = jarvis_perception.msg.GraspArray()

        self.legalUtterances = ['upper','lower','top','bottom','back','front','near','far','rear','left','right','anywhere','center']
        self.lastUtterance = ''
        self.axisAlignedBox = ''
        self.person_trans = ''
        self.person_rot = ''

        plotting = True

        self.likelihood = None
        self.var = None

        try:
            grip_server = rospy.ServiceProxy('return_grips',ReturnGrips)
            self.grips = grip_server()
            print self.grips
            pubgrips = rospy.Publisher('return_grips', jarvis_perception.msg.GraspArray, queue_size=10)
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e
示例#3
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");
示例#4
0
  def test_basic_localization(self):
    global_localization = int(sys.argv[1])
    self.target_x = float(sys.argv[2])
    self.target_y = float(sys.argv[3])
    self.target_a = float(sys.argv[4])
    tolerance_d = float(sys.argv[5])
    tolerance_a = float(sys.argv[6])
    target_time = float(sys.argv[7])

    if global_localization == 1:
      #print 'Waiting for service global_localization'
      rospy.wait_for_service('global_localization')
      global_localization = rospy.ServiceProxy('global_localization', Empty)
      resp = global_localization()

    rospy.init_node('test', anonymous=True)
    while(rospy.rostime.get_time() == 0.0):
      #print 'Waiting for initial time publication'
      time.sleep(0.1)
    start_time = rospy.rostime.get_time()
    # TODO: This should be replace by a pytf listener
    rospy.Subscriber('/tf', tfMessage, self.tf_cb)

    while (rospy.rostime.get_time() - start_time) < target_time:
      #print 'Waiting for end time %.6f (current: %.6f)'%(target_time,(rospy.rostime.get_time() - start_time))
      time.sleep(0.1)

    (a_curr, a_diff) = self.compute_angle_diff()
    print 'Curr:\t %16.6f %16.6f %16.6f' % (self.tf.translation.x, self.tf.translation.y, a_curr)
    print 'Target:\t %16.6f %16.6f %16.6f' % (self.target_x, self.target_y, self.target_a)
    print 'Diff:\t %16.6f %16.6f %16.6f' % (abs(self.tf.translation.x-self.target_x),abs(self.tf.translation.y - self.target_y), a_diff)
    self.assertNotEquals(self.tf, None)
    self.assertTrue(abs(self.tf.translation.x - self.target_x) <= tolerance_d)
    self.assertTrue(abs(self.tf.translation.y - self.target_y) <= tolerance_d)
    self.assertTrue(a_diff <= tolerance_a)
示例#5
0
    def _setup_ik(self):
        '''Sets up services for inverse kinematics'''
        ik_info_srv_name = ('pr2_' + self._side() +
                            '_arm_kinematics_simple/get_ik_solver_info')
        ik_srv_name = 'pr2_' + self._side() + '_arm_kinematics_simple/get_ik'
        rospy.wait_for_service(ik_info_srv_name)
        ik_info_srv = rospy.ServiceProxy(ik_info_srv_name,
                                         GetKinematicSolverInfo)
        solver_info = ik_info_srv()
        rospy.loginfo('IK info service has responded for '
                      + self._side() + ' arm.')
        rospy.wait_for_service(ik_srv_name)
        self.ik_srv = rospy.ServiceProxy(ik_srv_name,
                                         GetPositionIK, persistent=True)
        rospy.loginfo('IK service has responded for ' + self._side() + ' arm.')

        # Set up common parts of an IK request
        self.ik_request = GetPositionIKRequest()
        self.ik_request.timeout = rospy.Duration(4.0)
        self.ik_joints = solver_info.kinematic_solver_info.joint_names
        self.ik_limits = solver_info.kinematic_solver_info.limits
        ik_links = solver_info.kinematic_solver_info.link_names

        request = self.ik_request.ik_request
        request.ik_link_name = ik_links[0]
        request.pose_stamped.header.frame_id = 'base_link'
        request.ik_seed_state.joint_state.name = self.ik_joints
        request.ik_seed_state.joint_state.position = [0] * len(self.ik_joints)
示例#6
0
def turtlerun():
    #T = input('PLease enter the number of T:')
    pub = rospy.Publisher('/turtlesim1/turtle1/cmd_vel',Twist, queue_size=10)
    rate = rospy.Rate(50) 
    
    rospy.wait_for_service('/turtlesim1/turtle1/teleport_absolute')
    start = rospy.ServiceProxy('/turtlesim1/turtle1/teleport_absolute',TeleportAbsolute)
    start(5.5,5.5,0.5)	

    T = rospy.get_param("turtlesim1/T")

    t0 = rospy.get_time()

    while not rospy.is_shutdown():
        now_time = rospy.get_time()
        t = now_time-t0
        x = 3*sin((4*pi*t)/T)
        y = 3*sin((2*pi*t)/T)
        dx = 12*pi*cos((4.0*pi*t)/T)/T
        dy = 6*pi*cos((2.0*pi*t)/T)/T
        v = sqrt(dx*dx+dy*dy)
        dx2 = -48*pi*pi*sin((4*pi*t)/T)/(T*T)
        dy2 = -12*pi*pi*sin((2*pi*t)/T)/(T*T)
        w = ((dx*dy2)-(dy*dx2)) /((dx*dx) + (dy*dy))
        
        turtlemove = Twist(Vector3(v,0,0),Vector3(0,0,w))
        rospy.loginfo(turtlemove)
        pub.publish(turtlemove)

        rate.sleep() 
示例#7
0
    def _threadedCall(self, msg):
        rosMsg = rospy.AnyMsg()
        rosMsg._buff = msg

        rospy.wait_for_service(self._addr[1], timeout=5)
        serviceFunc = rospy.ServiceProxy(self._addr[1], self._srvCls)
        return serviceFunc(rosMsg)
示例#8
0
def talker():
    rospy.init_node('random_motors', anonymous=False)
    r = rospy.Rate(0.5) # 10hz
    joints = [rospy.get_param("~pan_name", 'pan_joint'), rospy.get_param("~tilt_name", 'tilt_joint')]
    torque_enable = dict()
    servo_position = dict()

    #Configure joints
    for controller in sorted(joints):
        try:
            rospy.loginfo("controller is "+controller)

            # Torque enable/disable control for each servo
            torque_service = '/' + controller + '/torque_enable'
            rospy.wait_for_service(torque_service)
            torque_enable[controller] = rospy.ServiceProxy(torque_service, TorqueEnable)

            # Start each servo in the disabled state so we can move them by hand
            torque_enable[controller](False)

            # The position controllers
            servo_position[controller] = rospy.Publisher('/' + controller + '/command', Float64)
        except:
            rospy.logerr("Can't contact servo services!")

    while not rospy.is_shutdown():
        for controller in sorted(joints):
            rand_val = uniform(-2,2)
            #Send random float to each joint
            servo_position[controller].publish(rand_val)
        r.sleep()
    """
    def verify_boards(self, check):
        if self.has_finished:
            return False

        try:
            rospy.wait_for_service('mcb_conf_results', 15)
        except:
            msg = "MCB conf results service not available. Unable to configure MCB's"
            rospy.logerr(msg)
            self.finished(False, msg)
            return False

        if not self.count_boards():
            return False

        if not self.check_link():
            return False

        if not self.get_serials():
            return False
        
        if check:
            return self.check_boards()

        return True
示例#10
0
def test():
    rospy.init_node('test')
    loginfo("Initialized node Controller")

    rospy.wait_for_service("/move_end_effector_trajectory")
    joint_action_server = rospy.ServiceProxy("/move_end_effector_trajectory", JointAction)
    tool_trajectory = rospy.ServiceProxy("/move_tool_trajectory", JointAction)
    loginfo("Initialized Joint Action Server Proxy")
    rospy.wait_for_service("/end_effector_position")
    position_server = rospy.ServiceProxy("/end_effector_position", EndEffectorPosition)
    loginfo("Initialized position server proxy")

#    rospy.sleep(5.0)

    loginfo("Making position call")
    initial_position = position_server().position
    loginfo(initial_position)
    joint_action_server([5.0, 10.0],
                        [Vector3(initial_position.x + 0.1*np.random.rand(),
                                 initial_position.y + 0.1*np.random.rand(),
                                 initial_position.z + 0.1*np.random.rand()),
                         initial_position],
                        [Vector3(0.11, 0.0, 0.0),
                         Vector3(0.0, 0.0, 0.0)])
    loginfo(position_server().position.x - initial_position.x)      
    loginfo(position_server().position.y - initial_position.y)      
    loginfo(position_server().position.z - initial_position.z)      
    def send_results(self, test_result):
        if self.data_sent:
            return 

        rospy.wait_for_service('/test_result', 15)
        self.result_service.call(test_result)
        self.data_sent = True
    def reset_iris_neutral_value(self):

        if self.dic_sequence_services['checked_sequence_of_missions'] == True:

                new_service = {}
                trigger_instant = self._widget.TriggerInstantNeutral.value()
                new_service['trigger_instant'] = trigger_instant
                self.dic_sequence_services['last_trigger_time'] = trigger_instant

                new_service['service_name']    = 'IrisPlusResetNeutral'
                new_service['inputs_service']  = {}
                self.dic_sequence_services['list_sequence_services'].append(new_service)

        else:
            try: 
                # time out of one second for waiting for service
                rospy.wait_for_service(self.namespace+'IrisPlusResetNeutral',1.0)
                try:
                    # request reseting neutral value for iris+ (implicit, with keywords)
                    ResetingNeutral = rospy.ServiceProxy(self.namespace+'IrisPlusResetNeutral', IrisPlusResetNeutral)
                    reply = ResetingNeutral()
                    if reply.received == True:
                        self._widget.ThrottleNeutralValue.setValue(reply.k_trottle_neutral)
                        rospy.logwarn('Service for reseting neutral value succeded')
                except rospy.ServiceException as exc:
                    rospy.logwarn("Service did not process request: " + str(exc))
                    rospy.logwarn('Service for reseting neutral value failed')
                    pass
            except rospy.ServiceException as exc:
                rospy.logwarn("Service did not process request: " + str(exc))
                rospy.logwarn('Service for reseting neutral value failed')      
                pass
示例#13
0
def moveBBSrv(req):
    
        try:
            rospy.wait_for_service('scan_base_pose',10)
        except rospy.ROSException, e:
            print "Service not available: %s"%e
            outcome_detectObjectSrv = 'failed'
示例#14
0
文件: aes.py 项目: Room1104/qtgui
def calltrig(name):
    rospy.wait_for_service(name)
    try:
        s = rospy.ServiceProxy(name,std_srvs.srv.Trigger)
        resp = s()
    except rospy.ServiceException,e:
        print "Failed: %s" % e
    def send_list_of_services(self):
        # # debug
        # for service in self.dic_sequence_services['list_sequence_services']:
        #     print(service)
        #     print('\n\n')

        # request service
        try: 
            # time out of one second for waiting for service
            rospy.wait_for_service(self.namespace+'ServiceSequencer',1.0)
            
            try:
                request = rospy.ServiceProxy(self.namespace+'ServiceSequencer', ServiceSequence)

                service_sequence = json.dumps(self.dic_sequence_services['list_sequence_services'])
                reply = request(service_sequence = service_sequence)

                if reply.received == True:
                    # if controller receives message
                    print('Success')

            except rospy.ServiceException as exc:
                rospy.logwarn("Service did not process request: " + str(exc))
            
        except rospy.ServiceException as exc:
            rospy.logwarn("Service did not process request: " + str(exc))
    def ResetSimulator(self):
        
        try: 
            # time out of one second for waiting for service
            rospy.wait_for_service(self.namespace+'ResetSimulator',1.0)
            
            try:
                AskForStart = rospy.ServiceProxy(self.namespace+'ResetSimulator', StartSim)

                # if button is pressed save data
                if self._widget.ButtonSTART_SIM.isChecked():
                    # request controller to save data
                    reply = AskForStart(True)
                    if reply.Started == True:
                        # if controller receives message, we know it
                        # print('Reseted')
                        self._widget.SimulatorSuccess.setChecked(True) 
                        self._widget.SimulatorFailure.setChecked(False) 
                    else:
                        self._widget.SimulatorSuccess.setChecked(False) 
                        self._widget.SimulatorFailure.setChecked(True)                         
                # else: do nothing

            except rospy.ServiceException:
                # print "Service call failed: %s"%e   
                self._widget.SimulatorSuccess.setChecked(False) 
                self._widget.SimulatorFailure.setChecked(True) 
            
        except: 
            # print "Service not available ..."        
            self._widget.SimulatorSuccess.setChecked(False) 
            self._widget.SimulatorFailure.setChecked(True)
            pass  
示例#17
0
	def get_path(self, request):
		print "Waiting for path service"
		rospy.wait_for_service('/ogmpp_path_planners/plan')
		print "Service ok"
		try:
			pathSrv = rospy.ServiceProxy('/ogmpp_path_planners/plan', OgmppPathPlanningSrv)
			path = OgmppPathPlanningSrvRequest()
			path.method = "uniform_prm"
			path.data.begin.x = self.robot_x
			path.data.begin.y = self.robot_y
			path.data.end.x = request.x
			path.data.end.y = request.y
			
			pathRes = pathSrv(path)
			#~ print pathRes
			
			ros_path = Path()
			
			ros_path.header.frame_id = "map"
			_map = OccupancyGrid()
			
			for p in self.path:
				ps = PoseStamped()
				ps.header.frame_id = "map"
				ps.pose.position.x = p[0] * _map.data.info.resolution + \
					_map.data.info.origin.position.x
				ps.pose.position.y = p[1] * _map.data.info.resolution + \
					_map.data.info.origin.position.y
				print ps
				ros_path.poses.append(ps)
			
			#~ self.path_publisher.publish(ros_path)

		except rospy.ServiceException, e:
			print "Service call failed: %s"%e
def update_kalman(ar_tags):
    print "started update"
    rospy.wait_for_service('innovation')
    update = rospy.ServiceProxy('innovation', NuSrv)
    listener = tf.TransformListener()
    while True:
        try:
            try:
               (trans, rot) = listener.lookupTransform(ar_tags['arZ'], ar_tags['ar1'], rospy.Time(0))
            except:
	       print "Couldn't look up transform"
               continue
            lin = Vector3()
            quat = Quaternion()
            lin.x = trans[0]
            lin.y = trans[1]
            lin.z = trans[2]
            quat.x = rot[0]
            quat.y = rot[1]
            quat.z = rot[2]
            quat.w = rot[3]
            transform = Transform()
            transform.translation = lin
            transform.rotation = quat
            test = update(transform, ar_tags['ar1'])
            print "Service call succeeded"
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e
    def UnArming_Quad(self,base_name=""):
        #This function is used to arm the quad."""

        #Un-Arming the Quad
        srv_path = self.namespace+'mavros/cmd/arming'
        # if base_name!="":
            # srv_path = "/%s/%s"%(base_name,srv_path)

        try:
            rospy.logwarn('Un-Arming Quad ...')
            rospy.wait_for_service(srv_path,2.0)

            try:
                arming = rospy.ServiceProxy(srv_path,CommandBool)
                arming_result=arming(False)
                if arming_result.success:
                    rospy.logwarn('Quad is Un-Armed!!!!')
                    # return True
                else:
                    rospy.logwarn('Cannot Un-arm quad')
                    # return False

            except:
                rospy.logwarn('Cannot Un-arm quad')
                # return False

        except:
            rospy.logwarn('No connection to Mavros')
示例#20
0
 def recoveryAction(self, error):
     if error == ErrorLevel.INFORMATIVE:
         rospy.loginfo("%s: recovery action %s: INFORMATIVE", self.name, error)
     elif error == ErrorLevel.ASK_AUTHORIZATION:
         # TODO: Unused!!!
         rospy.loginfo("%s: recovery action %s: ASK_AUTHORIZATION", self.name, error)
     elif error == ErrorLevel.ABORT_MISSION:
         self.abortMission()
     elif error == ErrorLevel.CONTROLLED_SURFACE:
         self.controlledSurface()
     elif error == ErrorLevel.SURFACE:
         self.surface()
     elif error == ErrorLevel.EMERGENCY_SURFACE:
         self.emergencySurface()
     elif error == ErrorLevel.DISABLE_PAYLOAD:
         try:
             rospy.wait_for_service("digital_output", 5)
             digital_out_service = rospy.ServiceProxy("digital_output", DigitalOutput)
             digital_output = DigitalOutputRequest()
             digital_output.digital_out = 3
             digital_output.value = False
             digital_out_service(digital_output)
         except rospy.exceptions.ROSException:
             rospy.logerr("%s, Error disabling payload.", self.name)
     else:
         rospy.loginfo("%s: recovery action %s: INVALID ERROR CODE", self.name, error)
示例#21
0
def ik_test(limb, point, orient):
    rospy.init_node("rsdk_ik_service_client")
    ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
    iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
    ikreq = SolvePositionIKRequest()
    hdr = Header(stamp=rospy.Time.now(), frame_id='base')
    poses = {
        'left': PoseStamped(
            header=hdr,
            pose=Pose(
                position= point,
                orientation=orient
                ),
            ),
        'right': PoseStamped(
            header=hdr,
            pose=Pose(
                position=point,
                orientation=orient
                ),
            ),
    }

    ikreq.pose_stamp.append(poses[limb])
    try:
        rospy.wait_for_service(ns, 5.0)
        resp = iksvc(ikreq)
    except (rospy.ServiceException, rospy.ROSException), e:
        rospy.logerr("Service call failed: %s" % (e,))
        return 1
    def __init__(self):
        rospy.init_node("max_sonar")
        
        self.sensor = maxSonar() 

        # start channel broadcast using SetupAnalogIn
        rospy.wait_for_service('arbotix/SetupAnalogIn')
        analog_srv = rospy.ServiceProxy('arbotix/SetupAnalogIn', SetupChannel) 
        
        req = SetupChannelRequest()
        req.topic_name = rospy.get_param("~name")
        req.pin = rospy.get_param("~pin")
        req.rate = int(rospy.get_param("~rate",10))
        analog_srv(req)

        # setup a range message to use
        self.msg = Range()
        self.msg.radiation_type = self.sensor.radiation_type
        self.msg.field_of_view = self.sensor.field_of_view
        self.msg.min_range = self.sensor.min_range
        self.msg.max_range = self.sensor.max_range

        # publish/subscribe
        self.pub = rospy.Publisher("sonar_range", Range)
        rospy.Subscriber("arbotix/"+req.topic_name, Analog, self.readingCb)

        rospy.spin()
    def __init__(self):
        self._position_controller_nav_frame = rospy.get_param('~position_controller_nav_frame', 'odom')

        self._current_pose = None
        self._robot_frame = rospy.get_param('~robot_frame', 'base_link')
        self._next_target_tf_name = rospy.get_param('~target_tf_name', 'path_follower_next_target')
        self._pose_threshold_d = rospy.get_param('~pose_threshold_d', 0.2)
        self._pose_threshold_r = rospy.get_param('~pose_threshold_r', 0.2)

        rospy.loginfo('create TransformListener')
        self._tf_listener = tf.TransformListener()
        rospy.loginfo('create TransformBroadcaster')
        self._tf_broadcaster = tf.TransformBroadcaster()

        set_pose_name = rospy.get_param('~set_pose', '/position_controller/set_target_pos')
        rospy.loginfo('waiting for %s...' % set_pose_name)
        rospy.wait_for_service(set_pose_name)
        self._set_pose = rospy.ServiceProxy(set_pose_name, SetPos)
        self._set_pose_thread = None

        # for debug
        self._path_pub = rospy.Publisher('path_follower_path', Path, queue_size=10)

        action_name = rospy.get_param('~action_name', '/follow_path')
        rospy.loginfo('create action server %s', action_name)
        self.server = actionlib.SimpleActionServer(action_name, FollowPathAction, self.execute, False)
        self.server.start()
示例#24
0
 def status(self):
     rospy.wait_for_service('/qbo_video_record/status')
     try:
         status = rospy.ServiceProxy('/qbo_video_record/status', StatusRecord)
         resStatus = status()
     except rospy.ServiceException, e:
         print "Service call failed: %s"%e
	def __init__(self):
		rospy.wait_for_service("static_map")
		static_map = rospy.ServiceProxy("static_map", GetMap)
		try:
			self.map = static_map().map
		except:
			print "error receiving map"
    def SaveDataClient(self):
        
        try: 
            # time out of one second for waiting for service
            rospy.wait_for_service(self.namespace+'SaveDataFromGui',1.0)
            
            try:
                AskForSavingOrNot = rospy.ServiceProxy(self.namespace+'SaveDataFromGui', SaveData)

                # if button is pressed save data
                if self._widget.ButtonRequestSave.isChecked():
                    # request controller to save data
                    reply = AskForSavingOrNot(True)
                    if reply.Saving == True:
                        # if controller receives message, we know it
                        print('Saving')
                else:
                    # request controller to STOP saving data
                    reply = AskForSavingOrNot(False)
                    if  reply.Saving == True:
                        # if controller receives message, we know it
                        print('Stopped Saving')

            except rospy.ServiceException, e:
                print "Service call failed: %s"%e 
            
        except: 
            print "Service not available ..."        
            pass     
示例#27
0
  def nextSolution(self, query):
    if query.client == self:
      while not query.finished:
        rospy.wait_for_service(self.prologNextSolutionService)
        request = rospy.ServiceProxy(self.prologNextSolutionService,
          PrologNextSolution)    

        try:
          response = request(id = str(query.id))
        except rospy.ServiceException, exception:
          raise Exception(
            "PrologNextSolution service request failed: %s" % exception)
        
        if response.status == PrologNextSolutionResponse.OK:
          yield json.loads(response.solution)
        elif response.status == PrologNextSolutionResponse.NO_SOLUTION:
          query.client = None
          return
        elif response.status == PrologNextSolutionResponse.WRONG_ID:
          query.client = None
          raise Exception("Prolog query id is invalid: "+
            "Another client may have terminated your query")
        elif response.status == PrologNextSolutionResponse.QUERY_FAILED:
          query.client = None
          raise Exception("Prolog query failed: %s" % response.solution)
        else:
          query.client = None
          raise Exception("Prolog query status unknown: %d", response.status)
示例#28
0
    def send_mission(self, droid_id, mission):
        service_addr = "/%s_%i/%s" % ( self.prefix, droid_id, Droid.SERVICE_MISSION )

        rospy.wait_for_service( service_addr )
        send_mission_proxy = rospy.ServiceProxy( service_addr, DroidMission )

        send_mission_proxy( mission )
示例#29
0
def main():
    global client_speak
    rospy.init_node("qbo_first_use")
    rospy.loginfo("Launching Qbo First Use")

    file_path = roslib.packages.get_pkg_dir("qbo_first_use") + "/FIRST_FILE"

    file_exists = True
    try:
        with open(file_path) as f:
            pass
    except IOError as e:
        file_exists = False

    if file_exists:
        rospy.loginfo("Qbo First Use program has terminated because this isn't Qbo first use")
        return
    else:
        rospy.loginfo("Launching Qbo First use procedure")

    client_speak = rospy.ServiceProxy("/qbo_talk/festival_say_no_wait", Text2Speach)
    rospy.wait_for_service("/qbo_talk/festival_say_no_wait")

    speak_this(
        "This is my first boot... Please connect me to a local network by following the instructions on the manual"
    )
    open(file_path, "w").close()
 def __init__(self,cam_marker_file,base_ee_file):        
     
     self.ready=False        
     
     #save file names
     self.name_cam_marker_file=cam_marker_file 
     self.name_base_ee_file=base_ee_file
     
     #count how many camera-marker poses in the file
     self.camera_marker_file=open(self.name_cam_marker_file,'r')
     self.num_of_lines_of_cam_marker_file=sum(1 for line in self.camera_marker_file)
     self.num_of_cam_marker_poses=self.num_of_lines_of_cam_marker_file/3
     
     #initiate marker poses w.r.t camera
     self.camera_marker_samples=TransformArray()
     self.camera_marker_samples.header.frame_id="/camera_rgb_optical_frame"
     
     #count how many base-end-effector poses in the file        
     self.base_ee_file=open(self.name_base_ee_file,'r')
     self.num_of_lines_of_base_ee_file=sum(1 for line in self.base_ee_file)
     self.num_of_base_ee_poses=self.num_of_lines_of_base_ee_file
     
     if self.num_of_cam_marker_poses != self.num_of_base_ee_poses:
         print 'number of camera-marker poses not equal to number of base-ee poses!\n'
         print 'please ctrl+c to exit\n'
         self.read=False
     
     #initiate ee poses w.r.t base
     self.base_ee_samples=TransformArray()
     self.base_ee_samples.header.frame_id="/base"
     
     rospy.wait_for_service('compute_effector_camera_quick')
     print 'waiting for service to be available '
     self.calibrate=rospy.ServiceProxy('compute_effector_camera_quick',compute_effector_camera_quick)
     print 'service server is ready'        
示例#31
0
def mommy(baby_name):
    global x
    global y
    global theta

    rospy.wait_for_service('spawn')
    try:
        new_turtle = rospy.ServiceProxy('spawn', Spawn)
        if baby_name == "angel":
            resp = new_turtle(5.5, 4.5, 0, baby_name)
        else:
            resp = new_turtle(5.5, 6.5, 0, baby_name)
    except rospy.ServiceException, e:
        print "%s" % e

    rospy.wait_for_service(baby_name + '/set_pen')
    try:
        changepen = rospy.ServiceProxy(baby_name + '/set_pen', SetPen)
        changepen(0, 0, 0, 1, 255)
    except rospy.ServiceException, e:
        print "%s" % e


if __name__ == '__main__':

    baby_name = rospy.myargv(argv=sys.argv)[1]

    mommy(baby_name)
    try:
        driver(baby_name)
    except rospy.ROSInterruptException:
示例#32
0
    def __init__(self, id, initialPosition, tf):
        self.id = id
        prefix = "/cf" + str(id)
        self.initialPosition = np.array(initialPosition)

        self.tf = tf

        rospy.wait_for_service(prefix + "/set_group_mask")
        self.setGroupMaskService = rospy.ServiceProxy(
            prefix + "/set_group_mask", SetGroupMask)
        rospy.wait_for_service(prefix + "/takeoff")
        self.takeoffService = rospy.ServiceProxy(prefix + "/takeoff", Takeoff)
        rospy.wait_for_service(prefix + "/land")
        self.landService = rospy.ServiceProxy(prefix + "/land", Land)
        # rospy.wait_for_service(prefix + "/stop")
        # self.stopService = rospy.ServiceProxy(prefix + "/stop", Stop)
        rospy.wait_for_service(prefix + "/go_to")
        self.goToService = rospy.ServiceProxy(prefix + "/go_to", GoTo)
        rospy.wait_for_service(prefix + "/upload_trajectory")
        self.uploadTrajectoryService = rospy.ServiceProxy(
            prefix + "/upload_trajectory", UploadTrajectory)
        rospy.wait_for_service(prefix + "/start_trajectory")
        self.startTrajectoryService = rospy.ServiceProxy(
            prefix + "/start_trajectory", StartTrajectory)
        rospy.wait_for_service(prefix + "/update_params")
        self.updateParamsService = rospy.ServiceProxy(
            prefix + "/update_params", UpdateParams)

        ### added by The-SS begin
        self.pubCmdFullState = rospy.Publisher(prefix + '/cmd_full_state',
                                               FullState,
                                               queue_size=1)
        self.pubCmdStop = rospy.Publisher(prefix + '/cmd_stop',
                                          Empty_msg,
                                          queue_size=1)

        self.worldFrame = '/world'
示例#33
0
 def __init__(self):
     rospy.wait_for_service("/next_phase")
     self.nextPhase = rospy.ServiceProxy("/next_phase", Empty)
示例#34
0
    def __init__(self):
        
        # timing params
        self.dt_sim = 0.01 # timestep of the simulation
        self.t_activate = rospy.get_param('/t_activate')
        self.t_final = rospy.get_param('/t_final')             
        
        # pop-up scenario params
        self.s_ego_at_popup = rospy.get_param('/s_ego_at_popup')
        self.s_obs_at_popup = rospy.get_param('/s_obs_at_popup')
        self.d_obs_at_popup = rospy.get_param('/d_obs_at_popup')
        
        # track params
        self.track_name = rospy.get_param('/track_name')
        self.s_begin_mu_segments = rospy.get_param('/s_begin_mu_segments')
        self.mu_segment_values = rospy.get_param('/mu_segment_values')
        self.N_mu_segments = len(self.s_begin_mu_segments)
        self.mu_segment_idx = 0
        
        # vehicle params
        self.robot_name = rospy.get_param('/robot_name') 
        self.vehicle_width = rospy.get_param('/car/kinematics/l_width')
        
        
        
        # algo params (from acado)
        N = 40 
        dt_algo = 0.1
        
        # set up pausing of gazebo
        rospy.wait_for_service('gazebo/pause_physics')
        pause_gazebo = rospy.ServiceProxy('gazebo/pause_physics', Empty)
        rospy.wait_for_service('gazebo/unpause_physics')
        unpause_gazebo = rospy.ServiceProxy('gazebo/unpause_physics', Empty)
        self.s_pause_gazebo = rospy.get_param('/s_pause_gazebo')
        n_pauses = len(self.s_pause_gazebo) 
        i_pauses = 0
        
        # init node subs pubs
        rospy.init_node('experiment_manager', anonymous=True)
        #self.clockpub = rospy.Publisher('/clock', Clock, queue_size=10)   
        self.pathglobalsub = rospy.Subscriber("pathglobal", Path, self.pathglobal_callback)
        self.statesub = rospy.Subscriber("state", State, self.state_callback)
        self.carinfosub = rospy.Subscriber("/fssim/car_info", CarInfo, self.fssim_carinfo_callback)
        self.trajstarsub = rospy.Subscriber("trajstar", Trajectory, self.trajstar_callback)
        self.obspub = rospy.Publisher('/obs', Obstacles, queue_size=1)
        self.obsvispub = rospy.Publisher('/obs_vis', Marker, queue_size=1)
        self.tireparampub = rospy.Publisher('/tire_params', TireParams, queue_size=1)
        self.ctrl_mode_pub = rospy.Publisher('/ctrl_mode', Int16, queue_size=1)
        self.statetextmarkerpub = rospy.Publisher('/state_text_marker', Marker, queue_size=1)        

        # init logging vars
        self.s_begin_log = rospy.get_param('/s_begin_log')
        self.N_iters_to_save = 60
        self.explog_iterationcounter = 0
        self.explog_activated = False
        self.stored_pathglobal = False
        self.stored_trajstar = False
        self.stored_trajcl = False
        self.explog_saved = False
        
        # init misc internal variables
        self.pathglobal = Path()
        self.received_pathglobal = False
        self.state = State()
        self.received_state = False
        self.trajstar = Trajectory()
        self.received_trajstar = False
        self.fssim_carinfo = CarInfo()
        
        while(not self.received_pathglobal):
            print("waiting for pathglobal")
            time.sleep(self.dt_sim*100)

        # init experiment variables
        self.scenario_id = rospy.get_param('/scenario_id')
        self.traction_adaptive  = rospy.get_param('/traction_adaptive')
        
        self.tireparams = TireParams()
        self.obs = Obstacles()
        self.obs.s = [self.s_obs_at_popup]
        self.obs.d = [self.d_obs_at_popup]
        self.obs.R = [0.5]
        wiggleroom = 1.0 # todo param
        self.obs.Rmgn = [0.5*self.obs.R[0] + 0.5*self.vehicle_width + wiggleroom]
        Xobs, Yobs = ptsFrenetToCartesian(np.array(self.obs.s), \
                                          np.array(self.obs.d), \
                                          np.array(self.pathglobal.X), \
                                          np.array(self.pathglobal.Y), \
                                          np.array(self.pathglobal.psi_c), \
                                          np.array(self.pathglobal.s))
        self.ctrl_mode = 0 # # 0: stop, 1: cruise_ctrl, 2: tamp 
        
        # Main loop
        self.exptime = 0 
        while (not rospy.is_shutdown()) and self.exptime<self.t_final :
            if (self.exptime >= self.t_activate):        
                rospy.loginfo_throttle(1, "Running experiment, ctrl mode = %i"%self.ctrl_mode)
                
                # HANDLE TRACTION IN SIMULATION                
                s_ego = self.state.s % self.s_lap                
                for i in range(self.N_mu_segments-1):
                    if(self.s_begin_mu_segments[i] <= s_ego <= self.s_begin_mu_segments[i+1]):
                        self.mu_segment_idx = i
                        break
                if(s_ego >= self.s_begin_mu_segments[-1]):
                    self.mu_segment_idx = self.N_mu_segments-1
                mu = self.mu_segment_values[self.mu_segment_idx] 

                #print "s_ego =              ", s_ego
                #print "state.s =            ", self.state.s
                #print "self.N_mu_segments = ", self.N_mu_segments
                #print "mu_segment_idx =     ", self.mu_segment_idx
                #print "mu in this section = ", self.mu_segment_values[self.mu_segment_idx] 
                
                # set tire params of sim vehicle
                
                
                if (0.0 <= mu <0.3): # ice
                    B = 4.0
                    C = 2.0
                    D = mu
                    E = 1.0
                elif (0.3 <= mu < 0.5): # snow
                    B = 5.0
                    C = 2.0
                    D = mu
                    E = 1.0
                elif (0.5 <= mu < 0.9): # wet
                    B = 12.0
                    C = 2.3
                    D = mu
                    E = 1.0
                elif (0.9 <= mu < 1.5): # dry
                    B = 10.0
                    C = 1.9
                    D = mu
                    E = 0.97
                elif (1.5 <= mu < 2.5): # dry + racing tires (gotthard default)
                    B = 12.56;
                    C = 1.38; 
                    D = mu;
                    E = 1.0               
                else: 
                    rospy.loginfo_throttle(1, "Faulty mu value in exp manager")
                                
                self.tireparams.tire_coefficient = 1.0        
                self.tireparams.B = B
                self.tireparams.C = C
                self.tireparams.D = -D
                self.tireparams.E = E    
                self.tireparams.header.stamp = rospy.Time.now()
                self.tireparampub.publish(self.tireparams)
                
                # POPUP SCENARIO
                if (self.scenario_id == 1):
                    self.ctrl_mode = 1 # cruise control
                    if(self.state.vx > 5):
                        self.ctrl_mode = 2 # tamp
                    m_obs = self.getobstaclemarker(Xobs,Yobs,self.obs.R[0])
                    m_obs.color.a = 0.3 # transparent before detect
                    if (self.state.s >= self.s_ego_at_popup):
                        self.obspub.publish(self.obs)
                        m_obs.color.a = 1.0 # non-transparent after detect
                        
                    self.obsvispub.publish(m_obs)
                
                # REDUCED MU TURN
                elif(self.scenario_id == 2):
                    self.ctrl_mode = 1 # pp cruise control from standstill
                    if(self.state.vx > 5): # todo
                        self.ctrl_mode = 2 # tamp cruise control when we get up to speed
                                       
                # RACING
                else:
                    self.ctrl_mode = 2 # tamp
                
                # SEND STOP IF EXIT TRACK
                dlb = np.interp(self.state.s,self.pathglobal.s,self.pathglobal.dlb)
                dub = np.interp(self.state.s,self.pathglobal.s,self.pathglobal.dub)
                if (self.state.d < dlb-1.0 or self.state.d > dub+1.0): # todo get from param
                    self.ctrl_mode = 0 # stop
                self.ctrl_mode_pub.publish(self.ctrl_mode)
                
                # publish text marker (state info)
                if(self.traction_adaptive):
                    traction_adaptive_str = "on"
                else:
                    traction_adaptive_str = "off"
                    
                state_text = "traction_adapt: " + traction_adaptive_str + "\n"  \
                             "vx: " + "%.3f" % self.state.vx + "\n"  \
                             "mu: " + "%.3f" % mu            
                self.statetextmarkerpub.publish(self.gettextmarker(state_text))    
                
                # handle pausing of gazebo
                if (i_pauses < n_pauses):
                    if (self.state.s >= self.s_pause_gazebo[i_pauses]):
                        pause_gazebo()
                        raw_input("Press Enter to continue...")
                        unpause_gazebo()
                        i_pauses += 1
                
                # save data for plot
                if (self.state.s >= self.s_begin_log and not self.explog_activated):
                    print "STARTED EXPLOG"
                    t_start_explog = copy.deepcopy(self.exptime) 
                    self.explog_activated = True
                    # store planned traj
                    self.trajstar_dict = {
                      "X": np.array(self.trajstar.X),
                      "Y": np.array(self.trajstar.Y),
                      "psi": np.array(self.trajstar.psi),
                      "s": np.array(self.trajstar.s),
                      "d": np.array(self.trajstar.d),
                      "deltapsi": np.array(self.trajstar.deltapsi),
                      "psidot": np.array(self.trajstar.psidot),
                      "vx": np.array(self.trajstar.vx),
                      "vy": np.array(self.trajstar.vy),
                      "ax": np.array(self.trajstar.ax),
                      "ay": np.array(self.trajstar.ay),
                      "Fyf": np.array(self.trajstar.Fyf),
                      "Fxf": np.array(self.trajstar.Fxf),
                      "Fyr": np.array(self.trajstar.Fyr),
                      "Fxr": np.array(self.trajstar.Fxr),
                      "Fzf": np.array(self.trajstar.Fzf),
                      "Fzr": np.array(self.trajstar.Fzr),
                      "kappac": np.array(self.trajstar.kappac),
                      "Cr": np.array(self.trajstar.Cr),
                      "t": np.arange(0,(N+1)*dt_algo,dt_algo),
                    }
                    self.stored_trajstar = True
                    
                    # initialize vars for storing CL traj
                    self.X_cl = []
                    self.Y_cl = []
                    self.psi_cl = []
                    self.s_cl = []
                    self.d_cl = []
                    self.deltapsi_cl = []
                    self.psidot_cl = []
                    self.vx_cl = []
                    self.vy_cl = []
                    self.ax_cl = []
                    self.ay_cl = []
                    self.t_cl = []
                    self.Fyf_cl = []
                    self.Fyr_cl = []
                    self.Fx_cl = []
                    
                if (self.state.s >= self.s_begin_log and self.explog_activated and self.exptime >= t_start_explog + self.explog_iterationcounter*dt_algo):
                    # build CL traj       
                    self.X_cl.append(self.state.X)
                    self.Y_cl.append(self.state.Y)
                    self.psi_cl.append(self.state.psi)
                    self.s_cl.append(self.state.s)
                    self.d_cl.append(self.state.d)
                    self.deltapsi_cl.append(self.state.deltapsi)
                    self.psidot_cl.append(self.state.psidot)
                    self.vx_cl.append(self.state.vx)
                    self.vy_cl.append(self.state.vy)
                    self.ax_cl.append(self.state.ax)
                    self.ay_cl.append(self.state.ay)
                    self.t_cl.append(self.exptime)
                    self.Fyf_cl.append(self.fssim_carinfo.Fy_f_l+self.fssim_carinfo.Fy_f_r)
                    self.Fyr_cl.append(self.fssim_carinfo.Fy_r)
                    self.Fx_cl.append(self.fssim_carinfo.Fx)
                    
                    # store CL traj as dict
                    if (self.explog_iterationcounter == N): # store N+1 values, same as trajstar
                        self.trajcl_dict = {
                          "X": np.array(self.X_cl),
                          "Y": np.array(self.Y_cl),
                          "psi": np.array(self.psi_cl),
                          "s": np.array(self.s_cl),
                          "d": np.array(self.d_cl),
                          "deltapsi": np.array(self.deltapsi_cl),
                          "psidot": np.array(self.psidot_cl),
                          "vx": np.array(self.vx_cl),
                          "vy": np.array(self.vy_cl),
                          "ax": np.array(self.ax_cl),
                          "ay": np.array(self.ay_cl),
                          "t": np.array(self.t_cl),
                          "Fyf": np.array(self.Fyf_cl),
                          "Fyr": np.array(self.Fyr_cl),
                          "Fx": np.array(self.Fx_cl),
                        }
                        self.stored_trajcl = True
                    
                    self.explog_iterationcounter +=1
                
                # save explog               
                if (self.stored_pathglobal and self.stored_trajstar and self.stored_trajcl and not self.explog_saved):  
                    explog = {
                      "pathglobal": self.pathglobal_dict,
                      "trajstar": self.trajstar_dict,
                      "trajcl": self.trajcl_dict,
                    }
                    filepath = "/home/larsvens/ros/tamp__ws/src/saarti/common/logs/"
                    filename = "explog"
                    if(self.scenario_id == 1):
                        filename = filename + "_popup"
                        if(np.min(self.mu_segment_values)>0.8):
                            filename = filename + "_dry"
                        else:
                            filename = filename + "_wet"
                    elif(self.scenario_id == 2):
                        filename = filename + "_reducedmuturn"
                    elif(self.scenario_id == 3):
                        filename = filename + "_racing"
                    if(self.traction_adaptive):
                        filename = filename + "_adaptive"
                    else:
                        filename = filename + "_nonadaptive"
                    filename = filename + ".npy"
                    
                    np.save(filepath+filename,explog)
                    self.explog_saved = True
                    print("SAVED EXPLOG to " + filepath + filename)
            
            else: # not reached activation time
                rospy.loginfo_throttle(1, "Experiment starting in %i seconds"%(self.t_activate-self.exptime))
            
            # handle exptime
            self.exptime += self.dt_sim
            msg = Clock()
            t_rostime = rospy.Time(self.exptime)
            msg.clock = t_rostime
            #self.clockpub.publish(msg)
                                     
            
            time.sleep(self.dt_sim)

        print 'simulation finished'
    
        # send shutdown signal
        message = 'run finished, shutting down'
        print message
        rospy.signal_shutdown(message)
示例#35
0
 def __init__(self):
     srv_nm = 'environment_server_right_arm/get_state_validity'
     rospy.wait_for_service(srv_nm)
     self.state_validator = rospy.ServiceProxy(srv_nm,
                                               GetStateValidity,
                                               persistent=True)
        start_time = rospy.get_param('~start_time')
        if start_time < 0.0:
            rospy.logerr('Negative start time, setting it to 0.0')
            start_time = 0.0
            start_now = True
        else:
            start_now = False
    else:
        start_now = True

    rospy.loginfo('Start time=%.2f s' % start_time)

    interpolator = rospy.get_param('~interpolator', 'lipb')

    try:
        rospy.wait_for_service('init_waypoints_from_file', timeout=30)
    except rospy.ROSException:
        raise rospy.ROSException('Service not available! Closing node...')

    try:
        init_wp = rospy.ServiceProxy('init_waypoints_from_file',
                                     InitWaypointsFromFile)
    except rospy.ServiceException as e:
        raise rospy.ROSException('Service call failed, error=%s', str(e))

    success = init_wp(Time(rospy.Time.from_sec(start_time)), start_now,
                      String(filename), String(interpolator))

    if success:
        rospy.loginfo('Waypoints file successfully received, '
                      'filename=%s', filename)
    with open (model_path + "cafe_table/model.sdf", "r") as table_file:
        table_xml=table_file.read().replace('\n', '')
    # Load Block URDF
    block_xml = ''
    with open (model_path + "block/model.urdf", "r") as block_file:
        block_xml=block_file.read().replace('\n', '')
    # Spawn Table SDF
    rospy.wait_for_service('/gazebo/spawn_sdf_model')
    try:
        spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
        resp_sdf = spawn_sdf("cafe_table", table_xml, "/",
                             table_pose, table_reference_frame)
    except rospy.ServiceException, e:
        rospy.logerr("Spawn SDF service call failed: {0}".format(e))
    # Spawn Block URDF
    rospy.wait_for_service('/gazebo/spawn_urdf_model')
    try:
        spawn_urdf = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel)
        resp_urdf = spawn_urdf("block", block_xml, "/",
                               block_pose, block_reference_frame)
    except rospy.ServiceException, e:
        rospy.logerr("Spawn URDF service call failed: {0}".format(e))

def delete_gazebo_models():
    # This will be called on ROS Exit, deleting Gazebo models
    # Do not wait for the Gazebo Delete Model service, since
    # Gazebo should already be running. If the service is not
    # available since Gazebo has been killed, it is fine to error out
    try:
        delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
        resp_delete = delete_model("cafe_table")
示例#38
0
    print "Got commands from param:\n%s" % commands_string

    headers = headers_string.split()

    cmd_all = [[float(x) for x in cur_line.split()]
               for cur_line in commands_string.split("\n")]
    cmd = [x for x in cmd_all if len(x) == len(headers)]
    print "Commands"
    print cmd

    arm_controller = 'right_arm_trajectory_controller'
    arm_query_topic = arm_controller + '/TrajectoryQuery'
    arm_start_topic = arm_controller + '/TrajectoryStart'

    print "Waiting Query Service: " + arm_query_topic
    rospy.wait_for_service(arm_query_topic)
    print "Found Query Service!"

    query_srv = rospy.ServiceProxy(arm_query_topic, TrajectoryQuery)
    query_resp = query_srv.call(TrajectoryQueryRequest(0))
    head_publisher = rospy.Publisher('head_controller/command', JointStates)

    print "Headers"
    print headers

    print "Queried Jointnames:"
    print query_resp.jointnames

    print "***** Remapping Arm Commands *****"
    arm_mapping = [headers.index(x) for x in query_resp.jointnames]
    print "Mapping:"
#!/usr/bin/env python

import rospy
from turtlesim.srv import SetPen

if __name__ == '__main__':

    rospy.init_node('I_am_client')
    rospy.wait_for_service('/turtle1/set_pen')
    pen = rospy.ServiceProxy('/turtle1/set_pen', SetPen)
    
    # put service call inside a try block to catch exceptions
    # in case call fails
    try:
        pen(255,0,0,10,0)
    # catch service exception        
    except rospy.ServiceException, error:
        print "ops! call has failed with this error: ", error 
#!/usr/bin/env python3

import rospy
from bitbots_msgs.msg import FootPressure
from bitbots_msgs.srv import FootScale, FootScaleRequest, FootScaleResponse
from std_srvs.srv import Empty, EmptyRequest, EmptyResponse

rospy.init_node("pressure_calibration")
rospy.loginfo(
    "Welcome to the foot calibration suite. Press enter when there is no load on the weight cells"
)
input()
rospy.loginfo("waiting for service /set_foot_zero")
rospy.wait_for_service("/set_foot_zero")
zero = rospy.ServiceProxy("/set_foot_zero", Empty)
if zero():
    rospy.loginfo("Successfully set the zero position for all sensors")
else:
    rospy.loginfo("There was an error :(")
    exit(1)

sensors = [
    'left foot, left front',
    'left foot, left back',
    'left foot, right front',
    'left foot, right back',
    'right foot, left front',
    'right foot, left back',
    'right foot, right front',
    'right foot, right back',
]
示例#41
0
def compare_occmap(occmap1, occmap2):
    global gettf_client
    rospy.wait_for_service('GetMapTransform')
    resp = gettf_client(occmap1, occmap2)
    return resp.confidence
示例#42
0
def initialize_commands():
    rospy.init_node('mazesolvernode', anonymous=True)
    rospy.wait_for_service('make_maze')
    rospy.wait_for_service('print_maze')
    rospy.wait_for_service('get_wall')
    rospy.wait_for_service('get_odom')
    rospy.wait_for_service('cs1567_move')
    rospy.wait_for_service('cs1567_turn')
    rospy.wait_for_service('cs1567_stop_all_motion')
    #    rospy.wait_for_service('constant_command')

    global make_maze_service, print_maze_service, get_odom_service, get_wall_service
    global constant_command_service
    global move_service
    global turn_service
    global stop_service
    global reset_odometry_service

    make_maze_service = rospy.ServiceProxy('make_maze', MakeNewMaze)
    print_maze_service = rospy.ServiceProxy('print_maze', Empty)
    get_wall_service = rospy.ServiceProxy('get_wall', GetMazeWall)
    get_odom_service = rospy.ServiceProxy('get_odom', GetOdometry)
    #    constant_command_service = rospy.ServiceProxy('constant_command', ConstantCommand)
    move_service = rospy.ServiceProxy('cs1567_move', CS1567RobotMove)
    turn_service = rospy.ServiceProxy('cs1567_turn', CS1567RobotTurn)
    stop_service = rospy.ServiceProxy('cs1567_stop_all_motion', StopAll)
    reset_odometry_service = rospy.ServiceProxy('cs1567_reset_odometry',
                                                CSReset)

    snd_pub = rospy.Publisher('/mobile_base/commands/sound', Sound)
    led1_pub = rospy.Publisher('/mobile_base/commands/led1', Led)
    led2_pub = rospy.Publisher('/mobile_base/commands/led2', Led)

    make_maze_service(5, 5)
    print_maze_service()
    init_maze()
    solve_maze(4, 4)
 def resetWorld(self):
     rospy.wait_for_service('/gazebo/reset_world')
     try:
         self.reset_world_proxy()
     except rospy.ServiceException as e:
         print("/gazebo/reset_world service call failed")
示例#44
0
#!/usr/bin/env python
import rospy
import math
import tf2_ros
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('tf2_turtle_listener')

    tfBuffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(tfBuffer)

    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    turtle_name = rospy.get_param('turtle', 'turtle2')
    spawner(4, 2, 0, turtle_name)

    turtle_vel = rospy.Publisher('%s/cmd_vel' % turtle_name,
                                 geometry_msgs.msg.Twist,
                                 queue_size=1)

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            trans = tfBuffer.lookup_transform(turtle_name, 'turtle1',
                                              rospy.Time())
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                tf2_ros.ExtrapolationException):
            rate.sleep()
            continue
示例#45
0
#!/usr/bin/env python

import sys
import rospy
from aims_jackal_sim.srv import *
import numpy as np

if __name__ == "__main__":
    rospy.wait_for_service('sim_reset')
    srv = rospy.ServiceProxy('sim_reset', SimReset)
    resp = srv(np.random.randint(low=0, high=1000))

    if resp.result:
        print('Sim reset successfully.')
    else:
        print('Failed to reset sim.')
示例#46
0
 def init_direction_service_client(self, service_name = "/crash_direction_service"):
     rospy.loginfo('Waiting for Service Server')
     rospy.wait_for_service(service_name) # wait for the service client /gazebo/delete_model to be running
     rospy.loginfo('Service Server Found...')
     self._direction_service = rospy.ServiceProxy(service_name, Trigger) # create the connection to the service
     self._request_object = TriggerRequest()
示例#47
0
    def __init__(self, name):
        """ Init the class """
        # Save node name
        self.name = name

        # Get config
        self.get_config()

        # Create publisher
        self.pub_thrusters = rospy.Publisher("/cola2_control/thrusters_data",
                                             ThrustersData,
                                             queue_size = 2)

        self.pub_external_ra = rospy.Publisher("/cola2_safety/external_recovery_action",
                                             RecoveryAction,
                                             queue_size = 2)

        # Init service clients
        rospy.loginfo("%s: waiting for services", self.name)

        try:
            rospy.wait_for_service('/cola2_control/set_joystick_axes_to_velocity', 20)
            self.set_joy_to_vel_srv = rospy.ServiceProxy(
                                '/cola2_control/set_joystick_axes_to_velocity', Empty)
        except rospy.exceptions.ROSException:
            self.captain_clients = False
            rospy.logfatal("%s: set joystick axes to velocity service is not available!", self.name)

        self.captain_clients = True
        try:
            rospy.wait_for_service('/cola2_control/disable_trajectory', 20)
            self.abort_mission_srv = rospy.ServiceProxy(
                                '/cola2_control/disable_trajectory', Empty)
        except rospy.exceptions.ROSException:
            self.captain_clients = False
            rospy.logfatal("%s: disable trajectory service not available!", self.name)

        try:
            rospy.wait_for_service('/cola2_control/disable_keep_position', 2)
            self.abort_keep_pose_srv = rospy.ServiceProxy(
                                '/cola2_control/disable_keep_position', Empty)
        except rospy.exceptions.ROSException:
            self.captain_clients = False
            rospy.logfatal("%s: disable keep position service not available!", self.name)

        try:
            rospy.wait_for_service('/cola2_control/disable_goto', 2)
            self.abort_goto_srv = rospy.ServiceProxy(
                                '/cola2_control/disable_goto', Empty)
        except rospy.exceptions.ROSException:
            self.captain_clients = False
            rospy.logfatal("%s: disable goto service not available!", self.name)

        try:
            rospy.wait_for_service('/cola2_control/submerge', 2)
            self.surface_srv = rospy.ServiceProxy(
                                '/cola2_control/submerge', Submerge)
        except rospy.exceptions.ROSException:
            self.captain_clients = False
            rospy.logfatal("%s: submerge service not available!", self.name)

        if not self.captain_clients:
            self.no_captain_clients_timer = rospy.Timer(rospy.Duration(0.4), self.no_captain_clients_message)

        try:
            rospy.wait_for_service('/cola2_control/disable_thrusters', 20)
            self.abort_thrusters_srv = rospy.ServiceProxy(
                                '/cola2_control/disable_thrusters', Empty)
        except rospy.exceptions.ROSException:
            self.no_disable_thrusters_service_timer = rospy.Timer(rospy.Duration(0.4), self.no_disable_thrusters_message)

        # Create service
        self.recovery_srv = rospy.Service('/cola2_safety/recovery_action',
                                        Recovery,
                                        self.recovery_action_srv)

        # Show message
        rospy.loginfo("%s: initialized", self.name)
 def resetSimulation(self):
     rospy.wait_for_service('/gazebo/reset_simulation')
     try:
         self.reset_simulation_proxy()
     except rospy.ServiceException as e:
         print("/gazebo/reset_simulation service call failed")
示例#49
0
def main(name,
         poi_x='*',
         poi_y='*',
         poi_z='*',
         ori_x='*',
         ori_y='*',
         ori_z='*',
         ori_w='*',
         lin_x='*',
         lin_y='*',
         lin_z='*',
         ang_x='*',
         ang_y='*',
         ang_z='*',
         timeout=None):
    modelstate = ModelState()
    pose = Pose()
    twist = Twist()
    pose, twist = gazebo_get_model_state.main(str(name), "world", timeout)
    if (poi_x != '*'):
        pose.position.x = float(poi_x)
    if (poi_y != '*'):
        pose.position.y = float(poi_y)
    if (poi_z != '*'):
        pose.position.z = float(poi_z)
    if (ori_x != '*'):
        pose.orientation.x = float(ori_x)
    if (ori_y != '*'):
        pose.orientation.y = float(ori_y)
    if (ori_z != '*'):
        pose.orientation.z = float(ori_z)
    if (ori_w != '*'):
        pose.orientation.w = float(ori_w)
    if (lin_x != '*'):
        twist.linear.x = float(lin_x)
    if (lin_y != '*'):
        twist.linear.y = float(lin_y)
    if (lin_z != '*'):
        twist.linear.z = float(lin_z)
    if (ang_x != '*'):
        twist.angular.x = float(ang_x)
    if (ang_y != '*'):
        twist.angular.y = float(ang_y)
    if (ang_z != '*'):
        twist.angular.z = float(ang_z)
    modelstate.model_name = name
    modelstate.pose = pose
    modelstate.twist = twist
    modelstate.reference_frame = "world"

    try:
        if (timeout == None):
            rospy.wait_for_service('gazebo/set_model_state')
        else:
            rospy.wait_for_service('gazebo/set_model_state', int(timeout))
        set_model_state_prox = rospy.ServiceProxy('gazebo/set_model_state',
                                                  SetModelState)
        set_model_state_prox(modelstate)
        print("gazebo/set_model_state %s!" % str(name))
    except rospy.ServiceException as e:
        print("Service call failed: %s" % e)
示例#50
0
def pr2_mover(object_list):

    # TODO: Initialize variables
    dict_list = []
    pick_centroids = []  # to be list of tuples (x, y, z)

    test_scene_num = Int32()
    test_scene_num.data = 1

    # TODO: Get/Read parameters
    pick_list_param = rospy.get_param('/object_list')
    dropbox_param = rospy.get_param('/dropbox')

    # TODO: Parse parameters into individual variables
    dropbox = {}
    for p in dropbox_param:
        dropbox[p['name']] = p['position']

    # TODO: Rotate PR2 in place to capture side tables for the collision map

    # TODO: Loop through the pick list
    for pick in pick_list_param:
        print(pick_list_param)

        pick_name = pick['name']
        pick_group = pick['group']

        print("\nLooking for [{}] to be placed in [{}] box.".format(
            pick_name, pick_group))

        object_name = String()
        object_name.data = pick_name

        # TODO: Get the PointCloud for a given object and obtain it's centroid

        # Index of the object to be picked in the `detected_objects` list
        pick_cloud = None
        for i, detected_object in enumerate(object_list):
            if (detected_object.label == pick_name):
                pick_cloud = detected_object.cloud

        if (pick_cloud == None):
            print("ERROR:::: {} not found in the detected object list".format(
                pick_name))
            continue

        points_arr = ros_to_pcl(pick_cloud).to_array()
        pick_centroid = np.mean(points_arr, axis=0)[:3]
        pick_centroids.append(pick_centroid)

        print("\nCentroid found : {}".format(pick_centroid))

        pick_pose = Pose()
        pick_pose.position.x = float(pick_centroid[0])
        pick_pose.position.y = float(pick_centroid[1])
        pick_pose.position.z = float(pick_centroid[2])

        # TODO: Assign the arm to be used for pick_place
        arm_name = String()
        if pick['group'] == 'red':
            arm_name.data = 'left'
        elif pick['group'] == 'green':
            arm_name.data = 'right'
        else:
            print "ERROR, group must be green or red!"

    # TODO: Create 'place_pose' for the object
        place_pose = Pose()
        place_pose.position.x = dropbox[arm_name.data][0]
        place_pose.position.y = dropbox[arm_name.data][1]
        place_pose.position.z = dropbox[arm_name.data][2]

        # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
        yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name,
                                   pick_pose, place_pose)
        dict_list.append(yaml_dict)

        # Wait for 'pick_place_routine' service to come up
        rospy.wait_for_service('pick_place_routine')

        try:
            pick_place_routine = rospy.ServiceProxy('pick_place_routine',
                                                    PickPlace)

            # TODO: Insert your message variables to be sent as a service request
            resp = pick_place_routine(test_scene_num, object_name, arm_name,
                                      pick_pose, place_pose)
            print("Response: ", resp.success)

        except rospy.ServiceException, e:
            print "Service call failed: %s" % e
示例#51
0
def pr2_mover(object_list):

    # TODO: Initialize variables
    object_name_list = []
    object_group = []
    dropbox_name = []
    dropbox_group = []
    dropbox_position = []

    # TODO: Get/Read parameters
    object_list_param = rospy.get_param('/object_list')
    dropbox_list_param = rospy.get_param('/dropbox')

    # TODO: Parse parameters into individual variables
    for i in range(len(object_list_param)):
        object_name_list.append(object_list_param[i]['name'])
        object_group.append(object_list_param[i]['group'])

    for i in range(len(dropbox_list_param)):
        dropbox_name.append(dropbox_list_param[i]['name'])
        dropbox_group.append(dropbox_list_param[i]['group'])
        dropbox_position.append(dropbox_list_param[i]['position'])

    # TODO: Rotate PR2 in place to capture side tables for the collision map

    labels = []
    centroids = []  # to be list of tuples (x, y, z)
    for object in object_list:
        labels.append(object.label)
        points_arr = ros_to_pcl(object.cloud).to_array()
        cntd = np.mean(points_arr, axis=0)[:3]
        centroids.append(
            [np.asscalar(cntd[0]),
             np.asscalar(cntd[1]),
             np.asscalar(cntd[2])])

    # TODO: Loop through the pick list
    test_no = 3
    dict_list = []
    for i in range(len(object_list_param)):
        test_scene_num = Int32()
        object_name = String()
        arm_name = String()
        pick_pose = Pose()
        place_pose = Pose()

        test_scene_num.data = test_no

        object_name.data = object_name_list[i]
        ind = search_in_list(object_name_list[i], labels)
        if ind == -1:
            continue

        # TODO: Get the PointCloud for a given object and obtain it's centroid
        pick_pose.position.x = centroids[ind][0]
        pick_pose.position.y = centroids[ind][1]
        pick_pose.position.z = centroids[ind][2]

        # TODO: Create 'place_pose' for the object
        assigned_group_name = object_group[i]
        ind2 = search_in_list(assigned_group_name, dropbox_group)
        place_pose.position.x = dropbox_position[ind2][0]
        place_pose.position.y = dropbox_position[ind2][1]
        place_pose.position.z = dropbox_position[ind2][2]

        # TODO: Assign the arm to be used for pick_place
        arm_name.data = dropbox_name[ind2]

        # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
        yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name,
                                   pick_pose, place_pose)
        dict_list.append(yaml_dict)

        # Wait for 'pick_place_routine' service to come up
        rospy.wait_for_service('pick_place_routine')

        try:
            pick_place_routine = rospy.ServiceProxy('pick_place_routine',
                                                    PickPlace)

            # TODO: Insert your message variables to be sent as a service request
            resp = pick_place_routine(test_scene_num, object_name, arm_name,
                                      pick_pose, place_pose)

            print("Response: ", resp.success)

        except rospy.ServiceException, e:
            print "Service call failed: %s" % e
示例#52
0
def control():
    # sp = np.load("xy_sin.npy")

    state_sub = rospy.Subscriber("/mavros/state",
                                 State,
                                 state_cb,
                                 queue_size=10)

    rospy.wait_for_service('mavros/cmd/arming')
    arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)

    rospy.wait_for_service('mavros/set_mode')
    set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)

    acutator_control_pub = rospy.Publisher("/mavros/actuator_control",
                                           ActuatorControl,
                                           queue_size=10)

    local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local",
                                    PoseStamped,
                                    queue_size=10)

    mocap_pos_pub = rospy.Publisher("/mavros/mocap/pose",
                                    PoseStamped,
                                    queue_size=10)

    imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, imu_cb, queue_size=10)
    local_pos_sub = rospy.Subscriber("/mavros/local_position/pose",
                                     PoseStamped,
                                     lp_cb,
                                     queue_size=10)
    local_vel_sub = rospy.Subscriber("/mavros/local_position/velocity",
                                     TwistStamped,
                                     lv_cb,
                                     queue_size=10)
    act_control_sub = rospy.Subscriber("/mavros/act_control/act_control_pub",
                                       ActuatorControl,
                                       act_cb,
                                       queue_size=10)

    rospy.init_node('control', anonymous=True)
    rate = rospy.Rate(50.0)

    # print("*"*80)
    while not rospy.is_shutdown() and not current_state.connected:
        rate.sleep()
        rospy.loginfo(current_state.connected)
    # print("#"*80)

    pose = PoseStamped()

    # mocap_pose = PoseStamped()

    offb_set_mode = SetModeRequest()
    offb_set_mode.custom_mode = "OFFBOARD"

    arm_cmd = CommandBoolRequest()
    arm_cmd.value = True

    last_request = rospy.Time.now()

    i = 0
    act = ActuatorControl()
    flag1 = False
    flag2 = False

    prev_imu_data = Imu()
    prev_time = rospy.Time.now()

    # prev_x = 0
    # prev_y = 0
    # prev_z = 0

    # prev_vx = 0
    # prev_vy = 0
    # prev_vz = 0
    prev_omega_x = 0
    prev_omega_y = 0
    prev_omega_z = 0

    prev_vx = 0
    prev_vy = 0
    prev_vz = 0
    delta_t = 0.02

    count = 0
    theta = 0.0
    #rospy.loginfo("Outside")

    r = np.random.rand(100000, 1) * 5

    x_trgt = []
    y_trgt = []

    row = 0
    theta = 0.0
    x_step = 0.0
    y_step = 0.0
    for t in xrange(0, 100000):
        # val = np.random.uniform(0,0.5)*5
        # y_step = 0.0
        for p in xrange(0, 50):
            x_trgt.append(x_step)
            y_trgt.append(r[t, 0] + y_step)
            x_step += 0.01
            y_step += 0.01
            theta += 1.0 / 100

    alpha = 0.01
    temp = 0.0
    y_new = []
    for r_n in range(0, len(y_trgt)):
        temp = temp + alpha * (y_trgt[r_n] - temp)
        y_new.append(temp)

    while not rospy.is_shutdown():
        if current_state.mode != "OFFBOARD" and (
                rospy.Time.now() - last_request > rospy.Duration(5.0)):
            offb_set_mode_response = set_mode_client(offb_set_mode)
            if offb_set_mode_response.mode_sent:
                rospy.loginfo("Offboard enabled")
                flag1 = True

            last_request = rospy.Time.now()
        else:
            if current_state.armed == False:
                arm_cmd_response = arming_client(arm_cmd)
                if arm_cmd_response.success:
                    rospy.loginfo("Vehicle armed")
                    flag2 = True

                last_request = rospy.Time.now()

        # rospy.loginfo("Inside")
        curr_time = rospy.Time.now()
        if flag1 and flag2:

            x_f.append(float(local_position.pose.position.x))
            y_f.append(float(local_position.pose.position.y))
            z_f.append(float(local_position.pose.position.z))

            vx_f.append(float(local_velocity.twist.linear.x))
            vy_f.append(float(local_velocity.twist.linear.y))
            vz_f.append(float(local_velocity.twist.linear.z))

            # print(local_velocity.twist.linear.x)

            orientation = [
                imu_data.orientation.w, imu_data.orientation.x,
                imu_data.orientation.y, imu_data.orientation.z
            ]
            (roll, pitch, yaw) = quaternion_to_euler_angle(
                imu_data.orientation.w, imu_data.orientation.x,
                imu_data.orientation.y, imu_data.orientation.z)
            r_f.append(float(mt.radians(roll)))
            p_f.append(float(mt.radians(pitch)))
            yaw_f.append(float(mt.radians(yaw)))

            sin_r_f.append(mt.sin(float(mt.radians(roll))))
            sin_p_f.append(mt.sin(float(mt.radians(pitch))))
            sin_y_f.append(mt.sin(float(mt.radians(yaw))))

            cos_r_f.append(mt.cos(float(mt.radians(roll))))
            cos_p_f.append(mt.cos(float(mt.radians(pitch))))
            cos_y_f.append(mt.cos(float(mt.radians(yaw))))

            rs_f.append(float(imu_data.angular_velocity.x))
            ps_f.append(float(imu_data.angular_velocity.y))
            ys_f.append(float(imu_data.angular_velocity.z))

            ix.append(float(ixx))
            iy.append(float(iyy))
            iz.append(float(izz))

            m.append(float(mass))

            u0.append(act_controls.controls[0])
            u1.append(act_controls.controls[1])
            u2.append(act_controls.controls[2])
            u3.append(act_controls.controls[3])

            pose.pose.position.x = x_trgt[row]
            pose.pose.position.y = y_new[row]
            pose.pose.position.z = 3

            row = row + 1

            x_des.append(0)
            y_des.append(0)
            z_des.append(3)
            sin_y_des.append(yaw)
            cos_y_des.append(yaw)

            w_mag.append(0)
            w_x.append(0)
            w_y.append(0)
            w_z.append(0)

            n_t = curr_time - prev_time
            #delta_t = float(n_t.nsecs) * 1e-9

            a_x.append(
                (float(local_velocity.twist.linear.x) - prev_vx) / delta_t)
            a_y.append(
                (float(local_velocity.twist.linear.y) - prev_vy) / delta_t)
            a_z.append(
                (float(local_velocity.twist.linear.z) - prev_vz) / delta_t)

            prev_vx = float(local_velocity.twist.linear.x)
            prev_vy = float(local_velocity.twist.linear.y)
            prev_vz = float(local_velocity.twist.linear.z)

            aplha_x.append(
                (float(imu_data.angular_velocity.x) - prev_omega_x) / delta_t)
            aplha_y.append(
                (float(imu_data.angular_velocity.y) - prev_omega_y) / delta_t)
            aplha_z.append(
                (float(imu_data.angular_velocity.z) - prev_omega_z) / delta_t)

            ax_f.append(float(imu_data.linear_acceleration.x))
            ay_f.append(float(imu_data.linear_acceleration.y))
            az_f.append(float(imu_data.linear_acceleration.z))

            prev_omega_x = float(imu_data.angular_velocity.x)
            prev_omega_y = float(imu_data.angular_velocity.y)
            prev_omega_z = float(imu_data.angular_velocity.z)

            theta += 0.5

            count += 1
            local_pos_pub.publish(pose)
            print('data_points =', data_points, " count =  ", count, "row =",
                  row)

            if (count >= data_points):
                break

        prev_time = curr_time
        rate.sleep()

    nn1_yaw_input_state = np.array([
        vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f,
        cos_p_f, cos_y_f, u3
    ],
                                   ndmin=2).transpose()
    nn1_yaw_grd_truth = np.array([a_x, a_y, a_z], ndmin=2).transpose()

    nn2_yaw_input_state = np.array([
        vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f,
        cos_p_f, cos_y_f, u0, u1, u2
    ],
                                   ndmin=2).transpose()
    nn2_yaw_grd_truth = np.array([aplha_x, aplha_y, aplha_z],
                                 ndmin=2).transpose()

    print('nn1_test_traj_1_input_state   :', nn1_yaw_input_state.shape)
    print('nn1_test_traj_1_grd_truth   :', nn1_yaw_grd_truth.shape)
    print('nn2_test_traj_1_input_state   :', nn2_yaw_input_state.shape)
    print('nn2_test_traj_1_grd_truth :', nn2_yaw_grd_truth.shape)

    np.save('nn1_test_traj_1_input_state.npy', nn1_yaw_input_state)
    np.save('nn1_test_traj_1_grd_truth.npy', nn1_yaw_grd_truth)
    np.save('nn2_test_traj_1_input_state.npy', nn2_yaw_input_state)
    np.save('nn2_test_traj_1_grd_truth.npy', nn2_yaw_grd_truth)

    s_test_traj_1 = np.array([
        x_f, y_f, z_f, vx_f, vy_f, vz_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f,
        cos_p_f, cos_y_f, rs_f, ps_f, ys_f, r_f, p_f, yaw_f
    ],
                             ndmin=2).transpose()
    u_test_traj_1 = np.array([u0, u1, u2, u3], ndmin=2).transpose()
    a_test_traj_1 = np.array([ax_f, ay_f, az_f], ndmin=2).transpose()

    print('s_test_traj_1 :', s_test_traj_1.shape)
    print('u_test_traj_1 :', u_test_traj_1.shape)
    print('a_test_traj_1 :', a_test_traj_1.shape)

    np.save('s_test_traj_1.npy', s_test_traj_1)
    np.save('u_test_traj_1.npy', u_test_traj_1)
    np.save('a_test_traj_1.npy', a_test_traj_1)
示例#53
0
def main():
    tags = {}
    cup_tags = []

    #Wait for the IK service to become available
    rospy.wait_for_service('compute_ik')
    rospy.init_node('service_query')
    right_gripper = baxter_gripper.Gripper('right')
    left_gripper = baxter_gripper.Gripper('left')
    right_gripper.calibrate()
    # left_gripper.calibrate()

    #Create the function used to call the service
    compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
    rospy.wait_for_service('fixed_multi_service')
    get_ar = rospy.ServiceProxy('fixed_multi_service', ARPose)

    tag3 = get_ar(3)
    tag4 = get_ar(4)
    tag5 = get_ar(5)
    cup_tags.append(tag3)
    cup_tags.append(tag4)
    cup_tags.append(tag5)

    def get_closest_cup_tag(ar_tag, cup_tags):
        def dist(x1, y1, z1, x2, y2, z2):
            return (x1 - x2)**2 + (y1 - y2)**2 + (z1 - z2)**2

        minDist = float('inf')
        minTag = None
        for T in cup_tags:
            d = dist(ar_tag.pos_x, ar_tag.pos_y, ar_tag.pos_z, T.pos_x,
                     T.pos_y, T.pos_z)
            if d < minDist:
                minDist = d
                minTag = T
        return minTag

    # starting position
    os.system(
        'rosrun cup_grabber joint_trajectory_file_playback.py -f starting_position'
    )

    left = baxter_interface.Limb('left')
    right = baxter_interface.Limb('right')
    grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
    grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
    lj = left.joint_names()
    rj = right.joint_names()

    def move(curr, trans):
        a = 14 * np.pi / 180
        cx, cy, cz = curr
        tx, ty, tz = trans
        nz = tz * np.cos(a) - ty * np.sin(a)
        ny = tz * np.sin(a) + ty * np.cos(a)
        return (cx + tx, cy + ny, cz + nz)

    ################### learning tag3
    while tag3.tag_name == 'notfound':
        tag3 = get_ar(3)
        print 'tag3 not found'
    cup_tags[0] = tag3
    currxyz = (tag3.pos_x, tag3.pos_y, tag3.pos_z)
    currori = (-.083, -.02, -.697, .712)

    right_gripper.open()
    currxyz = move(currxyz, (-.1, .11, -.1))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    currxyz = move(currxyz, (0, 0, .17))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    right_gripper.close()
    currxyz = move(currxyz, (0, .5, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    currxyz = move(currxyz, (-.2, 0, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    currxyz = move(currxyz, (.2, 0, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    currxyz = move(currxyz, (0, -.5, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    right_gripper.open()
    currxyz = move(currxyz, (0, 0, -.17))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)

    tag8 = get_ar(8)
    tag9 = get_ar(9)
    tag10 = get_ar(10)
    if tag8.tag_name != 'notfound': tags[8] = tag8
    if tag9.tag_name != 'notfound': tags[9] = tag9
    if tag10.tag_name != 'notfound': tags[10] = tag9

    ################### learning tag4
    while tag4.tag_name == 'notfound':
        tag4 = get_ar(4)
        print 'tag4 not found'
    cup_tags[1] = tag4
    currxyz = (tag4.pos_x, tag4.pos_y, tag4.pos_z)
    currxyz = move(currxyz, (-.1, .11, -.1))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    currxyz = move(currxyz, (0, 0, .17))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    right_gripper.close()
    currxyz = move(currxyz, (0, .5, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    currxyz = move(currxyz, (-.2, 0, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    currxyz = move(currxyz, (.2, 0, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    currxyz = move(currxyz, (0, -.5, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    right_gripper.open()
    currxyz = move(currxyz, (0, 0, -.17))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)

    tag8 = get_ar(8)
    tag9 = get_ar(9)
    tag10 = get_ar(10)
    if tag8.tag_name != 'notfound': tags[8] = tag8
    if tag9.tag_name != 'notfound': tags[9] = tag9
    if tag10.tag_name != 'notfound': tags[10] = tag9

    ################### learning tag5
    while tag5.tag_name == 'notfound':
        tag5 = get_ar(5)
        print 'tag5 not found'
    cup_tags[2] = tag5

    currxyz = (tag5.pos_x, tag5.pos_y, tag5.pos_z)
    currxyz = move(currxyz, (-.1, .1, -.1))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    currxyz = move(currxyz, (0, 0, .17))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    right_gripper.close()
    currxyz = move(currxyz, (0, .5, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    currxyz = move(currxyz, (-.2, 0, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    currxyz = move(currxyz, (.2, 0, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    currxyz = move(currxyz, (0, -.5, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    right_gripper.open()
    currxyz = move(currxyz, (0, 0, -.17))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)

    tag8 = get_ar(8)
    tag9 = get_ar(9)
    tag10 = get_ar(10)
    if tag8.tag_name != 'notfound': tags[8] = tag8
    if tag9.tag_name != 'notfound': tags[9] = tag9
    if tag10.tag_name != 'notfound': tags[10] = tag9

    ############################################# done sensing the environment
    ################ pick up bottle with '8' contents
    cup_tag_near_8 = get_closest_cup_tag(tag8, cup_tags)
    curr = None
    if cup_tag_near_8.tag_name == '3':
        currxyz = (tag3.pos_x, tag3.pos_y, tag3.pos_z)
        curr = 3
    elif cup_tag_near_8.tag_name == '4':
        currxyz = (tag4.pos_x, tag4.pos_y, tag4.pos_z)
        curr = 4
    elif cup_tag_near_8.tag_name == '5':
        currxyz = (tag5.pos_x, tag5.pos_y, tag5.pos_z)
        curr = 5
    currxyz = move(currxyz, (-.1, .1, -.1))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    currxyz = move(currxyz, (0, 0, .17))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    right_gripper.close()
    currxyz = move(currxyz, (0, .5, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    os.system('cd ~/Desktop/baxter-waiter/src/cup_grabber/src')
    os.system(
        'rosrun cup_grabber joint_trajectory_file_playback.py -f left_starting_position'
    )
    os.system('cd ~/Desktop/baxter-waiter/src/cup_grabber/src')
    os.system(
        'rosrun cup_grabber joint_trajectory_file_playback.py -f right_pouring'
    )
    currxyz = move(currxyz, (0, -.5, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    right_gripper.open()
    currxyz = move(currxyz, (0, 0, -.17))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)

    ################ pick up bottle with '9' contents
    cup_tag_near_9 = get_closest_cup_tag(tag9, cup_tags)
    if cup_tag_near_9.tag_name == '3':
        currxyz = (tag3.pos_x, tag3.pos_y, tag3.pos_z)
        curr = 3
    elif cup_tag_near_9.tag_name == '4':
        currxyz = (tag4.pos_x, tag4.pos_y, tag4.pos_z)
        curr = 4
    elif cup_tag_near_9.tag_name == '5':
        currxyz = (tag5.pos_x, tag5.pos_y, tag5.pos_z)
        curr = 5

    currxyz = move(currxyz, (-.1, .1, -.1))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    currxyz = move(currxyz, (0, 0, .17))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    right_gripper.close()
    currxyz = move(currxyz, (0, .5, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    os.system('cd ~/Desktop/baxter-waiter/src/cup_grabber/src')
    os.system(
        'rosrun cup_grabber joint_trajectory_file_playback.py -f right_pouring'
    )
    currxyz = move(currxyz, (0, -.5, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    right_gripper.open()
    currxyz = move(currxyz, (0, 0, -.17))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)

    ################ pick up bottle with '10' contents
    cup_tag_near_10 = get_closest_cup_tag(tag10, cup_tags)
    if cup_tag_near_10.tag_name == '3':
        currxyz = (tag3.pos_x, tag3.pos_y, tag3.pos_z)
        curr = 3
    elif cup_tag_near_10.tag_name == '4':
        currxyz = (tag4.pos_x, tag4.pos_y, tag4.pos_z)
        curr = 4
    elif cup_tag_near_10.tag_name == '5':
        currxyz = (tag5.pos_x, tag5.pos_y, tag5.pos_z)
        curr = 5

    currxyz = move(currxyz, (-.1, .1, -.1))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    currxyz = move(currxyz, (0, 0, .17))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    right_gripper.close()
    currxyz = move(currxyz, (0, .5, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    os.system('cd ~/Desktop/baxter-waiter/src/cup_grabber/src')
    os.system(
        'rosrun cup_grabber joint_trajectory_file_playback.py -f right_pouring'
    )
    currxyz = move(currxyz, (0, -.5, 0))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
    right_gripper.open()
    currxyz = move(currxyz, (0, 0, -.17))
    requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)

    os.system('cd ~/Desktop/baxter-waiter/src/cup_grabber/src')
    os.system('rosrun cup_grabber joint_trajectory_file_playback.py -f serve')
    os.system('cd ~/Desktop/baxter-waiter/src/cup_grabber/src')
    os.system('rosrun cup_grabber joint_trajectory_file_playback.py -f dab')
    goal_pose = Odometry()
    goal_pose.header.stamp = rospy.Time.now()
    goal_pose.header.frame_id = 'world'
    goal_pose.pose.pose.position = Point(pose[0][0]+x_offset, pose[0][1]+y_offset, 0.)
    return goal_pose

def get_result(result_aux):
    global result
    result.data = result_aux.data

if __name__ == '__main__':
    pub = rospy.Publisher('move_usv/goal', Odometry, queue_size=10)
    rospy.init_node('patrol')
    rate = rospy.Rate(1) # 10h
    rospy.Subscriber("move_usv/result", Float64, get_result)
    rospy.wait_for_service('/gazebo/unpause_physics')
    rospy.wait_for_service('/gazebo/pause_physics')
    rospy.wait_for_service('/gazebo/reset_simulation')
    unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
    pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
    resetSimulation = rospy.ServiceProxy('/gazebo/reset_simulation', Empty)
    #unpause()


    simulationNumber = 1
    while not rospy.is_shutdown():    
        try:
	    rospy.logerr("Simulation number %d", simulationNumber)
            for pose in waypoints:
                goal = goal_pose(pose)
                pub.publish(goal)
    def keyPressEvent(self, event):
        key = event.key()

        # If we have constructed the drone controller and the key is not generated from an auto-repeating key
        if controller is not None and not event.isAutoRepeat():
            # Handle the important cases first!
            if key == KeyMapping.ThrottleCut:
                controller.SendEmergency()
            elif key == KeyMapping.Emergency:
                controller.SendLand()  # modify to prevent damage to quad
            elif key == KeyMapping.Takeoff:
                controller.SendTakeoff()
            elif key == KeyMapping.Land:
                controller.SendLand()
            elif key == KeyMapping.Linear:
                controller.SendLinear()
            elif key == KeyMapping.Circle:
                controller.SendCircle()
            elif key == KeyMapping.Spiral:
                controller.SendSpiral()
            elif key == KeyMapping.Snake:
                controller.SendSnake()
            elif key == KeyMapping.Ready:
                controller.SendReady()
            elif key == KeyMapping.ToggleProcessing:
                controller.StartStop()
            elif key == KeyMapping.CamChange:
                rospy.wait_for_service('ardrone/togglecam')
                try:
                    switchcam = rospy.ServiceProxy('ardrone/togglecam',
                                                   ToggleCam)
                    switchcam()
                except rospy.ServiceException, e:
                    print "Service call failed: %s" % e

            elif key == KeyMapping.ProcessImg:
                if self.processImagesBool == False:
                    self.processImagesBool = True
                    print "Image processing is turned on"
                    super(KeyboardController, self).EnableImageProcessing()
                else:
                    self.processImagesBool = False
                    print "Image processing is turned off"
                    super(KeyboardController, self).DisableImageProcessing()
            else:
                # Now we handle moving, notice that this section is the opposite (+=) of the keyrelease section
                if key == KeyMapping.YawLeft:
                    self.yaw_velocity += 1
                elif key == KeyMapping.YawRight:
                    self.yaw_velocity += -1

                elif key == KeyMapping.PitchForward:
                    self.pitch += 1
                elif key == KeyMapping.PitchBackward:
                    self.pitch += -1

                elif key == KeyMapping.RollLeft:
                    self.roll += 1
                elif key == KeyMapping.RollRight:
                    self.roll += -1

                elif key == KeyMapping.IncreaseAltitude:
                    self.z_velocity += 1
                elif key == KeyMapping.DecreaseAltitude:
                    self.z_velocity += -1

            # finally we set the command to be sent. The controller handles sending this at regular intervals
            controller.SetCommand(self.roll, self.pitch, self.yaw_velocity,
                                  self.z_velocity)
    def onMessage(self, payload, isBinary):
        # Debug
        if isBinary:
            print("Binary message received: {0} bytes".format(len(payload)))
        else:
            print("Text message received: {0}".format(payload.decode('utf8')))

            # Do stuff
            # pub = rospy.Publisher('blockly', String, queue_size=10)
            # time.sleep(1)
            # pub.publish("blockly says: "+payload.decode('utf8'))

            # Simple text protocol for communication
            # first line is the name of the method
            # next lines are body of message
            message_text = payload.decode('utf8')
            message_data = message_text.split('\n', 1)

            if len(message_data) > 0:
                method_name = message_data[0]
                if len(message_data) > 1:
                    method_body = message_data[1]
                    if method_name.startswith('play'):
                        CodeStatus.set_current_status(CodeStatus.RUNNING)
                        BlocklyServerProtocol.build_ros_node(method_body)
                        rospy.loginfo('The file generated contains...')
                        os.system('cat test.py')
                        if method_name == 'play2':
                            CodeExecution.run_process(['python', 'test.py'])
                        elif method_name == 'play3':
                            CodeExecution.run_process(['python3', 'test.py'])
                    else:
                        rospy.logerr('Called unknown method %s', method_name)
                else:
                    if 'pause' == method_name:
                        CodeStatus.set_current_status(CodeStatus.PAUSED)
                    elif 'resume' == method_name:
                        CodeStatus.set_current_status(CodeStatus.RUNNING)
                    elif 'end' == method_name:
                        #End test.py execution
                        global pid
                        print("@@@@@@@@@@@@@@@@@@")
                        try:
                            print("kill pid="+str(pid))
                            os.kill(pid, signal.SIGKILL)
                            ros_nodes = rosnode.get_node_names()
                        except NameError:
                            print("execution script not running.")
                            pass

                        if '/imu_talker' in ros_nodes: #brain
                            ##set default values
                            pub = rospy.Publisher('/statusleds', String, queue_size=10, latch=True)
                            msg = 'blue_off'
                            pub.publish(msg)
                            msg = 'orange_off'
                            pub.publish(msg)
                        if '/crab_leg_kinematics' in ros_nodes: #spider
                            print("spider running")
                            pub = rospy.Publisher('/joy', Joy, queue_size=10, latch=True)
                            msg = Joy()
                            msg.header.stamp = rospy.Time.now()
                            rate = rospy.Rate(10)
                            valueAxe = 0.0
                            valueButton = 0
                            for i in range (0, 20):
                                msg.axes.append(valueAxe)
                            for e in range (0, 17):
                                msg.buttons.append(valueButton)
                            pub.publish(msg)
                            print("DEFAULT MESSAGES SENT")
                        if '/mavros' in ros_nodes: #rover
                            print("rover")
                            pub = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10)
                            msg = OverrideRCIn()
                            msg.channels[0] = 1500
                            msg.channels[1] = 0
                            msg.channels[2] = 1500
                            msg.channels[3] = 0
                            msg.channels[4] = 0
                            msg.channels[5] = 0
                            msg.channels[6] = 0
                            msg.channels[7] = 0
                            pub.publish(msg)
                        print("@@@@@@@@@@@@@@@@@@")

                    elif method_name.startswith('control'):
                        robot = method_name.split('control_')[1]
                        if robot.startswith('spider'):
                            direction = robot.split('spider_')[1]
                            pub = rospy.Publisher('/joy', Joy, queue_size=10)
                            msg = Joy()
                            msg.header.stamp = rospy.Time.now()
                            rate = rospy.Rate(10)
                               
                            valueAxe = 0.0
                            valueButton = 0
                            for i in range (0, 20):
                                msg.axes.append(valueAxe)
                            for e in range (0, 17):
                                msg.buttons.append(valueButton)

                            if direction == 'up':#forward
                                msg.axes[1] = 1
                            elif direction == 'down':#backwards
                                msg.axes[1] = -1
                            elif direction == 'left':#turn left
                                #msg.axes[0] = 1
                                msg.axes[2] = 1
                            elif direction == 'right':#turn rigth
                                #msg.axes[0] = -1
                                msg.axes[2] = -1

                            pub.publish(msg)
                            rate.sleep()

                        elif robot.startswith('rover'):
                            direction = robot.split('rover_')[1]   
                            rospy.wait_for_service('/mavros/set_mode')
                            change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
                            resp1 = change_mode(custom_mode='manual')
                            print (resp1)
                            if 'True' in str(resp1):
                                pub = rospy.Publisher('mavros/rc/override', OverrideRCIn, queue_size=10)
                                r = rospy.Rate(10) #2hz
                                msg = OverrideRCIn()
                                throttle_channel=2
                                steer_channel=0

                                speed_slow = 1558
                                #speed_turbo = 2000 #dangerous
                                speed = speed_slow

                                if direction == 'up':#forward
                                    msg.channels[throttle_channel]=speed
                                    msg.channels[steer_channel]=1385 #straight
                                elif direction == 'down':#backwards
                                    msg.channels[throttle_channel]=1450 #slow
                                    msg.channels[steer_channel]=1385 #straight
                                elif direction == 'left':#turn left
                                    msg.channels[throttle_channel]=speed
                                    msg.channels[steer_channel]=1285
                                elif direction == 'right':#turn rigth
                                    msg.channels[throttle_channel]=speed
                                    msg.channels[steer_channel]=1485

                                start = time.time()
                                flag=True
                                while not rospy.is_shutdown() and flag:
                                    sample_time=time.time()
                                    if ((sample_time - start) > 0.5):
                                        flag=False
                                    pub.publish(msg)
                                    r.sleep()
                    else:
                        rospy.logerr('Called unknown method %s', method_name)
示例#57
0
def main():
    global objInfos
    rospy.init_node('sequencer')

    move_to_homepose = rospy.ServiceProxy('iktest_controller/move_to_homepose',
                                          Trigger)
    move_to_cup_offset = rospy.ServiceProxy(
        'iktest_controller/move_to_cup_offset', OffsetMove)
    pick_up_dice_above = rospy.ServiceProxy(
        'iktest_controller/pick_up_dice_above', OffsetMove)
    pick_up_dice = rospy.ServiceProxy('iktest_controller/pick_up_dice',
                                      OffsetMove)
    pour_dice = rospy.ServiceProxy('iktest_controller/pour_dice', Trigger)

    #rospy.wait_for_service('iktest_controller/move_to_cup_offset', 3.0)
    rospy.wait_for_service('iktest_controller/pick_up_dice', 3.0)

    close_grip = rospy.ServiceProxy('gripper_controller_test/close_grip',
                                    Trigger)
    open_grip = rospy.ServiceProxy('gripper_controller_test/open_grip',
                                   Trigger)

    # Stored offset pose information
    '''
    dice_pose = Pose(
        position = Point(
        x = 0.776647640113,
        y =-0.0615496888226,
        z = -0.210376983209),
        orientation = Quaternion(
            x = 0.981404960951,
            y = -0.19031770757,
            z = 0.016510737149,
            w = -0.0187314806041
            )
    )
    '''

    # Main process loop
    while (True):

        # Step 1: pour dices out of the cup
        open_grip()
        move_to_homepose()
        rospy.sleep(1)
        pour_dice()

        # Step 2: Start loop of picking up dices
        cnt_round = 0
        FAILURE_TIME = 0
        while FAILURE_TIME < 10:
            cnt_round += 1
            if cnt_round > 15:
                break

            print "---------- THE %dth ROUND ------------" % cnt_round

            move_to_homepose()

            dice_pose = call_cv_service_detect_all_dices()
            if dice_pose is None:
                print "No dice"
                FAILURE_TIME += 1
                continue
            else:
                print "Detect the dice ... "

            def set_dice_orientation(dice_pose):
                dice_pose.position.z = -0.207973876142
                dice_pose.orientation.x = 0.5
                dice_pose.orientation.y = 0.0
                dice_pose.orientation.z = 0.0
                dice_pose.orientation.w = -0.0
                print "The dice pos detected by camera is:\n", dice_pose
                return dice_pose

            def set_dice_above(dice_pose):
                dice_pose.position.z += 0.1
                print "The above position for the dice is:\n", dice_pose
                return dice_pose

            dice_pose = set_dice_orientation(dice_pose)  # set quaternion

            # --------------------------------------
            #OFFSET_Z=0.06
            #dice_pose.position.z = dice_pose.position.z +OFFSET_Z
            dice_pose = set_dice_above(dice_pose)
            pick_up_dice_above(dice_pose)

            # Refine
            dice_pose = call_cv_service_detect_all_dices()
            # dice_pose = call_cv_service_detect_one_dice()
            if dice_pose is None:
                print "During refining the dice pos, no dice is found."
                FAILURE_TIME += 1
                continue
            else:
                print "Refine dice pos successful"
            '''
            # -------------- Publish dice info to the topic
            pretend.state="Dice Read"
            pretend.turn=1
            pretend.roll=cnt_round
            pretend.dice1=1
            pretend.dice2=2
            pretend.dice3=3
            pretend.dice4=4
            pretend.dice5=5
            pretend.dice1color='b'
            pretend.dice2color='b'
            pretend.dice3color='r'
            pretend.dice4color='bl'
            pretend.dice5color='y'

            nobj=len(objInfos)
            nthobj=0
            if nobj>nthobj:
                pretend.dice1=objInfos[nthobj].value
                pretend.dice1color=objInfos[nthobj].color
            nthobj=1
            if nobj>nthobj:
                pretend.dice1=objInfos[nthobj].value
                pretend.dice1color=objInfos[nthobj].color
            nthobj=2
            if nobj>nthobj:
                pretend.dice1=objInfos[nthobj].value
                pretend.dice1color=objInfos[nthobj].color
            nthobj=3
            if nobj>nthobj:
                pretend.dice1=objInfos[nthobj].value
                pretend.dice1color=objInfos[nthobj].color
            nthobj=4
            if nobj>nthobj:
                pretend.dice1=objInfos[nthobj].value
                pretend.dice1color=objInfos[nthobj].color
            # --------------
            '''

            # Gripper goes to the pos which is OFFSET_Z above the dice
            dice_pose = set_dice_orientation(dice_pose)
            #dice_pose.position.z = dice_pose.position.z +OFFSET_Z
            dice_pose = set_dice_above(dice_pose)
            pick_up_dice_above(dice_pose)
            rospy.sleep(1)

            # Gripper goes to grab the dice
            dice_pose.position.z = dice_pose.position.z - 0.1
            print "Height of dice: ", dice_pose.position.z
            pick_up_dice(dice_pose)  #//need Offset

        rospy.sleep(1)
        rospy.loginfo("Sequence complete.")
        break

    return
示例#58
0
import rospy

import dynamic_reconfigure.client


def callback(config):
    rospy.loginfo(
        "Config set to {int_param}, {double_param}, {str_param}, {bool_param}, {size}"
        .format(**config))


if __name__ == "__main__":
    rospy.init_node("dynamic_client")

    rospy.wait_for_service("dynamic_tutorials")
    client = dynamic_reconfigure.client.Client("dynamic_tutorials",
                                               timeout=30,
                                               config_callback=callback)

    r = rospy.Rate(0.1)
    x = 0
    b = False
    while not rospy.is_shutdown():
        x = x + 1
        if x > 10:
            x = 0
        b = not b
        client.update_configuration({
            "int_param": x,
            "double_param": (1 / (x + 1)),
import object_manipulation_msgs.msg
import geometry_msgs.msg
import sensor_msgs.msg
from numpy import *
import openravepy
import pickle
if __name__ == '__main__':
    env = openravepy.Environment()
    body = env.ReadKinBodyXMLFile('data/ketchup.kinbody.xml')
    env.AddKinBody(body, True)
    trimesh = env.Triangulate(body)
    env.Remove(body)
    env.Destroy()

    rospy.init_node('graspplanning_test')
    rospy.wait_for_service('GraspPlanning')
    GraspPlanningFn = rospy.ServiceProxy(
        'GraspPlanning', object_manipulation_msgs.srv.GraspPlanning)
    req = object_manipulation_msgs.srv.GraspPlanningRequest()
    req.arm_name = 'arm'
    #req.target.type = object_manipulation_msgs.msg.GraspableObject.POINT_CLUSTER
    req.target.cluster.header.frame_id = 'Base'
    offset = [0.3, -0.05, 0.3]
    req.target.cluster.points = [
        geometry_msgs.msg.Point32(p[0] + offset[0], p[1] + offset[1],
                                  p[2] + offset[2]) for p in trimesh.vertices
    ]
    req.target.cluster.channels.append(
        sensor_msgs.msg.ChannelFloat32(
            name='indices', values=trimesh.indices.flatten().tolist()))
    req.collision_object_name = ''
示例#60
0
 def __init__(self):
     rospy.wait_for_service('testit/bringup')
     rospy.wait_for_service('testit/teardown')
     rospy.wait_for_service('testit/status')
     rospy.wait_for_service('testit/test')
     rospy.wait_for_service('testit/results')
     rospy.wait_for_service('testit/bag/collect')
     rospy.wait_for_service('testit/clean')
     rospy.wait_for_service('testit/uppaal/annotate/coverage')
     rospy.wait_for_service('testit/uppaal/extract/failure')
     rospy.wait_for_service('testit/shutdown')
     rospy.wait_for_service('testit/credits')
     rospy.wait_for_service('testit/optimize')
     rospy.wait_for_service('testit/learn')
     self.bringup_service = rospy.ServiceProxy('testit/bringup',
                                               testit.srv.Command)
     self.teardown_service = rospy.ServiceProxy('testit/teardown',
                                                testit.srv.Command)
     self.status_service = rospy.ServiceProxy('testit/status',
                                              testit.srv.Command)
     self.test_service = rospy.ServiceProxy('testit/test',
                                            testit.srv.Command)
     self.learn_service = rospy.ServiceProxy('testit/learn',
                                             testit.srv.Command)
     self.results_service = rospy.ServiceProxy('testit/results',
                                               testit.srv.Command)
     self.bag_collect_service = rospy.ServiceProxy('testit/bag/collect',
                                                   testit.srv.Command)
     self.clean_service = rospy.ServiceProxy('testit/clean',
                                             testit.srv.Command)
     self.uppaal_annotate_coverage_service = rospy.ServiceProxy(
         'testit/uppaal/annotate/coverage', testit.srv.Command)
     self.uppaal_extract_failure_service = rospy.ServiceProxy(
         'testit/uppaal/extract/failure', testit.srv.Command)
     self.shutdown_service = rospy.ServiceProxy('testit/shutdown',
                                                testit.srv.Command)
     self.credits_service = rospy.ServiceProxy('testit/credits',
                                               testit.srv.Command)
     self.optimize_service = rospy.ServiceProxy('testit/optimize',
                                                testit.srv.Command)
     self.log_service = rospy.ServiceProxy('testit/log', testit.srv.Command)