ros python 重置位置
#!/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 重置位置的更多相关文章
- 如何查看Python 安装位置以及已经安装的库
如何查看Python 安装位置以及已经安装的库 步骤一: 1. Start 一个command prompt 2. 找到电脑中已经安装的Python 位置: where python 1 打开路径, ...
- 查看python安装位置和已安装库的相关操作
打开cmd.exe, *查看python安装位置 where python *查看已安装库 pip list 或者pip freeze *查看可以更新的第三方库 pip list --outdated ...
- 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 ...
- ros python 订阅robot_pose
#!/usr/bin/env python import rospy import tf import time from tf.transformations import * from std_m ...
- python代码位置引发的错误
觉得python对代码位置的要求简直是变态,缩进引发的错误我以前在博客里讲过了,如果不懂可以看看我以前的博客,今天又遇到了一个代码位置所引发的错误,现在给大家分享一下: 我想要打印出来一个5*5的实心 ...
- python 库位置
l\\Programs\\Python\\Python36\\lib 默认库安装路径 \\Programs\\Python\\Python36\\lib\\site-packages 第三方库安装 ...
- linux查看python安装位置
1, import sys print sys.path 即可打印所有python路径. 2, 执行命令whereis python即可显示出python相关的所有的路径,包括可执行文件路径,安装 ...
- python重置变量 sys.defaultencoding
<span style="font-family: Arial, Helvetica, sans-serif;">比如重置变量defaultencoding为utf-8 ...
- ros python 构造 pose
#!/usr/bin/env python import numpy as npfrom geometry_msgs.msg import Pose, Point, Quaternion, Twist ...
随机推荐
- [py]你真的了解多核处理器吗? 了解多线程
越来越多的人搞爬虫,设计到多线程爬取, 还有一些机器学习的一些模块也需要这玩意, 感觉自己不会逼格不高. 抽时间赶紧玩一玩这东西, 希望提高对软件的认知和归属感,不要太傻. cpu内部架构参考 你知道 ...
- Linux实验楼学习之一
查看当前所在目录 pwd 创建文件:1-1.txt touch 1-1.txt 进入统计目录下的etc目录 cd /etc 强行终止当前程序 Ctrl + c 常用快捷键 按键 作用 Ctrl+d 键 ...
- [LeetCode] 193. Valid Phone Numbers_Easy tag: Bash
Given a text file file.txt that contains list of phone numbers (one per line), write a one liner bas ...
- APPIUM Android 定位方式
原文地址https://www.cnblogs.com/sundalian/p/5629500.html 1.定位元素应用元素 1.1通过id定位元素 Android里面定位的id一般为resrouc ...
- testng入门教程4用TestNG执行case
使用TestNG类执行测试用例.这个类的主入口点在TestNG的框架运行测试.用户可以创建自己的TestNG的对象,并调用它以许多不同的方式: 在现有的testng.xml 合成testng.xml, ...
- .NET RSA解密、签名、验签
using System; using System.Collections.Generic; using System.Text; using System.IO; using System.Sec ...
- unity3D中 material中tiling和offset属性解释
贴图有可能是多行多列的一些图案组成的.当我们需要一帧,一帧的播放时候.也就是帧序列动画, 我们就需要用到tiling和offset两个属性, 默认图片的左下角为坐标圆点即:(0,0) tiling是图 ...
- Mysql闭包表之关于国家区域的一个实践
在电商系统中,我们总是会遇到一些树形结构数据的存储需求.如地理区域.位置信息存储,地理信息按照层级划分,会分为很多层级,就拿中国的行政区域划分为例,简单的省-市-县-镇-村就要五个级别.如果系统涉及到 ...
- zw版【转发·台湾nvp系列Delphi例程】HALCON SetComprise1
zw版[转发·台湾nvp系列Delphi例程]HALCON SetComprise1 procedure TForm1.Button1Click(Sender: TObject);var image0 ...
- Python: yield, python 实现tail -f
def CreateGenerator(file): with open(file,'r') as t: t.seek(0,2) while True: line=t.readline() if no ...