机器人系统ROS的简单实验、

接上回我们安装了ROS操作系统 安装机器人操作系统ROS在Ubuntu环境下
本文学习古月居ROS教学课件和相关视频，完成对ROS操作系统的简单学习使用

一、ROS工作环境和工作包

新建目录catkin_ws和src存放文件

cd ~
mkdir -p catkin_ws/src

创建ROS工作环境

cd ~/catkin_ws/src
catkin_init_workspace

编译环境

cd ~/catkin_ws
catkin_make

添加环境变量

cd ~/catkin_ws
source devel/setup.bash
gedit ~/.bashrc
echo $ROS_PACKAGE_PATH //查看环境

添加红框内容：

创建功能包

cd ~/catkin_ws/src
catkin_create_pkg learning_communication std_msgs rospy roscpp

编译功能包

cd ~/catkin_ws
catkin_make

1、话题编程

话题编程的流程：
1、创建发布者
2、创建订阅者
3、添加编译
4、用行程序

创建发布者

流程：
1、初始化ROS节点
2、向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型
3、创建消息数据
4、按照一定频率循环发布消息

发布者代码：

#include 
#include 

int main(int argc, char **argv)
{
	// ROS节点初始化
	ros::init(argc, argv, "velocity_publisher");

	// 创建节点句柄
	ros::NodeHandle n;

	// 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
	ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

	// 设置循环的频率
	ros::Rate loop_rate(10);

	int count = 0;
	while (ros::ok())
	{
	    // 初始化geometry_msgs::Twist类型的消息
		geometry_msgs::Twist vel_msg;
		vel_msg.linear.x = 0.5;
		vel_msg.angular.z = 0.2;

	    // 发布消息
		turtle_vel_pub.publish(vel_msg);
		ROS_INFO("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", 
				vel_msg.linear.x, vel_msg.angular.z);

	    // 按照循环频率延时
	    loop_rate.sleep();
	}

	return 0;
}

在CMakeLists.txt配置编译规则

add_executable(velocity_publisher src/velocity_publisher.cpp)//编译代码
target_link_libraries(velocity_publisher ${catkin_LIBRARIES})//连接作用

编译

cd ~/catkin_ws
catkin_make
roscore
rosrun turtlesim turtlesim_node
rosrun learning_communication velocity_publisher

创建订阅者

创建订阅者流程：
1、初始化ROS节点
2、订阅需要的 话题
3、循环等待话题消息，接收到消息后进入回调函数
4、在回调函数中完成消息的处理

订阅者代码：

#include 
#include "turtlesim/Pose.h"

// 接收到订阅的消息后，会进入消息回调函数
void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "pose_subscriber");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);

    // 循环等待回调函数
    ros::spin();

    return 0;
}

在CMakeLists.txt配置编译规则

add_executable(pose_subscriber src/pose_subscriber.cpp)
target_link_libraries(pose_subscriber ${catkin_LIBRARIES})

编译

cd ~/catkin_ws
catkin_make
roscore
rosrun turtlesim turtlesim_node
rosrun learning_topic pose_subscriber

2、服务编程

服务编程流程
1、创建服务端
2、创建客户端
3、添加编译
4、运行可执行程序

创建功能包：

cd ~/catkin_ws/src
catkin_create_pkg learning_service std_msgs rospy roscpp geometry_msgs turtlesim
cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash

客户端代码：

/**
 * 该例程将请求/spawn服务，服务数据类型turtlesim::Spawn
 */

#include 
#include 

int main(int argc, char** argv)
{
    // 初始化ROS节点
	ros::init(argc, argv, "turtle_spawn");

    // 创建节点句柄
	ros::NodeHandle node;

    // 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
	ros::service::waitForService("/spawn");
	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");

    // 初始化turtlesim::Spawn的请求数据
	turtlesim::Spawn srv;
	srv.request.x = 2.0;
	srv.request.y = 2.0;
	srv.request.name = "turtle2";

    // 请求服务调用
	ROS_INFO("Call service to spwan turtle[x:%0.6f, y:%0.6f, name:%s]", 
			 srv.request.x, srv.request.y, srv.request.name.c_str());

	add_turtle.call(srv);

	// 显示服务调用结果
	ROS_INFO("Spwan turtle successfully [name:%s]", srv.response.name.c_str());

	return 0;
};

添加编译规则：

add_executable(turtle_spawn src/turtle_spawn.cpp)
target_link_libraries(turtle_spawn ${catkin_LIBRARIES})

编译

cd ~/catkin_ws
catkin_make
roscore
rosrun turtlesim turtlesim_node
rosrun learning_service turtle_spawn

服务器端代码：

/**
 * 该例程将执行/turtle_command服务，服务数据类型std_srvs/Trigger
 */
 
#include 
#include 
#include 

ros::Publisher turtle_vel_pub;
bool pubCommand = false;

// service回调函数，输入参数req，输出参数res
bool commandCallback(std_srvs::Trigger::Request  &req,
         			std_srvs::Trigger::Response &res)
{
	pubCommand = !pubCommand;

    // 显示请求数据
    ROS_INFO("Publish turtle velocity command [%s]", pubCommand==true?"Yes":"No");

	// 设置反馈数据
	res.success = true;
	res.message = "Change turtle command state!"

    return true;
}

int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "turtle_command_server");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个名为/turtle_command的server，注册回调函数commandCallback
    ros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallback);

	// 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
	turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

    // 循环等待回调函数
    ROS_INFO("Ready to receive turtle command.");

	// 设置循环的频率
	ros::Rate loop_rate(10);

	while(ros::ok())
	{
		// 查看一次回调函数队列
    	ros::spinOnce();
		
		// 如果标志为true，则发布速度指令
		if(pubCommand)
		{
			geometry_msgs::Twist vel_msg;
			vel_msg.linear.x = 0.5;
			vel_msg.angular.z = 0.2;
			turtle_vel_pub.publish(vel_msg);
		}

		//按照循环频率延时
	    loop_rate.sleep();
	}

    return 0;
}

添加编译规则

add_executable(turtle_command_server src/turtle_command_server.cpp)
target_link_libraries(turtle_command_server ${catkin_LIBRARIES})

编译

cd ~/catkin_ws
catkin_make
roscore
rosrun turtlesim turtlesim_node
rosrun learning_service turtle_command_server

二、Rviz工具使用

虚拟机>>可移动设备>>camera>>启动
我们可以看到摄像头点亮

1、安装Rviz

安装相关包

sudo apt-get install ros-melodic-uvc-camera
sudo apt-get install ros-melodic-image-*
sudo apt-get install ros-melodic-rqt-image-view

启动

roscore

启动摄像机节点

rosrun uvc_camera uvc_camera_node

发生错误不影响摄像机录像

查看话题消息

rostopic list

查看摄像机信息

rostopic echo /camera_info

启动Rviz

rviz

2、Rviz打开摄像头

add添加image。

image Topic设置如下：

看样子我们是成功完成摄像了！

三、gazebo 仿真

1、URDF机器人搭建

创建机器人建模的功能包

cd ~/catkin_ws/src/
catkin_create_pkg mbot_description urdf xacro

新建文件如下：
urdf 存放机器人模型的URDF或xacro文件
meshes 放置URDF中引用的模型渲染文件，机器人外观纹理，让外观和实际更相似
launch 保存相关启动文件
config 保存rviz的配置文件、功能包的配置文件

编辑launch文件

cd ~/catkin_ws/src/mbot_description/launch
sudo gedit display_mbot_base_urdf.launch

内容如下：

<launch>
        <!-- 加载的参数名字叫robot_description，具体内容是urdf相关模型的路径 -->
	<param name="robot_description" textfile="$(find mbot_description)/urdf/mbot_base.urdf" />
	<!-- 设置GUI参数，显示关节控制插件 -->
	<param name="use_gui" value="true"/>
	<!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
	<!-- 运行robot_state_publisher节点，发布tf  -->
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
	<!-- 运行rviz可视化界面，保存每次打开之后的相关插件，保存到config文件夹下面 -->
	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mbot_description)/config/mbot_urdf.rviz" required="true" />
</launch>

第一步：使用圆柱体创建一个车体模型

cd ~/catkin_ws/src/mbot_description/urdf
sudo gedit mbot_base.urdf

内容如下：

放入文件中需要删除注释，不然会报错，后面一样。

<?xml version="1.0" ?>
<robot name="mbot">
    <link name="base_link">
        <visual>
            <origin xyz=" 0 0 0" rpy="0 0 0" />
            <!-- link坐标位置，放在最中央 ，xyz单位是米，rpy单位是弧度-->
            <geometry>
                <cylinder length="0.16" radius="0.20"/>
            </geometry>
            <!-- 机器人的外观效果，使用圆柱体，高0.16，半径0.20 -->
            <material name="yellow">
                <color rgba="1 0.4 0 1"/>
            </material>
             <!--  通过rgba来描述颜色，颜色命名为yellow,a为1是不透明-->
        </visual>
        <!--visual指的是一些物理属性的标签 -->
    </link>


</robot>

运行代码:

roslaunch mbot_description display_mbot_base_urdf.launch

设置Fixed Frame为base_list

在add添加RobotModel

第二步：使用圆柱体创建左轮

在mbot_base.urdf中添加代码

<joint name="left_wheel_joint" type="continuous">
        <origin xyz="0 0.19 -0.05" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="left_wheel_link"/>
        <axis xyz="0 1 0"/>
    </joint>
    <link name="left_wheel_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" />
            <geometry>
                <cylinder radius="0.06" length = "0.025"/>
            </geometry>
            <material name="white">
                <color rgba="1 1 1 0.9"/>
            </material>
        </visual>
    </link>

第三步：使用圆柱体创建右轮

在mbot_base.urdf中添加代码

<joint name="right_wheel_joint" type="continuous">
        <origin xyz="0 -0.19 -0.05" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="right_wheel_link"/>
        <axis xyz="0 1 0"/>
    </joint>

    <link name="right_wheel_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" />
            <geometry>
                <cylinder radius="0.06" length = "0.025"/>
            </geometry>
            <material name="white">
                <color rgba="1 1 1 0.9"/>
            </material>
        </visual>
    </link>

第四步：使用球体创建支撑脚

在mbot_base.urdf中添加代码

<joint name="front_caster_joint" type="continuous">
        <origin xyz="0.18 0 -0.095" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="front_caster_link"/>
        <axis xyz="0 1 0"/>
    </joint>
    <link name="front_caster_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <sphere radius="0.015" />
            </geometry>
            <material name="black">
                <color rgba="0 0 0 0.95"/>
            </material>
        </visual>
    </link>
    <joint name="back_caster_joint" type="continuous">
        <origin xyz="-0.18 0 -0.095" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="back_caster_link"/>
        <axis xyz="0 1 0"/>
    </joint>
    <link name="back_caster_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <sphere radius="0.015" />
            </geometry>
            <material name="black">
                <color rgba="0 0 0 0.95"/>
            </material>
        </visual>
    </link>

结果如下：

第五步：创建传感器摄像头

创建文件mbot_with_camera.urdf在前面代码的基础上新增代码如下：

<!--摄像头相关内容-->
    <link name="camera_link">
        <visual>
            <origin xyz=" 0 0 0 " rpy="0 0 0" />
            <geometry>
            
                <box size="0.03 0.04 0.04" />
                   <!--外观是个盒子模拟成摄像头，长宽高分别是0.03 0.04 0.04-->
            </geometry>
            <material name="black">
                <color rgba="0 0 0 0.95"/>
            </material>
        </visual>
    </link>

    <joint name="camera_joint" type="fixed">
        <origin xyz="0.17 0 0.10" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="camera_link"/>
    </joint>

创建文件display_mbot_with_camera_urdf.launch在前面launch文件的基础修改urdf文件位置

<launch>
        <!-- 加载的参数名字叫robot_description，具体内容是urdf相关模型的路径 -->
	<param name="robot_description" textfile="$(find mbot_description)/urdf/mbot_with_camera.urdf" />
	<!-- 设置GUI参数，显示关节控制插件 -->
	<param name="use_gui" value="true"/>
	<!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
	<!-- 运行robot_state_publisher节点，发布tf  -->
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
	<!-- 运行rviz可视化界面，保存每次打开之后的相关插件，保存到config文件夹下面 -->
	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mbot_description)/config/mbot_urdf.rviz" required="true" />
</launch>

运行程序

 roslaunch mbot_description display_mbot_with_camera_urdf.launch 

结果如下：

第五步：创建传感器激光雷达

创建文件mbot_with_laser.urdf在前面代码的基础上新增代码如下：

    <!--激光雷达配置内容-->
    <link name="laser_link">
		<visual>
			<origin xyz=" 0 0 0 " rpy="0 0 0" />
			<geometry>
				<cylinder length="0.05" radius="0.05"/>
			</geometry>
			<material name="black"/>
		</visual>
    </link>
    <joint name="laser_joint" type="fixed">
        <origin xyz="0 0 0.105" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="laser_link"/>
    </joint>

创建文件display_mbot_with_laser_urdf.launch在前面launch文件的基础修改urdf文件位置

<launch>
        <!-- 加载的参数名字叫robot_description，具体内容是urdf相关模型的路径 -->
	<param name="robot_description" textfile="$(find mbot_description)/urdf/mbot_with_laser.urdf" />
	<!-- 设置GUI参数，显示关节控制插件 -->
	<param name="use_gui" value="true"/>
	<!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
	<!-- 运行robot_state_publisher节点，发布tf  -->
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
	<!-- 运行rviz可视化界面，保存每次打开之后的相关插件，保存到config文件夹下面 -->
	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mbot_description)/config/mbot_urdf.rviz" required="true" />
</launch>

运行程序

roslaunch mbot_description display_mbot_with_laser_urdf.launch

结果如下：

第五步：创建传感器Kinect

创建文件mbot_with_kinect.urdf在前面代码的基础上新增代码如下：

    <!--Kinect配置-->
    <link name="kinect_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 1.5708"/>
            <geometry>
                <mesh filename="package://mbot_description/meshes/kinect.dae" />
                 <!--直接加载Kinect的外观纹理描述文件-->
            </geometry>
        </visual>
    </link>
    <joint name="laser_joint" type="fixed">
        <origin xyz="0.15 0 0.11" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="kinect_link"/>
    </joint>

创建文件display_mbot_with_kinect_urdf.launch在前面launch文件的基础修改urdf文件位置

<launch>
   <!-- 加载的参数名字叫robot_description，具体内容是urdf相关模型的路径 -->
	<param name="robot_description" textfile="$(find mbot_description)/urdf/mbot_with_kinect.urdf" />
	<!-- 设置GUI参数，显示关节控制插件 -->
	<param name="use_gui" value="true"/>
	<!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
	<!-- 运行robot_state_publisher节点，发布tf  -->
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
	<!-- 运行rviz可视化界面，保存每次打开之后的相关插件，保存到config文件夹下面 -->
	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mbot_description)/config/mbot_urdf.rviz" required="true" />
</launch>

运行程序

roslaunch mbot_description display_mbot_with_kinect_urdf.launch

2、gazebo 仿真器搭建环境

参考Gazebo搭建一个三维虚拟环境 —— 通过Rviz显示机器人camera采集的信息

1. 新建gazebo机器人仿真工具包

cd ~/catkin_ws/src
catkin_create_pkg mbot_gazebo std_msgs rospy roscpp
cd ~/catkin_ws
catkin_make

2. 创建工程所需文件

cd ~/catkin_ws/src/mbot_gazebo
mkdir launch
cd launch
gedit gazebo_empty.launch

添加内容如下：

<launch>
    <!-- 设置launch文件的参数 -->
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="false"/>
    <arg name="debug" default="false"/>

    <!-- 运行gazebo仿真环境 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="debug" value="$(arg debug)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>

    <!-- 加载机器人模型描述参数 -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/xacro/gazebo/mbot_gazebo.xacro'" /> 

    <!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node> 

    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" >
        <param name="publish_frequency" type="double" value="50.0" />
    </node>

    <!-- 在gazebo中加载机器人模型-->
    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
          args="-urdf -model mrobot -param robot_description"/> 

</launch>

4. 修改CMakeLisets.txt内容

gedit ~/catkin_ws/src/mbot_gazebo/CMakeLists.txt

在find_package中添加

gazebo_plugins
gazebo_ros
gazebo_ros_control

5. 修改 package.xml 文件内容

gedit ~/catkin_ws/src/mbot_gazebo/package.xml

添加

<build_depend>gazebo_plugins</build_depend>
<build_depend>gazebo_ros</build_depend>
<build_depend>gazebo_ros_control</build_depend>
<build_export_depend>gazebo_plugins</build_export_depend>
<build_export_depend>gazebo_ros</build_export_depend>
<build_export_depend>gazebo_ros_control</build_export_depend>
<exec_depend>gazebo_plugins</exec_depend>
<exec_depend>gazebo_ros</exec_depend>
<exec_depend>gazebo_ros_control</exec_depend>

5. 编译

cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash

待完成

7. 进入gazebo软件

gazebo

2、点击 Edit ----> Buliding Editor 进入如下界面：

3、设置房间
4、退出当前编辑界面，File最下面那个。在Insert，插入建立的模型。

四、参考

B站【古月居】古月 · ROS入门21讲
Gazebo搭建一个三维虚拟环境 —— 通过Rviz显示机器人camera采集的信息