ROS开发小乌龟自动爬行与实时位姿获取

本文从下面博客改编而来

ROS编程入门教程(全过程示例代码)_lhb0709的专栏-CSDN博客

首先创建工作空间(working space)

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace

创建功能包pkg ,

catkin_create_pkg beginner_tutorials std_msgs rospy roscpp

创建源代码目录和源码cpp

mkdir -p ~/catkin_ws/src/beginner_tutorials/src

 talker.cpp 

#include "ros/ros.h"
#include "std_msgs/String.h"

#include <sstream>
#include <string>   
#include "geometry_msgs/Twist.h" 

/**
 * This tutorial demonstrates simple sending of messages over the ROS system.
 */
int main(int argc, char **argv)
{
  /**
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line. For programmatic
   * remappings you can use a different version of init() which takes remappings
   * directly, but for most command-line programs, passing argc and argv is the easiest
   * way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  ros::init(argc, argv, "talker");

  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;

  /**
   * The advertise() function is how you tell ROS that you want to
   * publish on a given topic name. This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing. After this advertise() call is made, the master
   * node will notify anyone who is trying to subscribe to this topic name,
   * and they will in turn negotiate a peer-to-peer connection with this
   * node.  advertise() returns a Publisher object which allows you to
   * publish messages on that topic through a call to publish().  Once
   * all copies of the returned Publisher object are destroyed, the topic
   * will be automatically unadvertised.
   *
   * The second parameter to advertise() is the size of the message queue
   * used for publishing messages.  If messages are published more quickly
   * than we can send them, the number here specifies how many messages to
   * buffer up before throwing some away.
   */
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
  ros::Publisher turtle1_pub=n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10); //topic


  ros::Rate loop_rate(10);

  /**
   * A count of how many messages we have sent. This is used to create
   * a unique string for each message.
   */
  int count = 1;
  while (ros::ok())
  {
    /**
     * This is a message object. You stuff it with data, and then publish it.
     */
    std_msgs::String msg;

    std::stringstream ss;
    ss << "hello world from talker" << count;
    
    msg.data = ss.str();

    ROS_INFO("%s", msg.data.c_str());

    /**
     * The publish() function is how you send messages. The parameter
     * is the message object. The type of this object must agree with the type
     * given as a template parameter to the advertise<>() call, as was done
     * in the constructor above.
     */
    chatter_pub.publish(msg);
    geometry_msgs::Twist turtle_msg;

    turtle_msg.linear.x = 1.0;  //前后(+ -) m/s
    turtle_msg.linear.y = 0.0;  //左右(+-) m/s
    turtle_msg.linear.z = 0.0;
 
    turtle_msg.angular.x = 0;
    turtle_msg.angular.y = 0;
    turtle_msg.angular.z = 0.99; //机器人的自转速度,+左转,-右转,单位是rad/s
    turtle1_pub.publish(turtle_msg);
    ROS_INFO("publish turtle run command[%0.2f m/s,%0.2f rad/s]", turtle_msg.linear.x, turtle_msg.angular.z);


    ros::spinOnce();

    loop_rate.sleep();
    ++count;
  }


  return 0;
}

 listener.cpp 

#include "ros/ros.h"
#include "std_msgs/String.h"

#include "turtlesim/Pose.h"
/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

void turtleCallback(const turtlesim::Pose::ConstPtr&  msg)
{
  ROS_INFO("turtle pose :x=%.2f,y=%.2f,linear=%.2f,angular=%.2f,theta=%.2f",
        msg->x,msg->y,msg->linear_velocity,msg->angular_velocity,msg->theta);
}

int main(int argc, char **argv)
{
  /**
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line. For programmatic
   * remappings you can use a different version of init() which takes remappings
   * directly, but for most command-line programs, passing argc and argv is the easiest
   * way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  ros::init(argc, argv, "listener");

  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;

  /**
   * The subscribe() call is how you tell ROS that you want to receive messages
   * on a given topic.  This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing.  Messages are passed to a callback function, here
   * called chatterCallback.  subscribe() returns a Subscriber object that you
   * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
   * object go out of scope, this callback will automatically be unsubscribed from
   * this topic.
   *
   * The second parameter to the subscribe() function is the size of the message
   * queue.  If messages are arriving faster than they are being processed, this
   * is the number of messages that will be buffered up before beginning to throw
   * away the oldest ones.
   */
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
  ros::Subscriber turtle_sub = n.subscribe<turtlesim::Pose>("/turtle1/pose", 100, turtleCallback);


  /**
   * ros::spin() will enter a loop, pumping callbacks.  With this version, all
   * callbacks will be called from within this thread (the main one).  ros::spin()
   * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
   */

  ros::MultiThreadedSpinner spinner(4); // Use 4 threads
  ros::spin();

  return 0;
}

程序包(~/catkin_ws/src/beginner_tutorials)的 CMakeLists.txt文件,修改如下(最后两行,要不要都行)

 返回工作空间编译

catkin_make 

启动小乌龟

roscore
rosrun turtlesim turtlesim_node

同样在工作空间分别开启两个控制台分别启动talker 和 listener

source ./devel/setup.bash

rosrun beginner_tutorials talker 

source ./devel/setup.bash

rosrun beginner_tutorials listener

测试效果如下图,

 

实现原理:

启动手动控制小乌龟游戏 ,方法见

ROS 安装教程Ubuntu16.04(2022年最新)_lhb0709的专栏-CSDN博客

teleop_turtle    是虚拟机键盘节点

turtlesim    是小乌龟节点

/turtle1/cmd_vel 是topic

依次使用命令查看turtle1/cmd_vel的类型和数据结构(代码中要用)

rostopic list
rostopic info /turtle1/cmd_vel
rosmsg info geometry_msgs/Twist

 依次使用命令查看turtle1/pose的类型和数据结构(代码中要用)

rostopic list
rostopic info /turtle1/pose
rosmsg info turtlesim/Pose

 

 Python 实现的位姿显示代码参考,同时会python和QT,几分钟就看懂了

ROS实操入门系列(二)操控小乌龟时,监控行走的路线(第二章补充Topic干货!) - 古月居 (guyuehome.com)

  • 0
    点赞
  • 17
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
好的,以下是实现小乌龟做五边形运动并输出位姿ROS Python 代码: ```python #!/usr/bin/env python import rospy from geometry_msgs.msg import Twist from turtlesim.msg import Pose class TurtleController(): def __init__(self): # 初始化ROS节点 rospy.init_node('turtle_controller', anonymous=True) # 创建小乌龟速度发布者 self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) # 创建小乌龟姿订阅者 self.pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose, self.pose_callback) # 初始化小乌龟位置和朝向角度 self.x = 0.0 self.y = 0.0 self.theta = 0.0 # 休眠1秒等待ROS节点初始化完成 rospy.sleep(1) def pose_callback(self, pose): # 更新小乌龟位置和朝向角度 self.x = pose.x self.y = pose.y self.theta = pose.theta def move(self): # 创建Twist消息并设置线速度和角速度 vel_msg = Twist() vel_msg.linear.x = 1.0 vel_msg.angular.z = 72.0 / 180.0 * 3.1416 # 计算五边形需要移动的时间 time = 100.0 / vel_msg.linear.x # 获取当前时间 start_time = rospy.Time.now().to_sec() # 循环移动小乌龟 while rospy.Time.now().to_sec() - start_time < time: self.velocity_publisher.publish(vel_msg) # 停止小乌龟运动 vel_msg.linear.x = 0.0 vel_msg.angular.z = 0.0 self.velocity_publisher.publish(vel_msg) # 输出小乌龟的位置和朝向角度 rospy.loginfo("位姿:x={}, y={}, theta={}".format(self.x, self.y, self.theta)) if __name__ == '__main__': try: # 创建TurtleController对象并移动小乌龟 turtle_controller = TurtleController() turtle_controller.move() except rospy.ROSInterruptException: pass ``` 运行代码后,小乌龟会在ROS环境下画出一个五边形,并输出小乌龟的位置和朝向角度。注意需要在另一个终端中运行`roscore`命令开启ROS环境。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值