ROS1云课→09功能包小定制（CLI命令行接口）
【摘要】
ROS1云课→08基础实践（CLI命令行接口）
默认的turtlesim：
开始定制化：
首先，将turtlesim源码拷贝到base_tutorials/src目录下：
图标换一下：
然后修改源码turtle_frame.cpp：
QVector<QString> tu...
默认的turtlesim：
开始定制化：
首先，将turtlesim源码拷贝到base_tutorials/src目录下：
图标换一下：
然后修改源码turtle_frame.cpp：
-
QVector<QString> turtles;
-
turtles.append("rosbot.png");
-
/*turtles.append("box-turtle.png");
-
turtles.append("robot-turtle.png");
-
turtles.append("sea-turtle.png");
-
turtles.append("diamondback.png");
-
turtles.append("electric.png");
-
turtles.append("fuerte.png");
-
turtles.append("groovy.png");
-
turtles.append("hydro.svg");
-
turtles.append("indigo.svg");*/
活动背景改色，场地改大一些，ROSbotSim_2022：
-
#define DEFAULT_BG_R 0x11
-
#define DEFAULT_BG_G 0x22
-
#define DEFAULT_BG_B 0x33
-
-
namespace turtlesim
-
{
-
-
TurtleFrame::TurtleFrame(QWidget* parent, Qt::WindowFlags f)
-
: QFrame(parent, f)
-
, path_image_(777, 777, QImage::Format_ARGB32)
-
, path_painter_(&path_image_)
-
, frame_count_(0)
-
, id_counter_(0)
-
{
-
setFixedSize(777, 777);
-
setWindowTitle("ROSbotSim_2022开学大吉");
如果去查看节点列表，会看到出现了一个新的节点，叫做/turtlesim。可以通过使用rosnode info nameNode命令查看节点信息。可以看到很多能用于程序调试的信息：
$ rosnode info /turtlesim
在以上信息中，可以看到Publications（及相应主题）、Subscriptions（及相应主题）、该节点具有的Services（srv）及它们各自唯一的名称。
接下来介绍如何使用主题和服务与该节点进行交互。
通过rostopic使用pub参数，可以发布任何节点都可以订阅的主题。只需要用正确的名称将主题发布出去。将会在以后做这个测试，现在要使用一个节点，并让节点做如下工作：
$ rosrun turtlesim turtle_teleop_key
通过节点订阅的主题，可以使用箭头键移动机器人，如下图所示：
补充：rqt
比如清除：
其他内容依据具体要求进行补充。
介绍了ROS系统的架构及其工作方式的基本信息。学习了一些基本概念、工具及如何同节点、主题和服务进行交互的示例。一开始，所有这些概念可能看起来有些复杂且不太实用，但在后面的课程中，会逐渐理解这样的应用。
最好在继续后续课程的学习之前，对这些概念及示例进行练习，因为在后面的课程里，将假定已经熟悉所有的概念及其用途。
请注意如果想查询某个名词或功能的解释，且无法在课程中找到相关内容或答案，那么可以通过以下链接访问ROS官方资源http://www.ros.org。而且还可以通过访问ROS社区http://answers.ros.org提出自己的问题。
思考，如何实现高精度曲线绘制。
-
#include <boost/bind.hpp>
-
#include <ros/ros.h>
-
#include <turtlesim/Pose.h>
-
#include <geometry_msgs/Twist.h>
-
#include <std_srvs/Empty.h>
-
-
turtlesim::PoseConstPtr g_pose;
-
turtlesim::Pose g_goal;
-
-
enum State
-
{
-
FORWARD,
-
STOP_FORWARD,
-
TURN,
-
STOP_TURN,
-
};
-
-
State g_state = FORWARD;
-
State g_last_state = FORWARD;
-
bool g_first_goal_set = false;
-
-
#define PI 3.141592
-
-
void poseCallback(const turtlesim::PoseConstPtr& pose)
-
{
-
g_pose = pose;
-
}
-
-
bool hasReachedGoal()
-
{
-
return fabsf(g_pose->x - g_goal.x) < 0.001 && fabsf(g_pose->y - g_goal.y) < 0.001 && fabsf(g_pose->theta - g_goal.theta) < 0.0001;
-
}
-
-
bool hasStopped()
-
{
-
return g_pose->angular_velocity < 0.0001 && g_pose->linear_velocity < 0.0001;
-
}
-
-
void printGoal()
-
{
-
ROS_INFO("New goal [%f %f, %f]", g_goal.x, g_goal.y, g_goal.theta);
-
}
-
-
void commandTurtle(ros::Publisher twist_pub, float linear, float angular)
-
{
-
geometry_msgs::Twist twist;
-
twist.linear.x = linear;
-
twist.angular.z = angular;
-
twist_pub.publish(twist);
-
}
-
-
void stopForward(ros::Publisher twist_pub)
-
{
-
if (hasStopped())
-
{
-
ROS_INFO("Reached goal");
-
g_state = TURN;
-
g_goal.x = g_pose->x;
-
g_goal.y = g_pose->y;
-
g_goal.theta = fmod(g_pose->theta + PI/2.0, 2*PI);
-
printGoal();
-
}
-
else
-
{
-
commandTurtle(twist_pub, 0, 0);
-
}
-
}
-
-
void stopTurn(ros::Publisher twist_pub)
-
{
-
if (hasStopped())
-
{
-
ROS_INFO("Reached goal");
-
g_state = FORWARD;
-
g_goal.x = cos(g_pose->theta) * 4 + g_pose->x;
-
g_goal.y = sin(g_pose->theta) * 4 + g_pose->y;
-
g_goal.theta = g_pose->theta;
-
printGoal();
-
}
-
else
-
{
-
commandTurtle(twist_pub, 0, 0);
-
}
-
}
-
-
-
void forward(ros::Publisher twist_pub)
-
{
-
if (hasReachedGoal())
-
{
-
g_state = STOP_FORWARD;
-
commandTurtle(twist_pub, 0, 0);
-
}
-
else
-
{
-
commandTurtle(twist_pub, 1.0*(fabsf(g_pose->x - g_goal.x)+fabsf(g_pose->y - g_goal.y)), 0.0);
-
}
-
}
-
-
void turn(ros::Publisher twist_pub)
-
{
-
if (hasReachedGoal())
-
{
-
g_state = STOP_TURN;
-
commandTurtle(twist_pub, 0, 0);
-
}
-
else
-
{
-
commandTurtle(twist_pub, 0.0, 0.8*fabsf(g_pose->theta - g_goal.theta));
-
}
-
}
-
-
void timerCallback(const ros::TimerEvent&, ros::Publisher twist_pub)
-
{
-
if (!g_pose)
-
{
-
return;
-
}
-
-
if (!g_first_goal_set)
-
{
-
g_first_goal_set = true;
-
g_state = FORWARD;
-
g_goal.x = cos(g_pose->theta) * 4 + g_pose->x;
-
g_goal.y = sin(g_pose->theta) * 4 + g_pose->y;
-
g_goal.theta = g_pose->theta;
-
printGoal();
-
}
-
-
if (g_state == FORWARD)
-
{
-
forward(twist_pub);
-
}
-
else if (g_state == STOP_FORWARD)
-
{
-
stopForward(twist_pub);
-
}
-
else if (g_state == TURN)
-
{
-
turn(twist_pub);
-
}
-
else if (g_state == STOP_TURN)
-
{
-
stopTurn(twist_pub);
-
}
-
}
-
-
int main(int argc, char** argv)
-
{
-
ros::init(argc, argv, "draw_square");
-
ros::NodeHandle nh;
-
ros::Subscriber pose_sub = nh.subscribe("turtle1/pose", 1, poseCallback);
-
ros::Publisher twist_pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1);
-
ros::ServiceClient reset = nh.serviceClient<std_srvs::Empty>("reset");
-
ros::Timer timer = nh.createTimer(ros::Duration(0.016), boost::bind(timerCallback, _1, twist_pub));
-
-
std_srvs::Empty empty;
-
reset.call(empty);
-
-
ros::spin();
-
}
文章来源: zhangrelay.blog.csdn.net，作者：zhangrelay，版权归原作者所有，如需转载，请联系作者。
原文链接：zhangrelay.blog.csdn.net/article/details/126568674
【版权声明】本文为华为云社区用户转载文章，如果您发现本社区中有涉嫌抄袭的内容，欢迎发送邮件进行举报，并提供相关证据，一经查实，本社区将立刻删除涉嫌侵权内容，举报邮箱：
cloudbbs@huaweicloud.com
- 点赞
- 收藏
- 关注作者
评论（0）