def pub_data(self, query):
""" publish data from DB to ROS """
rospy.loginfo('Start publishing data to ROS')
posts = self.db.posts.find(query)
rospy.loginfo('number of data in DB %s', posts.count())
rate = rospy.Rate(rospy.get_param('data_rate'))
for post in posts:
if post['tags'][0]=='image':
rospy.loginfo('publishing Image')
msg = Image()
msg.header.stamp = rospy.Time.from_sec(post['time'])
msg.height = post['height']
msg.width = post['width']
msg.data = post['data']
self.image_pub.publish(msg)
rospy.loginfo(msg)
if post['tags'][0]=='joint_state':
rospy.loginfo('publishing Joint state')
msg = JointState()
msg.header.stamp = rospy.Time.from_sec(post['time'])
msg.name = post['joint_names']
msg.position = post['position']
msg.velocity = post['velocity']
msg.effort = post['effort']
self.joint_pub.publish(msg)
rospy.loginfo(msg)
if post['tags'][0]=='lidar':
rospy.loginfo('publishing Lidar')
msg = LaserScan()
msg.header.stamp = rospy.Time.from_sec(post['time'])
msg.time_increment = post['time_increment']
msg.angle_min = post['angle_min']
msg.angle_max = post['angle_max']
msg.range_min = post['range_min']
msg.range_max = post['range_max']
msg.angle_increment = post['angle_increment']
msg.ranges = post['ranges']
self.lidar_pub.publish(msg)
rospy.loginfo(msg)
if post['tags'][0]=='odom':
rospy.loginfo('publishing Odometry')
msg = Odometry()
msg.header.stamp = rospy.Time.from_sec(post['time'])
msg.pose.pose.position.x = post['position'][0]
msg.pose.pose.position.y = post['position'][1]
msg.pose.pose.position.z = post['position'][2]
msg.twist.twist.linear.x = post['velocity'][0]
msg.twist.twist.linear.y = post['velocity'][1]
msg.twist.twist.angular.z = post['velocity'][2]
self.odom_pub.publish(msg)
rospy.loginfo(msg)
rate.sleep()
def db_to_bag(self, query, filename):
""" transfer data from database to bag file using query """
bag = rosbag.Bag(filename, 'w')
query_list = re.sub('[^\w]', ' ', query).split()
rospy.loginfo(query_list)
rospy.loginfo(query)
field = query_list[0]
value = query_list[1]
query = {field: value}
posts = self.get_data(query)
try:
rospy.loginfo('number of data in DB %s', posts.count())
for post in posts:
if post['tags'][0] == 'image':
msg = Image()
msg.height = post['height']
msg.width = post['width']
msg.data = post['data']
bag.write('/image_rgb',
msg,
t=rospy.Time.from_sec(post['time']))
rospy.loginfo(msg)
if post['tags'][0] == 'joint_states':
msg = JointState()
msg.name = post['joint_names']
msg.position = post['position']
msg.velocity = post['velocity']
msg.effort = post['effort']
bag.write('/JointState',
msg,
t=rospy.Time.from_sec(post['time']))
rospy.loginfo(msg)
if post['tags'][0] == 'lidar':
msg = LaserScan()
msg.time_increment = post['time_increment']
msg.angle_min = post['angle_min']
msg.angle_max = post['angle_max']
msg.range_min = post['range_min']
msg.range_max = post['range_max']
msg.angle_increment = post['angle_increment']
msg.ranges = post['ranges']
bag.write('/scan',
msg,
t=rospy.Time.from_sec(post['time']))
rospy.loginfo(msg)
if post['tags'][0] == 'odom':
msg = Odometry()
msg.pose.pose.position.x = post['position'][0]
msg.pose.pose.position.y = post['position'][1]
msg.pose.pose.position.z = post['position'][2]
msg.twist.twist.linear.x = post['velocity'][0]
msg.twist.twist.linear.y = post['velocity'][1]
msg.twist.twist.angular.z = post['velocity'][2]
bag.write('/odom',
msg,
t=rospy.Time.from_sec(post['time']))
rospy.loginfo(msg)
finally:
bag.close()
rospy.loginfo("Save DB to bag complete!")
rospy.loginfo('number of data read from DB %s', posts.count())
