目录
1、rospy 网上开源小例子
2、python PoseStamped 例子
3、python常识
4、rospy 规范写法与介绍
5、rospy.is_shutdown()
6、python写ros 话题
7、订阅话题
1、rospy 网上开源小例子
http://codingdict.com/sources/py/rospy.html
2、python PoseStamped 例子
def __init__(self):
rospy.init_node("mav_control_node")
rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.pose_callback)
rospy.Subscriber("/mavros/rc/in", RCIn, self.rc_callback)
self.cmd_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=1)
self.cmd_vel_pub = rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped", Twist, queue_size=1)
self.rc_override = rospy.Publisher("/mavros/rc/override", OverrideRCIn, queue_size=1)
# mode 0 = STABILIZE
# mode 4 = GUIDED
# mode 9 = LAND
self.mode_service = rospy.ServiceProxy('/mavros/set_mode', SetMode)
self.arm_service = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
self.takeoff_service = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL)
self.rc = RCIn()
self.pose = Pose()
self.timestamp = rospy.Time()
def visualRobotCallback(self, pose_stamped_msg):
"""
Callback for ROS topic
Get the new updated position of robot from camera
:param pose_stamped_msg: (PoseStamped ROS message)
"""
self.robot_pos[0] = pose_stamped_msg.pose.position.x
self.robot_pos[1] = pose_stamped_msg.pose.position.y
self.robot_yaw = euler_from_quaternion([pose_stamped_msg.pose.orientation.x, pose_stamped_msg.pose.orientation.y,
pose_stamped_msg.pose.orientation.z, pose_stamped_msg.pose.orientation.w])[2]
if NO_TARGET_MODE and self.target_pos_changed:
# simulate the target's position update
self.target_pos[0] = 0.0
self.target_pos[1] = 0.0
self.target_yaw = 0.0
self.target_pos_changed = False
3、python常识
字典是无序的，列表（list）和元组（tuple）都是有序的序列，它们的元素在底层是挨着存放的。
4、rospy 规范写法与介绍
5、rospy.is_shutdown()
用于检测是否退出，是否按Ctrl C 或者其他
rate = rospy.Rate(10)
rate.sleep() 是为了休眠
6、python写ros 话题
导入ROS模块
用python编写ROS的程序有很多有点，Python的numpy模块可以方便快速的完成机器人规划、正逆运动学的开发，如果需要完成更复杂的计算功能，可以使用scipy模块完成科学计算，对采集数据的时间系列分析可以采用pandas做数据分析，最重要的是Python的matplotlib模块可以完成绝大部分的数据绘图，可以与pyqt5结合完成数据的可视化显示。当然Python还可以方便的调用机器人学习、深度学习等模块完成人工智能的开发，用最短的时间完成机器人的智能化。
Python中使用ROS首先要导入rospy模块
import rospy
完成机械臂的控制还需要导入其他数据模块，ROS在C++中的数据类型在Python也可以找到，其中最常用的数据模块是std_msgs、sensor_msgs、geometry_msgs模块(下面代码作为参考)
from std_msgs.msg import Float64MultiArray
from sensor_msgs.msg import JointState
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist,PoseStamped
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from random import sample
from math import pow, sqrt
发送话题:ros话题是最常用的通讯方式，通过话题实现数据传输，下面将介绍话题发布。
首先，需要建立一个一个节点，并发起一个或多个话题
#!/usr/bin/env python
# -*-coding:utf-8-*-
# 本文档用于发送关节角度
# 程序员：CYT
# 版权：哈尔滨工业大学(深圳）
# 日期：初稿：2019.11.6
# 转载
import rospy
from std_msgs.msg import Float64MultiArray
import os
import numpy as np
def talker():
# 建立节点
rospy.init_node("joint_position_command", anonymous=True)
#建立话题
pub = rospy.Publisher("joint_command", Float64MultiArray, queue_size=1)
#设置发送频率
rate = rospy.Rate(100) # 100hz
#假设数据
command_pos = np.zeros([1000,7])
# 重写数据
kk = len(command_pos[:, 0])
n = len(command_pos[0, :])
command_data = np.zeros([kk, n])
for i in range(kk):
for j in range(n):
command_data[i, j] = command_pos[i, j]
#主循环中发送数据
k = 0
while not rospy.is_shutdown():
if k == kk:
break
tip_str = "第 %s 次命令：" % k
rospy.loginfo(tip_str)
send_data = Float64MultiArray()
send_data.data = command_data[k, :]
print send_data.data
#发送数据
pub.publish(send_data)
#休眠
rate.sleep()
k = k + 1
if __name__ == '__main__':
try:
talker()
except rospy.ROSInterruptException:
pass
Float64MultiArray, queue_size=1)
7、订阅话题
(这一小块的代码全部来自于这篇文章 :https://blog.csdn.net/weixin_43956732/article/details/105951288)
Python接受话题有两种方式，但是与C++相比，其只有rospy.spin()，没有ros::spinOnce，所以想要在循环中处理需要特别注意。
第一种方式：rospy.Subscriber
第一种方式与c++类似，调用回调函数来处理，但是因为只有rospy.spin()来调用回调函数，所以程序会阻塞在回调函数中，接受到一个数据，调用一次。
#建立节点
rospy.init_node('listener', anonymous=True)
#订阅话题
rospy.Subscriber('/joint_states', JointState, callback)
#调用回调函数，并阻塞，直到程序结束
rospy.spin()
第二种方式：rospy.wait_for_message
该方法无需节点，也无需回调函数，其与一个函数类似，等待话题发布消息，当接收到一个消息时，返回数据，继续执行后面的程序。相对于第一种，我们把它称为半阻塞，接收的话题如果没有发布消息，它会一直等待，但是接收到一个消息后，等待结束，会继续执行后面的程序。
msg2 = rospy.wait_for_message('/joint_states', JointState, timeout=None)
完整的代码:通过两个函数演示两种不同的就收方法，当上文中话题启动后，下文中的程序就可接受到数据。
#!/usr/bin/env python
# -*-coding:utf-8-*-
# 本文档用于接收信息
# 程序员：CYT
# 版权：哈尔滨工业大学(深圳）
# 日期：初稿：2019.12.12
import numpy as np
import rospy
from std_msgs.msg import Float64MultiArray
def callback(data):
print "msg:", data
def listener1():
#建立节点
rospy.init_node('listener', anonymous=True)
#订阅话题
rospy.Subscriber('joint_command', Float64MultiArray, callback)
#调用回调函数，并阻塞
rospy.spin()
def listener2():
#用循环来订阅所有数据
while not rospy.is_shutdown():
#订阅话题
msg = rospy.wait_for_message('joint_command', Float64MultiArray, timeout=None)
print "msg: %s" % msg
if __name__ == '__main__':
#运行程序1
#listener1()
#运行程序2
listener2()
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
