ROS基础教程——代码补充

ROS基础教程——代码补充

1. VMware许可证

MC60H-DWHD5-H80U9-6V85M-8280D

2. Vscode代码编译配置

{
// 有关 tasks.json 格式的文档，请参见
    // https://go.microsoft.com/fwlink/?LinkId=733558
    "version": "2.0.0",
    "tasks": [
        {
            "label": "catkin_make:debug", //代表提示的描述性信息
            "type": "shell",  //可以选择shell或者process,如果是shell代码是在shell里面运行一个命令，如果是process代表作为一个进程来运行
            "command": "catkin_make",//这个是我们需要运行的命令
            "args": [],//如果需要在命令后面加一些后缀，可以写在这里，比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”
            "group": {"kind":"build","isDefault":true},
            "presentation": {
                "reveal": "always"//可选always或者silence，代表是否输出信息
            },
            "problemMatcher": "$msCompile"
        }
    ]
}

3. Demo01程序代码

功能包

roscpp rospy std_msgs turtlesim

发布信息

#! /usr/bin/env python
# _*_ coding=UTF-8 _*_

import rospy
from geometry_msgs.msg import Twist

if __name__ == "__main__":

    rospy.init_node("control_circle_p")

    pub = rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=1000)

    rate = rospy.Rate(10)
    msg = Twist()
    msg.linear.x = 1.0
    msg.linear.y = 0.0
    msg.linear.z = 0.0
    msg.angular.x = 0.0
    msg.angular.y = 0.0
    msg.angular.z = 0.5

    while not rospy.is_shutdown():
        pub.publish(msg)
        rate.sleep()

订阅信息

#! /usr/bin/env python
# _*_ coding=UTF-8 _*_

import rospy
from turtlesim.msg import Pose

def doPose(data):
    rospy.loginfo("turtle:x=%.2f, y=%.2f,theta=%.2f",data.x,data.y,data.theta)

if __name__ == "__main__":


    rospy.init_node("sub_pose_p")

    sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=1000)

    rospy.spin()

服务通信

#! /usr/bin/env python
# _*_ coding=UTF-8 _*_

import rospy
from turtlesim.srv import Spawn,SpawnRequest,SpawnResponse

if __name__ == "__main__":

    rospy.init_node("set_turtle_p")

    client = rospy.ServiceProxy("/spawn",Spawn)

    client.wait_for_service()

    req = SpawnRequest()
    req.x = 2.0
    req.y = 2.0
    req.theta = -1.57
    req.name = "my_turtle_p"
    try:
        response = client.call(req)

        rospy.loginfo("success!, calls:%s",response.name)
    except expression as identifier:
        rospy.loginfo("fail!")

参数服务器

#! /usr/bin/env python
# _*_ coding=UTF-8 _*_

import rospy

if __name__ == "__main__":
    rospy.init_node("hehe")
    rospy.set_param("/turtlesim/background_r",160)
    rospy.set_param("/turtlesim/background_g",160)
    rospy.set_param("/turtlesim/background_b",255)
    # rospy.set_param("background_r",255)
    # rospy.set_param("background_g",0)
    # rospy.set_param("background_b",255) 

4. Demo02代码

功能包

tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs turtlesim

launch文件

<launch>
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key_control" output="screen"/>

    <node pkg="tf_test" type="turtle_srv.py" name="turtle_spawn" output="screen"/>

    <node pkg="tf_test" type="turtle_sub.py" name="tf_sub1" args="turtle1" output="screen"/>
    <node pkg="tf_test" type="turtle_sub.py" name="tf_sub2" args="turtle2" output="screen"/>
   
    <node pkg="tf_test" type="turtle_pub.py" name="tf_pub" output="screen"/>
</launch>

订阅方

#! /usr/bin/env python
# _*_ coding=UTF-8 _*_

import rospy
import sys
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
import tf2_ros
import tf_conversions

turtle_name = ""

def doPose(pose):
    # rospy.loginfo("x = %.2f",pose.x)

    broadcaster = tf2_ros.TransformBroadcaster()

    tfs = TransformStamped()
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()

    tfs.child_frame_id = turtle_name
    tfs.transform.translation.x = pose.x
    tfs.transform.translation.y = pose.y
    tfs.transform.translation.z = 0.0

    qtn = tf_conversions.transformations.quaternion_from_euler(0, 0, pose.theta)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]

    broadcaster.sendTransform(tfs)


if __name__ == "__main__":

    rospy.init_node("sub_tfs_p")

    if len(sys.argv) < 2:
        rospy.loginfo("argv failed")
    else:
        turtle_name = sys.argv[1]

    rospy.Subscriber(turtle_name + "/pose",Pose,doPose)

    rospy.spin()

发布方

#! /usr/bin/env python
# _*_ coding=UTF-8 _*_

import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped, Twist
import math

if __name__ == "__main__":

    rospy.init_node("sub_tfs_p")

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

    rate = rospy.Rate(10)

    pub = rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=1000)
    while not rospy.is_shutdown():

        rate.sleep()
        try:
            #def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
            trans = buffer.lookup_transform("turtle2","turtle1",rospy.Time(0))

            twist = Twist()

            twist.linear.x = 0.5 * math.sqrt(math.pow(trans.transform.translation.x,2) + math.pow(trans.transform.translation.y,2))
            twist.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)

            pub.publish(twist)

        except Exception as e:
            rospy.logwarn("!Warning:%s",e)

服务方

#! /usr/bin/env python
# _*_ coding=UTF-8 _*_

import rospy
from turtlesim.srv import Spawn, SpawnRequest, SpawnResponse

if __name__ == "__main__":

    rospy.init_node("turtle_spawn_p")

    client = rospy.ServiceProxy("/spawn",Spawn)

    client.wait_for_service()

    req = SpawnRequest()
    req.x = 1.0
    req.y = 1.0
    req.theta = 3.14
    req.name = "turtle2"

    try:
        response = client.call(req)
        rospy.loginfo("success, turtle name:%s",response.name)
    except Exception as e:
        rospy.loginfo("fail!")

5. Demo03代码

功能包

roscpp rospy std_msgs urdf xacro

安装ros功能包

sudo apt install ros-melodic-joint-state-publisher-gui

launch文件

<launch>
    <param name="robot_description" textfile="$(find urdf_test)/urdf/robot.urdf" />

    <node pkg="rviz" type="rviz" name="rviz_test"/>

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />

    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />

</launch>

URDF

<robot name="mycar">
   
    <link name="base_footprint">
        <visual>
            <geometry>
                <sphere radius="0.001" />
            </geometry>
        </visual>
    </link>

    <link name="base_link">
            <visual>
                <geometry>
                    <cylinder radius="0.1" length="0.08" />
                </geometry>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <material name="yellow">
                    <color rgba="0.8 0.3 0.1 0.5" />
                </material>
            </visual>
    </link>

        <joint name="base_link2base_footprint" type="fixed">
            <parent link="base_footprint" />
            <child link="base_link"/>
            <origin xyz="0 0 0.055" />
        </joint>

    <link name="left_wheel">
        <visual>
            <geometry>
                <cylinder radius="0.0325" length="0.015" />
            </geometry>
            <origin xyz="0 0 0" rpy="1.5705 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>

    </link>

    <joint name="left_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="left_wheel" />
        <origin xyz="0 0.1 -0.0225" />
        <axis xyz="0 1 0" />
    </joint>


    <link name="right_wheel">
        <visual>
            <geometry>
                <cylinder radius="0.0325" length="0.015" />
            </geometry>
            <origin xyz="0 0 0" rpy="1.5705 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>

    </link>

    <joint name="right_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="right_wheel" />
        <origin xyz="0 -0.1 -0.0225" />
        <axis xyz="0 1 0" />
    </joint>

    <link name="front_wheel">
        <visual>
            <geometry>
                <sphere radius="0.0075" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>
    </link>

    <joint name="front_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="front_wheel" />
        <origin xyz="0.0925 0 -0.0475" />
        <axis xyz="1 1 1" />
    </joint>

    <link name="back_wheel">
        <visual>
            <geometry>
                <sphere radius="0.0075" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>
    </link>

    <joint name="back_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="back_wheel" />
        <origin xyz="-0.0925 0 -0.0475" />
        <axis xyz="1 1 1" />
    </joint>
</robot>