基于Linux系统的ROS海龟跟踪实验

最新推荐文章于 2024-09-05 22:41:51 发布

Death__God_

最新推荐文章于 2024-09-05 22:41:51 发布

阅读量711

点赞数 1

文章标签： python 开发语言 linux 目标跟踪

本文链接：https://blog.csdn.net/Death__God_/article/details/125385454

版权

一、启动安装与启动roscore

可以去鱼香ROS一键安装ROS,网址为http://fishros.com

进去后点击一键安装

就能看到一键安装指令

wget http://fishros.com/install -O fishros && . fishros

安装完毕后即可启动roscore

二、工作空间的创建

使用catkin_create_pkg 创建功能包，__name__为仍以附属名，后面为依赖，如果使用python编程，可不加roscpp。

三、进入__name__底下的src进行python文件的编写。

海龟跟踪的代码如下：

#!/usr/bin/env python3

from lib2to3.pgen2.token import RPAR
from operator import inv
import re
import sys
import rospy
import numpy as np
import math
from turtlesim.srv import Spawn
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose

global  pose1
global topicPub

def pose1Callback(msg):
    global pose1
    pose1.x=msg.x
    pose1.y=msg.y
    rospy.loginfo("Turtle1111 pose: x:%0.6f, y:%0.6f", msg.x, msg.y)

def pose2Callback(msg):
    global pose1
    pose2 = Pose()
    pose2.x=msg.x
    pose2.y=msg.y
    rospy.loginfo("Turtle2222pose: x:%0.6f, y:%0.6f", msg.x, msg.y)
    pose2.theta = msg.theta
    c = math.cos(msg.theta)
    s = math.sin(msg.theta)

    inv_Org = Pose()
    inv_Org.x = -1*(        c * pose2.x  +  s  *  pose2.y       )
    inv_Org.y = -1*(  -1*s  * pose2.x  +  c  * pose2.y       )

    Rotated_P = Pose()
    Rotated_P.x=            c * pose1.x  +  s  *  pose1.y 
    Rotated_P.y=   -1*   s * pose1.x  +  c *  pose1.y 

    relatedPose = Pose()
    relatedPose.x = Rotated_P.x  +  inv_Org.x
    relatedPose.y = Rotated_P.y + inv_Org.y

    vel_msg = Twist()
    vel_msg.linear.x = math.sqrt(relatedPose.x * relatedPose.x + relatedPose.y * relatedPose.y )
    vel_msg.angular.z = math.atan2(relatedPose.y , relatedPose.x)
    turtle_vel_pub.publish(vel_msg)




if __name__ == "__main__":
    pose1 = Pose()
    rospy.init_node("turtle_following")
    try:
        add_turtle = rospy.ServiceProxy('/spawn' , Spawn)
        response = add_turtle(1 , 1 , 0, "turtle2")
    except rospy.ServiceException as e:
        print("Service call failed:%s",e)

    turtle_vel_pub = rospy.Publisher('/turtle2/cmd_vel',Twist,queue_size=10)
    rospy.Subscriber("/turtle1/pose",Pose,pose1Callback)
    rospy.Subscriber("/turtle2/pose",Pose,pose2Callback)
    rospy.spin()

在编辑过程中，遇到了一个问题