#!/usr/bin/env python
import rospy
import math
import sys
import commands
import yaml
from tf import transformations
from geometry_msgs.msg import PoseWithCovarianceStamped
class PoseSetter(rospy.SubscribeListener):
def __init__(self, pose):
self.pose = pose
def peer_subscribe(self, topic_name, topic_publish, peer_publish):
p = PoseWithCovarianceStamped()
p.header.frame_id = "map"
p.pose.pose.position.x = self.pose[]
p.pose.pose.position.y = self.pose[]
(p.pose.pose.orientation.x,
p.pose.pose.orientation.y,
p.pose.pose.orientation.z,
p.pose.pose.orientation.w) = transformations.quaternion_from_euler(, , self.pose[])
p.pose.covariance[*+] = 0.5 * 0.5
p.pose.covariance[*+] = 0.5 * 0.5
p.pose.covariance[*+] = math.pi/12.0 * math.pi/12.0
peer_publish(p)
rospy.signal_shutdown("closed!")
filenm="/opt/bp/tmp"
sh="rostopic list|grep initialpose"
globalmap="/opt/bp/maps/global_map.yaml"
x=True
with open(filenm, 'r') as f:
content=f.read()
if(content==""):
with open(globalmap) as k:
cmapyaml = yaml.load(k)
with open("/opt/bp/maps/"+str(cmapyaml["hotelid"])+"/"+str(cmapyaml["floor"])+"/map.yaml") as j:
mapyaml = yaml.load(j)
pose=mapyaml["origin"]
else:
pose =map(float,content.split(","))
while():
if(commands.getstatusoutput(sh)[]==):
break
rospy.init_node('pose_setter', anonymous=True)
if(x):
pub=rospy.Publisher("initialpose", PoseWithCovarianceStamped,PoseSetter(pose),queue_size=)
x=False
else:
print("x=False")
rospy.spin()

ros python 重置位置的更多相关文章

  1. 如何查看Python 安装位置以及已经安装的库

    如何查看Python 安装位置以及已经安装的库 步骤一: 1. Start 一个command prompt 2. 找到电脑中已经安装的Python 位置: where python 1 打开路径, ...

  2. 查看python安装位置和已安装库的相关操作

    打开cmd.exe, *查看python安装位置 where python *查看已安装库 pip list 或者pip freeze *查看可以更新的第三方库 pip list --outdated ...

  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 python 订阅robot_pose

    #!/usr/bin/env python import rospy import tf import time from tf.transformations import * from std_m ...

  5. python代码位置引发的错误

    觉得python对代码位置的要求简直是变态,缩进引发的错误我以前在博客里讲过了,如果不懂可以看看我以前的博客,今天又遇到了一个代码位置所引发的错误,现在给大家分享一下: 我想要打印出来一个5*5的实心 ...

  6. python 库位置

    l\\Programs\\Python\\Python36\\lib   默认库安装路径 \\Programs\\Python\\Python36\\lib\\site-packages 第三方库安装 ...

  7. linux查看python安装位置

    1, import sys print sys.path 即可打印所有python路径.   2, 执行命令whereis python即可显示出python相关的所有的路径,包括可执行文件路径,安装 ...

  8. python重置变量 sys.defaultencoding

    <span style="font-family: Arial, Helvetica, sans-serif;">比如重置变量defaultencoding为utf-8 ...

  9. ros python 构造 pose

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

随机推荐

  1. Thinkpad机器BIOS下清除安全芯片和指纹数据的方法

    清除安全芯片: 首先在刚开机出现ThinkPad图标时,按F1进入BIOS界面,然后长按关机按钮关机(注意一定是断电的关机,不是重新启动)然后开机再按F1键进入BIOS设置.选择“Securiy”-〉 ...

  2. SQL Expression Language Tutorial 学习笔记二

    11. Using Textual SQL 直接使用 SQL 如果实在玩不转, 还是可以通过 test() 直接写 SQL. In [51]: s = text( ...: "SELECT ...

  3. c++四种强制类型转化

    c++ 数据类型转换: static_cast dynamic_cast reinterpret_cast const_cast C++ 类型转换(C风格的强制转换): (1)将浮点型数据赋值给整型变 ...

  4. Mybatis—三剑客之generator使用方法

    三剑客之generator主要用于自动生成POJO实体类   准备素材: mybatis-generator-core-1.3.2.jar     mysql-connector-java-5.1.2 ...

  5. python 内置函数bytearray

    1.参考文档 class bytearray([source[, encoding[, errors]]]) Return a new array of bytes. The bytearray cl ...

  6. Python: 类中为什么要定义__init__()方法

    学习并转自:https://blog.csdn.net/geerniya/article/details/77487941 1. 不用init()方法定义类 定义一个矩形的类,目的是求周长和面积. c ...

  7. 使用基于 PHP 的开源软件 YOURLS 搭建短链接地址服务

    使用基于 PHP 的开源软件 YOURLS搭建 系统配置 php7.1+mysql5.7+nginx 下载源代码 git clone https://github.com/YOURLS/YOURLS. ...

  8. mysql 触发器 trigger用法 two (稍微复杂的)

    触发器(trigger):监视某种情况,并触发某种操作. 触发器创建语法四要素:1.监视地点(table) 2.监视事件(insert/update/delete) 3.触发时间(after/befo ...

  9. SNMP学习笔记之SNMPWALK 安装与使用详解

    0x00 简介 snmpwalk是SNMP的一个工具,它使用SNMP的GETNEXT请求查询指定OID(SNMP协议中的对象标识)入口的所有OID树信息,并显示给用户.通过snmpwalk也可以查看支 ...

  10. P3868 [TJOI2009]猜数字

    [TJOI2009]猜数字 中国剩余定理 求解i=1 to n : x≡a[i] (mod b[i])的同余方程组 设 t= ∏i=1 to n b[i] 我们先求出 i=1 to n : x≡1 ( ...