#!/usr/bin/env python
import rospy
import tf
import time
from tf.transformations import *
from std_msgs.msg import String
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Quaternion
filenm="/opt/bp/tmp"
tms=
tp_x=0.0
tp_y=0.0
tp_a=0.0
i=tms
def get_pos(data):
global tp_x
global tp_y
global tp_a
global i
global tms
(roll, pitch, yaw) = euler_from_quaternion([data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w])
if(i==):
rospy.loginfo("current position(x:%f,y:%f),theta:%f", data.position.x, data.position.y, yaw)
if(tp_x==round(data.position.x,)):
if(tp_y==round(data.position.y,)):
if(tp_a==round(yaw,)):
rospy.loginfo("still!")
else:
with open(filenm, 'w+') as f:
f.write(str(data.position.x)+","+str(data.position.y)+","+str(yaw))
rospy.loginfo("write!")
else:
with open(filenm, 'w+') as f:
f.write(str(data.position.x)+","+str(data.position.y)+","+str(yaw))
rospy.loginfo("write!")
else:
with open(filenm, 'w+') as f:
f.write(str(data.position.x)+","+str(data.position.y)+","+str(yaw))
rospy.loginfo("write!")
i=tms
tp_x=round(data.position.x,)
tp_y=round(data.position.y,)
tp_a=round(yaw,)
i=i-
#rospy.loginfo("current position(x:%f,y:%f,z:%f)", data.position.x, data.position.y, data.position.z) def poslistener():
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('poslistener', anonymous=True) rospy.Subscriber("robot_pose", Pose, get_pos) # spin() simply keeps python from exiting until this node is stopped
rospy.spin() if __name__ == '__main__':
poslistener()

ros python 订阅robot_pose的更多相关文章

  1. python ros 创建节点订阅robot_pose

    建立文件夹hello_rospy,再在该目录下建立子目录src,cd到该src目录,运行如下命令创建工作包 catkin_create_pkg beginner_tutorials std_msgs ...

  2. python ros 订阅robot_pose获取机器人位置

    #!/usr/bin/env python import rospy import tf from tf.transformations import * from std_msgs.msg impo ...

  3. How to fix the conflict between ROS Python and Conda

    Problem: Ever since I have installed Conda, ROS does not seem to work. And predictably it is because ...

  4. ROS多线程订阅消息

    对于一些只订阅一个话题的简单节点来说,我们使用ros::spin()进入接收循环,每当有订阅的话题发布时,进入回调函数接收和处理消息数据.但是更多的时候,一个节点往往要接收和处理不同来源的数据,并且这 ...

  5. ros python 重置位置

    #!/usr/bin/env python import rospy import math import sys import commands import yaml from tf import ...

  6. ros python 构造 pose

    #!/usr/bin/env python import numpy as npfrom geometry_msgs.msg import Pose, Point, Quaternion, Twist ...

  7. ros Python找不到msg包的问题解决办法

    https://answers.ros.org/question/113671/catkin-package-cannot-find-own-message-type-python/ 原因是因为.py ...

  8. ros python

    https://www.ncnynl.com/archives/201611/1059.html python的节点需要对节点设置权限为可执行,在.py文件所在的路径执行如下命令 $ touch ta ...

  9. ros python 四元数 转 欧拉角

    import sysimport math w = -0.99114048481x = -0.00530699081719y = 0.00178255140781z = -0.133612662554 ...

随机推荐

  1. 浅谈Android View滑动冲突

    引言 上一篇文章我们从源码的角度介绍了View事件分发机制,这一篇文章我们就通过介绍滑动冲突的规则和一个实例来更加深入的学习View的事件分发机制. 1.外部滑动方向和内部滑动方向不一致 考虑这样一种 ...

  2. html03

    快速开发指南:1.新建页面之后,编写外部样式表文件,引入到HTML页面中2.用浏览器打开页面->F12->sources->打开css文件 右边编写样式,左边查看效果3.样式编写完成 ...

  3. Summary: Prime

    最近遇到很多问题都跟Prime有关,于是总结一下: Prime definition:A prime number (or a prime) is a natural number greater t ...

  4. 20155333 2016-2017-2 《Java程序设计》第九周学习总结

    20155333 2016-2017-2 <Java程序设计>第九周学习总结 教材学习内容总结 JDBC(Java DataBase Connectivity) 驱动的四种类型 JDBC- ...

  5. CPU VS GPU(性能调优 12.1)

    CPU VS GPU 关于绘图和动画有两种处理的方式:CPU(中央处理器)和GPU(图形处理器).在现代iOS设备中,都有可以运行不同软件的可编程芯片,但是由于历史原因,我们可以说CPU所做的工作都在 ...

  6. sql when null 判断

    Sql Server 中使用case when then 判断某字段是否为null,和判断是否为字符或数字时的写法不一样,如果不注意,很容易搞错 错误方法: CASE columnName WHEN ...

  7. clock_gettime接口和linux时间系统

    RTC/OSCRTC,Real Time Clock,真实时钟,也叫做CMOS时钟,又叫walltimeOSC,Operation System Clock,系统时钟.在开机时操作系统取得RTC中的时 ...

  8. 如何在Linux环境下通过uwgsi部署Python服务

    部署python程序时常常会遇到同一台服务器上2.x和3.x共存的情况,不同应用需要使用不用的python版本,使用virtualenv创建虚拟环境能很好地解决这一问题. 首先,需要在服务器上安装vi ...

  9. python函数—形参、实参、位置参数、关键字参数

    1.通过def function_name([parameter]): 定义,函数一遇到return即结束运行.如果函数没有定义返回值,则返回None,如果定义了一个返回值,则返回该对象,如果一个re ...

  10. JavaScript实现表单验证

    表单验证可以通过 JavaScript 来完成 以下示例代码用于判断表单字段(Name)值是否存在,如果存在,则弹出信息,否则阻止表单提交: <!DOCTYPE html> <htm ...