[C++] ROS介绍

• 管理Node节点间进行通讯的
• 每个Node节点都需要到Ros Master中进行通讯
  

# 启动ROS Master
roscore

• 具备单一功能的可执行程序
• 可以单独编译，可执行，可管理
• 存放在package中
  

• Node节点单元
• 采用了分布式网络结构
• 节点间通过RPC + TCP/UDP进行通讯
  

• 可以采用python，c++，lisp等语言进行开发。
• 遵循协议进行编码，和编程语言无关
  

• BSD许可。可修改，可复用，可商用。
• 开源使软件进步
  ​
  4.工作目录说明
  

• workspace： 工作空间
• build：ros编译打包的结果产出目录。我们不需要对这个文件夹做任何编辑操作，属于自动生成。
• devel: 开发所需要的目录
• src：存放package的目录
• CMakeLists.txt: 整个工作空间编译的脚本。此文件我们通常不用去做修改操作。
  ​
  

• pkg1： package的名称，开发过程中根据自己实际情况进行创建设定。
• CMakeLists.txt: 当前package的编译脚本。通常需要为c++代码添加编译时的依赖，执行等操作。
• package.xml: package相关信息。通常添加一些ros库的支持
• include文件夹: 存放c++ 头文件的
• config文件夹：存放参数配置文件，格式为yaml
• launch文件夹：存放.launch文件的。
• src：c++源代码
• scripts：python源代码
• srv：存放定义的service
• msg: 存放自定义的消息协议
• action： 存放自定义的action
  

mkdir -p first_ws/src

cd first_ws
catkin_make

cd first_ws/src

catkin_create_pkg demo_cpp roscpp rospy rosmsg

cd first_ws
source devel/setup.bash

cd ~/clion/bin
./clion.sh

#include <iostream>
#include "ros/ros.h"

using namespace std;

int main(int argc, char **argv) {
//初始化ros，并且设置节点名称
  ros::init(argc, argv, "cpphello");
//创建节点
  ros::NodeHandle node;

  cout << "hello ros cpp" << endl;
  return 0;
}

#!/usr/bin/env python
# coding:utf-8
import rospy

if __name__ == '__main__':
  # 创建节点
  rospy.init_node("pyhello")
  print("hello ros python")

• C++对应API
  

  ros::spin();

  
  
• Python对应API
  

  rospy.spin()

  
  

• C++对应API
  

  ros::Rate rate(10);
  while(true) {
    rate.sleep();
  }

  
  
• Python对应API
  

  rate = rospy.Rate(10)
  while True:
    rate.sleep()

  
  

• C++对应API
  

  ros::ok()

  
  
• Python对应API
  

  rospy.is_shutdown()

  
  

rosnode list

rosnode ping /节点名称

rosnode info /节点名称

rosnode kill /节点名称

rosnode cleanup

<?xml version="1.0"?>
<package format="2">
  <name>demo_py</name>
  <version>0.0.0</version>
  <description>The demo_py package</description>
  <maintainer email="itheima@todo.todo">itheima</maintainer>
  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rosmsg</build_depend>
  <build_depend>rospy</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rosmsg</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rosmsg</exec_depend>
  <exec_depend>rospy</exec_depend>

  <export>
  </export>
</package>

• name： 描述了当前package的名称
• version: 描述了当前package的版本信息
• build_depend: 编译时依赖
• build_export_depend: 编译导出依赖
• exec_depend: 执行依赖
  

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
    roscpp
    rosmsg
    rospy
    )

###########
## Build ##
###########

add_executable(exe_name src/xxx.cpp src/h1.h src/h2.h)

target_link_libraries(
    exe_name
    ${catkin_LIBRARIES}
)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
    roscpp
    rosmsg
    rospy
    )

###########
## Build ##
###########

add_executable(exe_name src/xxx.cpp src/h1.h src/h2.h)

target_link_libraries(
    exe_name
    ${catkin_LIBRARIES}
)

#include "ros/ros.h"
#include <iostream>
#include "std_msgs/String.h"

using namespace std;

int main(int argc, char **argv) {

  string nodeName = "cpppublisher";
  string topicName = "cpptopic";

  //初始化ros节点
  ros::init(argc, argv, nodeName);
  ros::NodeHandle node;

  //通过节点创建publisher
  const ros::Publisher &publisher = node.advertise<std_msgs::String>(topicName, 1000);

  //按照一定周期，发布消息
  int index = 0;
  ros::Rate rate(1);
  while (ros::ok()) {
    std_msgs::String msg;

    msg.data = "hello pub " + to_string(index);
    publisher.publish(msg);
    rate.sleep();
    index++;
  }
  return 0;
}

rostopic list

rostopic echo cpptopic

rosrun rqt_topic rqt_topic

#include "ros/ros.h"
#include <iostream>
#include "std_msgs/String.h"

using namespace std;

void topicCallback(const std_msgs::String::ConstPtr &msg) {
  cout << (*msg).data << endl;
}

int main(int argc, char **argv) {
  string nodeName = "cppsubscriber";
  string topicName = "cpptopic";

  //初始化节点
  ros::init(argc, argv, nodeName);
  ros::NodeHandle node;
  //创建订阅者
  const ros::Subscriber &subscriber = node.subscribe(topicName, 1000, topicCallback);
// 阻塞线程
  ros::spin();
  return 0;
}

rosrun demo_topic cpppublisher

rostopic type cpptopic

rostopic pub cpptopic std_msgs/String hello -r 10

rosrun rqt_publisher rqt_publisher

rosrun rqt_graph rqt_graph

#!/usr/bin/env python
# coding:utf-8

import rospy
from std_msgs.msg import String

if __name__ == '__main__':
  nodeName = "pypublisher"
  topicName = "pytopic"
  # 初始化节点
  rospy.init_node(nodeName)

  # 创建发布者
  publisher = rospy.Publisher(topicName, String, queue_size=1000)

  rate = rospy.Rate(10)
  count = 0
  while not rospy.is_shutdown():
    # 发布消息
    publisher.publish("hello %d" % count)
    rate.sleep()
    count += 1;

rostopic list

rostopic echo pytopic

rosrun rqt_topic rqt_topic

rosmsg list

rosmsg show std_msgs/String

rosmsg show geometry_msgs/Twist

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

string name
int64 age

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rosmsg
  rospy
  message_generation
)

add_message_files(
    FILES
    Student.msg
)

generate_messages(
    DEPENDENCIES
    std_msgs
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES demo_msg
#  CATKIN_DEPENDS roscpp rosmsg rospy
#  DEPENDS system_lib
    CATKIN_DEPENDS message_runtime
)

catkin_make

rosmsg show demo_msgs/Student

<build_depend>demo_msgs</build_depend>
<build_export_depend>demo_msgs</build_export_depend>
<exec_depend>demo_msgs</exec_depend>

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rosmsg
  rospy
  demo_msgs
)

#include "demo_msgs/Student"

demo_msgs::Student stu;
stu.name = "itheima";
stu.age = 13;

from demo_msgs.msg import Student

stu = Student()
stu.name = "itheima"
stu.age = 13

string name
Student leader

add_message_files(
    FILES
    Student.msg
    Team.msg
)

string name
Student leader
std_msgs/String intro

string name
Student leader
std_msgs/String intro
geometry_msgs/Twist location

find_package(catkin REQUIRED COMPONENTS
    roscpp
    rosmsg
    rospy
    message_generation
    geometry_msgs
    )

generate_messages(
    DEPENDENCIES
    std_msgs
    geometry_msgs
)

<build_depend>geometry_msgs</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rosmsg
  rospy
  message_generation
  geometry_msgs
)

generate_messages(
    DEPENDENCIES
    std_msgs
    geometry_msgs
)

string name
Student leader
std_msgs/String intro
geometry_msgs/Twist location
Student[] members

rosrun turtlesim turtlesim_node

rosrun turtlesim turtle_teleop_key

rosnode list

rosnode info /turtlesim

--------------------------------------------------------------------------------
Node [/turtlesim]
Publications: 
 * /rosout [rosgraph_msgs/Log]
 * /turtle1/color_sensor [turtlesim/Color]
 * /turtle1/pose [turtlesim/Pose]

Subscriptions: 
 * /turtle1/cmd_vel [geometry_msgs/Twist]

Services: 
 * /clear
 * /kill
 * /reset
 * /spawn
 * /turtle1/set_pen
 * /turtle1/teleport_absolute
 * /turtle1/teleport_relative
 * /turtlesim/get_loggers
 * /turtlesim/set_logger_level


contacting node http://ubuntu:42049/ ...
Pid: 20218
Connections:
 * topic: /rosout
  * to: /rosout
  * direction: outbound
  * transport: TCPROS
 * topic: /turtle1/cmd_vel
  * to: /teleop_turtle (http://ubuntu:44433/)
  * direction: inbound
  * transport: TCPROS

• Publications：此节点上定义的发布者
• Subscriptions：此节点上定义的订阅者
• Services：此节点上定义的服务
• 进程id，占用的网络端口
• Connections： 此节点和其他节点间的连接信息
  

--------------------------------------------------------------------------------
Node [/teleop_turtle]
Publications: 
 * /rosout [rosgraph_msgs/Log]
 * /turtle1/cmd_vel [geometry_msgs/Twist]

Subscriptions: None

Services: 
 * /teleop_turtle/get_loggers
 * /teleop_turtle/set_logger_level


contacting node http://ubuntu:44433/ ...
Pid: 20443
Connections:
 * topic: /rosout
  * to: /rosout
  * direction: outbound
  * transport: TCPROS
 * topic: /turtle1/cmd_vel
  * to: /turtlesim
  * direction: outbound
  * transport: TCPROS

• /teleop_turtle节点存在一个发布者，往/turtle1/cmd_vel主题中发布数据。
• /turtlesim节点存在一个订阅者，去/turtle1/cmd_vel主题中获取数据。
  

rosrun rqt_graph rqt_graph

rosrun rqt_publisher rqt_publisher

rostopic pub /turtle1/cmd_vel geometry_msgs/Twist "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 3.0"

• 面板的坐标原点在左下脚，即左下角为(0,0)
• 面板的X轴是自左向右，数值是0开始正向增长
• 面板的Y轴是自下向上，数值是0开始正向增长
• 面板的宽度和高度相同，值为11.088899
  

• 小乌龟的坐标原点为小乌龟的中心点。
  

rostopic type /turtle1/cmd_vel

rosmsg show geometry_msgs/Twist

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

rosservice call/reset

• 创建QT UI
• 创建 Publisher
• 整合QT UI 和 Publisher进行数据发布
  

add_compile_options(-std=c++11)

##############################################################################
# Qt Environment
##############################################################################

set(CMAKE_INCLUDE_CURRENT_DIR ON)
set(CMAKE_AUTOMOC ON)

find_package(Qt5 COMPONENTS Core Gui Widgets PrintSupport)
##############################################################################

#include "ros/ros.h"
#include <iostream>
using namespace std;

int main(int argc, char *argv[]) {
  string nodeName = "qt_turtle_ctrl";
// 创建节点
  ros::init(argc,argv,nodeName);
  ros::NodeHandle node;
  return 0;
}

add_executable(turtle_control src/turtle_control.cpp)
target_link_libraries(turtle_control
    ${catkin_LIBRARIES}
    Qt5::Core
    Qt5::Gui
    Qt5::Widgets
    Qt5::PrintSupport
 )

#include <QtWidgets>
#include <QApplication>

// 创建Qt Application
QApplication app(argc, argv);
// 创建窗体
QWidget window;
// 设置窗体为当前激活窗体
app.setActiveWindow(&window);
//显示窗体
window.show();
// Qt程序执行
app.exec();

// 设置布局
QFormLayout layout;
window.setLayout(&layout);
// 距离输入框
QLineEdit editLinear;
layout.addRow("距离", &editLinear);
// 角度输入框
QLineEdit editDegrees;
layout.addRow("角度", &editDegrees);
// 发送按钮
QPushButton btnSend("发送");
layout.addRow(&btnSend);

btnSend.connect(&btnSend, &QPushButton::clicked, &window, btnClicked);

void btnClicked() {
  std::cout << "clicked" << std::endl;
}

const ros::Publisher &publisher = node.advertise(topicName, 1000);

rostopic type /turtle1/cmd_vel

#include "geometry_msgs/Twist.h"

const ros::Publisher &publisher = node.advertise<geometry_msgs::Twist>(topicName, 1000);

//创建消息
geometry_msgs::Twist twist;
//填充消息数据
twist.linear.x = linearX;
twist.angular.z = angularZ * M_PI / 180;
//发送消息
publisherPtr->publish(twist);

#include "ros/ros.h"
#include <iostream>
#include "std_msgs/String.h"
#include "QtWidgets"
#include "QApplication"
#include "geometry_msgs/Twist.h"
#include "math.h"

using namespace std;


void sendclicked(ros::Publisher &publisher, QLineEdit &lineredit, QLineEdit &angularedit) {

  double x = lineredit.text().toDouble();
  double z = angularedit.text().toDouble();
  geometry_msgs::Twist twist;
  twist.linear.x = x;
  twist.angular.z = z *M_PI / 180;
  publisher.publish(twist);
//  cout << "clicked" << endl;
}

int main(int argc, char **argv) {

  string nodeName = "turtle_ctrl";
  string topicName = "turtle1/cmd_vel";

  //初始化ros节点
  ros::init(argc, argv, nodeName);
  ros::NodeHandle node;

  //创建publisher
  ros::Publisher publisher = node.advertise<geometry_msgs::Twist>(topicName, 1000);

  //创建Qt，application
  QApplication app(argc, argv);

  //创建窗体
  QWidget window;
  //设置窗体大小及标题
  window.resize(600, 150);
  window.setWindowTitle("小乌龟控制器");
  //表单布局
  QFormLayout layout;
  window.setLayout(&layout);

  //线速度输入框
  QLineEdit lineredit("0.0");
  layout.addRow("线速度", &lineredit);
  //角速度输入框
  QLineEdit angularedit("0.0");
  layout.addRow("线速度", &angularedit);

  //button
  QPushButton btn("发送");

  layout.addRow(&btn);

  //点击事件
  btn.connect(&btn, &QPushButton::clicked, &window, [&publisher, &lineredit, &angularedit]() {
    sendclicked(publisher, lineredit, angularedit);
  });
  //展示窗体
  window.show();

  //程序退出
  return app.exec();
}

#!/usr/bin/env python
#coding:utf-8

chmod +x turtle_control.py

import rospy

if __name__ == '__main__':
  nodeName = "qt_turle_ctrl";
  # 创建ros node
  rospy.init_node(nodeName, anonymous=True)

from PyQt5.QtWidgets import *

# 创建Qt程序
app = QApplication(sys.argv) 
# 创建窗体
window = QWidget()
window.setWindowTitle("小乌龟控制")
window.resize(400, 0)
# 显示窗体
window.show();
# Qt程序执行
sys.exit(app.exec_())

# 设在布局
layout = QFormLayout()
window.setLayout(layout)

# 添加组件
editLinear = QLineEdit()
editLinear.setText("0.0")
layout.addRow("距离", editLinear)

editAngular = QLineEdit()
editAngular.setText("0.0")
layout.addRow("角度", editAngular)

btnSend = QPushButton("发送")
layout.addRow(btnSend)

btnSend.clicked.connect(btnClicked)

define btnClicked():
  print "clicked"

rostopic type /turtle1/cmd_vel

from geometry_msgs.msg import Twist

publisher = rospy.Publisher(topicName, Twist, queue_size=1000)

# 创建消息
twist = Twist()
# 填充数据
twist.linear.x = linearX
twist.angular.z = angluarZ * math.pi / 180
# 发送消息
publisher.publish(twist)

#!/usr/bin/env python
# coding:utf-8

import rospy
from geometry_msgs.msg import Twist

from PyQt5.QtWidgets import *
import sys
import math


def btnClicked(editLinear, editAngluar, publisher):
  linearX = float(editLinear.text())
  angluarZ = float(editAngular.text())

  # 创建消息
  twist = Twist()
  # 填充数据
  twist.linear.x = linearX
  twist.angular.z = angluarZ * math.pi / 180
  # 发送消息
  publisher.publish(twist)


if __name__ == '__main__':
  nodeName = "qt_turle_ctrl";
  topicName = "/turtle1/cmd_vel"

  # 创建ros node
  rospy.init_node(nodeName, anonymous=True)

  # 创建publisher
  publisher = rospy.Publisher(topicName, Twist, queue_size=1000)

  # 创建Qt程序
  app = QApplication(sys.argv)
  # 创建窗体
  window = QWidget()
  window.setWindowTitle("小乌龟控制")
  window.resize(400, 0)

  # 设在布局
  layout = QFormLayout()
  window.setLayout(layout)

  # 添加组件
  editLinear = QLineEdit()
  editLinear.setText("0.0")
  layout.addRow("距离", editLinear)

  editAngular = QLineEdit()
  editAngular.setText("0.0")
  layout.addRow("角度", editAngular)

  btnSend = QPushButton("发送")
  layout.addRow(btnSend)

  # 事件
  btnSend.clicked.connect(lambda: btnClicked(editLinear, editAngular, publisher))

  window.show()

  sys.exit(app.exec_())

• 创建QT UI
• 创建 Publisher
• 整合QT UI 和 Subscriber进行数据订阅
  

#include "ros/ros.h"
#include <iostream>
#include "std_msgs/String.h"
#include "QtWidgets"
#include "QApplication"
#include "geometry_msgs/Twist.h"
#include "math.h"
#include "turtlesim/Pose.h"

using namespace std;

QLabel *lbX;
QLabel *lbY;
QLabel *linerV;
QLabel *angularV;
QLabel *theta;

void receiveCall(const turtlesim::Pose::ConstPtr &message) {
  lbX->setText(QString::fromStdString(to_string(message->x)));
  lbY->setText(QString::fromStdString(to_string(message->y)));
  linerV->setText(QString::fromStdString(to_string(message->linear_velocity)));
  angularV->setText(QString::fromStdString(to_string(message->angular_velocity)));
  theta->setText(QString::fromStdString(to_string(message->theta)));

}


void sendclicked(ros::Publisher &publisher, QLineEdit &lineredit, QLineEdit &angularedit) {

  double x = lineredit.text().toDouble();
  double z = angularedit.text().toDouble();
  geometry_msgs::Twist twist;
  twist.linear.x = x;
  twist.angular.z = z * M_PI / 180;
  publisher.publish(twist);
//  cout << "clicked" << endl;
}

int main(int argc, char **argv) {

  string nodeName = "turtle_ctrl";
  //publisher的广播地址
  string topicName = "turtle1/cmd_vel";
  //subcriber的接收地址
  string receiveName = "/turtle1/pose";

  //初始化ros节点
  ros::init(argc, argv, nodeName);
  ros::NodeHandle node;

  //开启异步轮循器
  ros::AsyncSpinner spinner(2);
  spinner.start();

  //创建publisher
  ros::Publisher publisher = node.advertise<geometry_msgs::Twist>(topicName, 1000);

  //创建subcriber
  const ros::Subscriber &subscriber = node.subscribe<turtlesim::Pose>(receiveName, 100, receiveCall);

  //创建Qt，application
  QApplication app(argc, argv);

  //创建窗体
  QWidget window;
  //设置窗体大小及标题
  window.resize(600, 150);
  window.setWindowTitle("小乌龟控制器");
  //表单布局
  QFormLayout layout;
  window.setLayout(&layout);

  //线速度输入框
  QLineEdit lineredit("0.0");
  layout.addRow("线速度", &lineredit);
  //角速度输入框
  QLineEdit angularedit("0.0");
  layout.addRow("角速度", &angularedit);

  //当前X坐标
  lbX = new QLabel();
  layout.addRow("当前X坐标", lbX);
  //当前Y坐标
  lbY = new QLabel();
  layout.addRow("当前Y坐标", lbY);
  //当前线速度
  linerV = new QLabel();
  layout.addRow("当前线速度", linerV);
  //当前角速度
  angularV = new QLabel();
  layout.addRow("当前角速度", angularV);
  //当前角度
  theta = new QLabel();
  layout.addRow("当前角速度", theta);

  //button
  QPushButton btn("发送");

  layout.addRow(&btn);

  //点击事件
  btn.connect(&btn, &QPushButton::clicked, &window, [&publisher, &lineredit, &angularedit]() {
    sendclicked(publisher, lineredit, angularedit);
  });
  //展示窗体
  window.show();

//  ros::spin();
  //程序退出
  return app.exec();
}

• 假如将当前程序日志等级设置为DEBUG，可查看到的日志信息包含:DEBUG , INFO , WARN , ERROR , FATAL
• 假如将当前程序日志等级设置为INFO，可查看到的日志信息包含: INFO , WARN , ERROR , FATAL
• 假如将当前程序日志等级设置为WARN，可查看到的日志信息包含: WARN , ERROR , FATAL
• 假如将当前程序日志等级设置为ERROR，可查看到的日志信息包含: ERROR , FATAL
• 假如将当前程序日志等级设置为FATAL，可查看到的日志信息包含: FATAL
  

ROS_DEBUG("打印的内容");
ROS_INFO("打印的内容");
ROS_WARN("打印的内容");
ROS_ERROR("打印的内容");
ROS_FATAL("打印的内容");

ROS_DEBUG_STREAM("打印的内容" << "hello");
ROS_INFO_STREAM("打印的内容" << "hello");
ROS_WARN_STREAM("打印的内容" << "hello");
ROS_ERROR_STREAM("打印的内容" << "hello");
ROS_FATAL_STREAM("打印的内容" << "hello");

rospy.logdebug("打印的内容")
rospy.loginfo("打印的内容")
rospy.logwarn("打印的内容")
rospy.logerror("打印的内容")
rospy.logfatal("打印的内容")

rosrun rqt_console rqt_console

• client端负责发送请求(Request)给server端。
• server端负责接收client端发送的请求数据。
• server端收到数据后，根据请求数据和当前的业务需求，产生数据，将数据(Response)返回给client端。
  

• 同步数据访问
• 具有响应反馈机制
• 一个server多个client
• 注重业务逻辑处理
  

• service的地址名称
• client端访问server端的数据格式
• server端响应client端的数据格式
  

#include "ros/ros.h"
#include <iostream>
#include "roscpp_tutorials/TwoInts.h"

using namespace std;

bool callback(roscpp_tutorials::TwoIntsRequest &request, roscpp_tutorials::TwoIntsResponse &response) {
  // 运算逻辑
  response.sum = request.a + request.b;
  return true;
}

int main(int argc, char **argv) {
  string nodeName = "cppservice";
  string serviceName = "/demo_service/add_tow_int";

  // 创建节点
  ros::init(argc, argv, nodeName);
  ros::NodeHandle node;

  // 创建服务端
  const ros::ServiceServer &server = node.advertiseService(serviceName, callback);

  //阻塞
  ros::spin();
  return 0;
}

rosservice list

rosservice call /demo_service/add_tow_int "a:1 b:3"

rosrun rqt_service_caller rqt_service_caller

#include "ros/ros.h"
#include <iostream>

#include "roscpp_tutorials/TwoInts.h"

using namespace std;

int main(int argc, char **argv) {
  string nodeName = "service_client";
  string serviceName = "/demo_service/add_tow_int";

  //创建节点
  ros::init(argc, argv, nodeName);
  ros::NodeHandle node;

  //创建client
  ros::ServiceClient &&client = node.serviceClient<roscpp_tutorials::TwoInts>(serviceName);

  //创建service
  roscpp_tutorials::TwoInts service;
  service.request.a = 10;
  service.request.b = 5;
  //调用service
  client.call(service);
//打印结果
  ROS_INFO_STREAM(service.response.sum);

  return 0;
}

rosrun demo_service client

#!/usr/bin/env python
# coding:utf-8

import rospy
from roscpp_tutorials.srv import TwoInts


def handler(request):
  rospy.loginfo("python call")
  return request.a + request.b


if __name__ == '__main__':
  nodeName = "pyservice";
  serviceName = "/demo_service/add_two_int"

  # 创建节点
  rospy.init_node(nodeName)
  # 创建service
  rospy.Service(serviceName, TwoInts, handler)
  # 阻塞线程
  rospy.spin()

rosservice list

rosservice call /demo_service/add_tow_int "a:1 b:3"

rosrun rqt_service_caller rqt_service_caller

#!/usr/bin/env python
# encoding:utf-8

import rospy

from roscpp_tutorials.srv import TwoInts

if __name__ == '__main__':
  nodeName = "pyclient"
  serviceName = "/demo_service/add_two_int"

  # 创建节点
  rospy.init_node(nodeName)

  # 等待服务器连接
  rospy.wait_for_service(serviceName)
  # 创建服务调用代理
  call = rospy.ServiceProxy(serviceName, TwoInts)
  # 调用服务
  result = call(4, 9)

  # 打印服务调用结果
  rospy.loginfo(result)

rosrun demo_service client.py

ro***v list

ro***v show roscpp_tutorials/TwoInts

int64 a
int64 b
---
int64 sum

float64 a
float64 b
string option
---
float64 result

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rosmsg
  rospy
  message_generation
)

add_service_files(
    FILES
    NumOption.srv
)

generate_messages(
    DEPENDENCIES
    std_msgs
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES demo_msg
#  CATKIN_DEPENDS roscpp rosmsg rospy
#  DEPENDS system_lib
    CATKIN_DEPENDS message_runtime
)

catkin_make

ro***v show demo_srvs/NumOption

<build_depend>demo_srvs</build_depend>
<build_export_depend>demo_srvs</build_export_depend>
<exec_depend>demo_srvs</exec_depend>

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rosmsg
  rospy
  demo_srvs
)

#include "demo_srvs/NumOption.h"

demo_srvs::NumOption service;
service.request.a = 10;
service.request.b = 5;
service.request.option = "+";

from demo_srvs.srv import NumOption

Student student
---
string id

<build_depend>demo_msgs</build_depend>
<build_export_depend>demo_msgs</build_export_depend>
<exec_depend>demo_msgs</exec_depend>

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rosmsg
  rospy
  message_generation
  demo_msgs
)

generate_messages(
    DEPENDENCIES
    std_msgs
    demo_msgs
)

add_service_files(
    FILES
    NumOption.srv
    GenStudentId.srv
)

#include <iostream>
#include "ros/ros.h"

#include "QApplication"
#include "QtWidgets"

using namespace std;

void clickDone(QLineEdit *xEdit, QLineEdit *yEdit) {
  double distX = xEdit->text().toDouble();
  double distY = yEdit->text().toDouble();

  ROS_INFO_STREAM("distX: " << distX << "  distY:" << distY);
}

int main(int argc, char **argv) {
  string nodeName = "turtle_control";

  // 创建节点
  ros::init(argc, argv, nodeName, ros::init_options::AnonymousName);
  ros::NodeHandle node;

  // 创建Qt程序
  QApplication app(argc, argv);
  //窗体
  QWidget window;
  window.setWindowTitle("小乌龟控制器");
  window.resize(400, 0);
  //布局
  QFormLayout layout;
  window.setLayout(&layout);

  // 目标x坐标
  QLineEdit xEdit("10.54444");
  layout.addRow("x坐标", &xEdit);

  // 目标y坐标
  QLineEdit yEdit("5.54444");
  layout.addRow("y坐标", &yEdit);
  
  //当前姿态坐标信息
  QHBoxLayout poseLayout;
  layout.addRow(&poseLayout);

  QFormLayout xLayout;
  QLabel xLb;
  xLayout.addRow("x坐标", &xLb);
  poseLayout.addLayout(&xLayout);

  QFormLayout yLayout;
  QLabel yLb;
  yLayout.addRow("y坐标", &yLb);
  poseLayout.addLayout(&yLayout);

  QFormLayout thetaLayout;
  QLabel thetaLb;
  thetaLayout.addRow("角度", &thetaLb);
  poseLayout.addLayout(&thetaLayout);

  // 执行按钮
  QPushButton btnDone("执行");
  layout.addRow(&btnDone);

  // 事件设置
  btnDone.connect(&btnDone, &QPushButton::clicked, bind(&clickDone, &xEdit, &yEdit));

  window.show();

  return app.exec();
}

//小乌龟控制地址
string velTopicName = "/turtle1/cmd_vel";
//小乌龟数据获得
string poseTopicName = "/turtle1/pose";
// 创建小乌龟移动发布者
ros::Publisher &&publisher = node.advertise<geometry_msgs::Twist>(velTopicName, 1000);
// 创建小乌龟位置的订阅者
const ros::Subscriber &subscriber = node.subscribe(poseTopicName, 1000, poseCallback);

void poseCallback(const turtlesim::Pose::ConstPtr &msg) {
  ROS_INFO_STREAM("x: " << msg->x);
  ROS_INFO_STREAM("y: " << msg->y);
  ROS_INFO_STREAM("theta: " << msg->theta);
  ROS_INFO_STREAM("linear: " << msg->linear_velocity);
  ROS_INFO_STREAM("angular: " << msg->angular_velocity);
  ROS_INFO_STREAM("degrees: " << msg->theta * 180 / M_PI);
  ROS_INFO_STREAM("=========================================");
}

距离 = 速度 * 时间
速度 = 距离 / 时间

distance = sqrt(pow(srcX - distX) + pow(srcY - distY))

ros::Rate rate(1);
for (int i = 0; i < 5; ++i) {
  //设置速度
  geometry_msgs::Twist twist;
  twist.linear.x = linearVel;
  twist.angular.z = 0;
  publisher.publish(twist);

  rate.sleep();
 }

double runDistance = 0;
while (runDistance < distance) {

  //设置速度
  geometry_msgs::Twist twist;
  twist.linear.x = linearVel;
  twist.angular.z = 0;
  publisher.publish(twist);

  rate.sleep();

  runDistance += linearVel / hz;
}

while (calcDistance(srcX, srcY, distX, distY) > 0.1) {
    // 获取srcX，srcY
    srcX = pose->x;
    srcY = pose->y;

    //设置速度
    geometry_msgs::Twist twist;
    twist.linear.x = linearVel;
    twist.angular.z = 0;
    publisher.publish(twist);
    rate.sleep();
}

• 速度是否是绝对平均
• 距离差值是否是绝对为0
• 时间是否绝对为预期
  

• 位式控制，只考虑当前。
• pid不仅考虑当前，还要考虑历史
• pid输出更多样，更平滑。不是是true或者false，
  

rosrun rqt_plot rqt_plot

//计算当前距离目标点的距离
double linearDistance = calcDistance(srcX, srcY, distX, distY);
//计算剩下了的平均速度
double linearTargetVel = linearDistance / time;

//计算速度误差
double linearError = linearTargetVel - linearVel;

linearTotalError += linearError;

double linearDeltaError = linearError - linearLastError;
linearLastError = linearError;