mavros_driver.py 文件源码

python
阅读 16 收藏 0 点赞 0 评论 0

项目:offboard 作者: Stifael 项目源码 文件源码
def set_mode(self, mode):
        if not self.current_state.connected:
            print "No FCU connection"

        elif self.current_state.mode == mode:
            print "Already in " + mode + " mode"

        else:

            # wait for service
            rospy.wait_for_service("mavros/set_mode")   


            # service client
            set_mode = rospy.ServiceProxy("mavros/set_mode", SetMode)


            # set request object
            req = SetModeRequest()
            req.custom_mode = mode


            # zero time 
            t0 = rospy.get_time()


            # check response
            while not rospy.is_shutdown() and (self.current_state.mode != req.custom_mode):
                if rospy.get_time() - t0 > 2.0: # check every 5 seconds

                    try:
                        # request 
                        set_mode.call(req)

                    except rospy.ServiceException, e:
                        print "Service did not process request: %s"%str(e)

                    t0 = rospy.get_time()


            print "Mode: "+self.current_state.mode + " established"
评论列表
文章目录


问题


面经


文章

微信
公众号

扫码关注公众号