// 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;
}