앞에 쓰다

로봇의 밑받침을 앞으로 가기 위해 지정된 거리를 지정하기 위해'1미터 앞으로 가자'는 코드에 따라(이 코드는 많은ros 참고서에 있을 것이다).프로그램 인용을 편리하게 하기 위해서, 나는 직접 그것을 서비스로 바꾸어 나중에 프로그램 내에서 호출하기 편리하게 했다.

소스 코드

좀 길어요. 마지막 링크에 넣었어요.

서비스 설명 및 코드

현재의 서비스는 요청할 때 전진할 거리를 휴대하고 디스크에서 상응하는 거리를 전진하는 것이다.코드는 다음과 같이 아주 작게 변경되었습니다.

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist, Point
from math import copysign, sqrt, pow
import tf
from carry.srv import MoveTowards


class CalibrateLinear():
    def __init__(self):
	    rospy.init_node('move_towards_server', anonymous=False)	
	    service= rospy.Service('move_towards', MoveTowards, self.move)
    def move(self,distanse_you_want):
        print distanse_you_want.a
        # Set rospy to execute a shutdown function when terminating the script
        rospy.on_shutdown(self.shutdown)
        
        # How fast will we check the odometry values?
        self.rate = rospy.get_param('~rate', 20)
        r = rospy.Rate(self.rate)
        
        # Set the distance to travel
        self.test_distance = rospy.get_param('~test_distance', distanse_you_want.a) # meters
        self.speed = rospy.get_param('~speed', 0.15) # meters per second
        self.tolerance = rospy.get_param('~tolerance', 0.01) # meters
        self.odom_linear_scale_correction = rospy.get_param('~odom_linear_scale_correction', 1.0)
        self.start_test = rospy.get_param('~start_test', True)
        
        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
        
 
        # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
        self.base_frame = rospy.get_param('~base_frame', '/base_footprint')

        # The odom frame is usually just /odom
        #self.odom_frame = rospy.get_param('~odom_frame', '/odom')          
        self.odom_frame = rospy.get_param('~odom_frame', '/odom_combined')

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()
        
        # Give tf some time to fill its buffer
        rospy.sleep(2)
        
        # Make sure we see the odom and base frames
        self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))        
            
        rospy.loginfo("Bring up rqt_reconfigure to control the test.")
  
        self.position = Point()
        
        # Get the starting position from the tf transform between the odom and base frames
        self.position = self.get_position()
        
        x_start = self.position.x
        y_start = self.position.y
            
        move_cmd = Twist()
            
        while not rospy.is_shutdown():
            # Stop the robot by default
            move_cmd = Twist()
            
            if self.start_test:
                # Get the current position from the tf transform between the odom and base frames
                self.position = self.get_position()
                
                # Compute the Euclidean distance from the target point
                distance = sqrt(pow((self.position.x - x_start), 2) +
                                pow((self.position.y - y_start), 2))
                                
                # Correct the estimated distance by the correction factor
                distance *= self.odom_linear_scale_correction
                
                # How close are we?
                error =  distance - self.test_distance
                
                # Are we close enough?
                if not self.start_test or abs(error) <  self.tolerance:
                    self.start_test = False
                    params = {'start_test': False}
                    rospy.loginfo(params)
                else:
                    # If not, move in the appropriate direction
                    move_cmd.linear.x = copysign(self.speed, -1 * error)
            else:
                self.position = self.get_position()
                x_start = self.position.x
                y_start = self.position.y
		        break
                
            self.cmd_vel.publish(move_cmd)
            r.sleep()

        # Stop the robot
        self.cmd_vel.publish(Twist())
	print 'finish moving'
	return []

    def get_position(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return Point(*trans)
        
    def shutdown(self):
        # Always stop the robot when shutting down the node
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)
 
if __name__ == '__main__':
    try:
        CalibrateLinear()
        rospy.spin()
    except:
        rospy.loginfo("Calibration terminated.")

필요한 수정 사항

1. srv 추가

'앞으로 이동하는 거리'를 제출해야 하기 때문에 srv가 필요합니다. 유형은 요청은 부동점수(전진하는 거리)로 공백일 수 있습니다.다음은 제가 새로 지은 Move Towards입니다.srv, 아주 간단한 두 줄, 첫 번째 줄은 요청, 두 번째 줄은 구분선, 응답은 비어 있습니다.

float32 a
---

srv를 만드는 방법에 대해서는 쓰지 않겠습니다. ROS wiki 링크를 참고하십시오.ROS 메시지 및 ROS 서비스를 만듭니다.

2. 원본 프로그램의 수정

우선 import 서비스, 주의 형식:from '패키지 이름.srv' import 'srv 이름'.파일 이름은 MoveTowards이지만srv. 그런데 밑에 import MoveTowards.

from carry.srv import MoveTowards

다음은 이 부분입니다.

    def __init__(self):
	    rospy.init_node('move_towards_server', anonymous=False)	
	    service= rospy.Service('move_towards', MoveTowards, self.move)
    def move(self,distanse_you_want):

이전에'1미터 앞으로 이동하는 코드 세그먼트'는 초기화 안에서 함수 모브로 이동했고 초기화는 노드와 서비스만 정의하는 문장만 포함했다.서비스 요청이 있으면 리셋 함수'move'를 실행합니다.다음 사항에 유의하십시오.

        #self.odom_frame = rospy.get_param('~odom_frame', '/odom')          
        self.odom_frame = rospy.get_param('~odom_frame', '/odom_combined')

왜냐하면 저희 밑판은 로봇이...pose_ekf 이 기능 패키지는 센서 정보 융합을 했고 발표된 프레임은/odom/odom이 아니라 combined이기 때문에 주의해야 합니다.rosrun rqt 사용 가능tf_tree rqt_tf_tree는 tf 트리를 보고 도대체 어떤 나무인지 봅시다.이 줄이 틀렸으니 로봇은 틀림없이 움직일 수 없을 것이다.그리고 두 개의 작은 부분, 몇 가지 세부 사항을 바꾸었다. (#의 두 줄을 표시했다)

            else:
                self.position = self.get_position()
                x_start = self.position.x
                y_start = self.position.y
		        break    ##
                
            self.cmd_vel.publish(move_cmd)
            r.sleep()

        # Stop the robot
        self.cmd_vel.publish(Twist())
	print 'finish moving'
	return []            ##

디스켓이 지정한 위치에 도착한 후에break가 순환을 벗어나고 원본 프로그램에break가 없습니다. 실행이 끝난 후while 안에 있습니다. 그러나 이 서버가 정보를 되돌려 주어야 합니다. 그렇지 않으면 클라이언트 프로그램이 계속 기다리고 있습니다. 프로그램이 계속 막혀서 실행되지 않습니다.마지막으로 리턴을 추가했습니다. 비록 '빈' 으로 되돌아왔지만, 리턴이 실행되었는지 클라이언트가 알지 못해서 프로그램이 막혔습니다.

첨부

코드 복사에 약간의 축소 문제가 있는 것 같으니 다운로드 링크를 첨부합시다