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())