// void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc) void callback (const sensor_msgs::PointCloud2ConstPtr& pc, const sensor_msgs::ImageConstPtr& im) { if (!cloud_and_image_received_) { //Write cloud //transform to target frame bool found_transform = tf_.waitForTransform(pc->header.frame_id, to_frame_, pc->header.stamp, ros::Duration(10.0)); tf::StampedTransform transform; if (found_transform) { //ROS_ASSERT_MSG(found_transform, "Could not transform to camera frame"); tf_.lookupTransform(to_frame_,pc->header.frame_id, pc->header.stamp, transform); ROS_DEBUG("[TransformPointcloudNode:] Point cloud published in frame %s", pc->header.frame_id.c_str()); } else { ROS_ERROR("No transform for pointcloud found!!!"); return; } bag_.write(input_cloud_topic_, pc->header.stamp, *pc); geometry_msgs::TransformStamped transform_msg; tf::transformStampedTFToMsg(transform, transform_msg); bag_.write(input_cloud_topic_ + "/transform", transform_msg.header.stamp, transform_msg); ROS_INFO("Wrote cloud to %s", bag_name_.c_str()); //Write image found_transform = tf_.waitForTransform(im->header.frame_id, to_frame_, im->header.stamp, ros::Duration(10.0)); if (found_transform) { //ROS_ASSERT_MSG(found_transform, "Could not transform to camera frame"); tf_.lookupTransform(to_frame_,im->header.frame_id, im->header.stamp, transform); ROS_DEBUG("[TransformPointcloudNode:] Point cloud published in frame %s", im->header.frame_id.c_str()); } else { ROS_ERROR("No transform for image found!!!"); return; } bag_.write(input_image_topic_, im->header.stamp, im); tf::transformStampedTFToMsg(transform, transform_msg); bag_.write(input_image_topic_ + "/transform", transform_msg.header.stamp, transform_msg); ROS_INFO("Wrote image to %s", bag_name_.c_str()); cam_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(input_camera_info_topic_); bag_.write(input_camera_info_topic_, cam_info_->header.stamp, cam_info_); ROS_INFO("Wrote Camera Info to %s", bag_name_.c_str()); cloud_and_image_received_ = true; } }
void write(rosbag::Bag& bag, const std::string& topic, const ros::Time& stamp, const ecto::tendril& t) const { MessageConstPtr mcp; t >> mcp; bag.write(topic, stamp, *mcp); }
void ptRecorder::publishPoint() { pointPub.publish(pos); bag.write("point", ros::Time::now(), pos); //Save points in rosbag ROS_INFO("Waypt: %f",wayptCounter); ROS_INFO_STREAM("Waypoint: " << pos); //Display in console wayptCounter++; }
void record_message(rosbag::Bag &b) { static int count = 0; std_msgs::String str; str.data = boost::str(boost::format("foo%1%") % count); std_msgs::Int32 i; i.data = count; log("writing stuff into bag"); try { bag.write("chatter", ros::Time::now(), str); bag.write("numbers", ros::Time::now(), i); } catch (const std::exception &e) { log("Oops! could not write to bag: %s, %s", e.what(), strerror(errno)); return; } }
bool writeSummaryToBag(ros::ServiceClient& sum_client, rosbag::Bag& bag) { PlanningSummary planning_sum; if (!sum_client.call(planning_sum)) { return false; } ROS_INFO("Writing grasp planning log to bag: %s", bag.getFileName().c_str()); try { bag.write("grasp_template_planning_log", ros::Time::now(), planning_sum.response); } catch (rosbag::BagIOException ex) { ROS_ERROR("Problem when writing log file >%s< : %s.", bag.getFileName().c_str(), ex.what()); return false; } return true; }