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>