// 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);
}
int OnExit()
{
std::cout << "I shut myself down!" << std::endl;
if(p_bag) { p_bag->close(); delete p_bag; }
delete p_view;
return 0;
}
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 open_bag() {
log("opening bag");
try {
bag.open("/sdcard/test.bag", rosbag::bagmode::Write);
} catch (rosbag::BagException e) {
log("could not open bag file for writing: %s, %s", e.what(), LASTERR);
return;
}
}
int main(int argc, char** argv) {
ros::init(argc, argv, "pointRecorder_node");
ft = 1;
ptRecorder joy_teleop_node;
ros::spin();
//Close bag
bag.close();
return 0;
}
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 OnInit()
{
std::cout << "Init" << std::endl;
if (this->argc != 3) { std::cout << " please provide path to bag file and a topic name!" << std::endl; exit(0); }
std::string bag_file(wxString(this->argv[1]).mb_str());
std::string topic(wxString(this->argv[2]).mb_str());
char* av = "bag_gui";
ros::init(argc, &av, "bag_gui");
nh = new ros::NodeHandle();
one_.setOutputLabels(labels_);
one_.setPixelSearchRadius(8,2,2);
one_.setSkipDistantPointThreshold(8);
seg_.setNormalCloudIn(normals_);
seg_.setLabelCloudInOut(labels_);
seg_.setClusterGraphOut(graph_);
p_bag = new rosbag::Bag();
p_view = new rosbag::View();
try { p_bag->open(bag_file, rosbag::bagmode::Read); }
catch (rosbag::BagException) { std::cerr << "Error opening file " << bag_file << std::endl; return false; }
p_view->addQuery(*p_bag, rosbag::TopicQuery(topic));
rosbag::View::iterator vit = p_view->begin();
while(vit!=p_view->end()) { timeline.push_back(vit); ++vit; }
tl_it = timeline.begin();
sensor_msgs::PointCloud2::ConstPtr last_msg = (*tl_it)->instantiate<sensor_msgs::PointCloud2>();
pcl::fromROSMsg<PT>(*last_msg, *pc_);
compute();
r_rgb_ = Gui::Core::rMan()->create<PC>("r_rgb", pc_);
r_seg_ = Gui::Core::rMan()->create<PC>("r_seg", segmented_);
v_rgb_ = r_rgb_->createView<Col>("v_rgb");
v_seg_ = r_seg_->createView<Col>("v_seg");
boost::function<void (wxMouseEvent&, Gui::Resource<PC>*)> f = boost::bind(&MainApp::mouseShowNext, this, _1, _2);
v_seg_ ->registerMouseCallback(f);
boost::function<void (wxKeyEvent&, Gui::Resource<PC>*)> k = boost::bind(&MainApp::keyShowNext, this, _1, _2);
v_seg_ ->registerKeyCallback(k);
v_seg_->show();
v_rgb_->show();
std::cout<<"Done Init"<<std::endl;
return true;
}
bool TrajectoryVideoLookup::openBag(const std::string& bagpath, const rosbag::bagmode::BagMode bagmode, rosbag::Bag& bag)
{
boost::filesystem::path path(bagpath);
if(boost::filesystem::is_directory(path))
{
// check for default bag file name "video_lookup.bag"
path = path / "video_lookup.bag";
bag.open( path.string(), bagmode );
return true;
}
else
{
// check if a bag file
if( path.extension().c_str() == ".bag" )
{
// is it a bag file?
if( boost::filesystem::exists( path ) || bagmode==rosbag::bagmode::Write )
{
// proceed to read
bag.open(path.string(), bagmode);
return true;
}
else
{
// no file to read
ROS_INFO("No such bag file exists.");
}
}
else
{
// can't read this file
ROS_INFO("Provided file is not a bag: %s", path.extension().c_str() );
}
}
return false;
}
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;
}
void ptRecorder::joyCallback(const sensor_msgs::Joy::ConstPtr &msg) {
// process and publish
geometry_msgs::Twist twistMsg;
// check button, change variable button to switch to another button
bool switchActive = (msg->buttons[Button] == 1);
if (switchActive) {
//Run following code if button is pressed for the 1st time
if(ft){
ft=0;
bag.open("/home/fyp-trolley/catkin_ws/waypts.bag", rosbag::bagmode::Write); //ToDo: Open bag only if button is pressed
}
publishPoint();
}
}
PointCloudCapturer(ros::NodeHandle &n)
: nh_(n), synchronizer_( MySyncPolicy(1), cloud_sub_, camera_sub_)
{
nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/kinect_head/camera/rgb/points"));
nh_.param("input_image_topic", input_image_topic_, std::string("/kinect_head/camera/rgb/image_color"));
nh_.param("input_camera_info_topic", input_camera_info_topic_, std::string("/kinect_head/camera/rgb/camera_info"));
// cloud_sub_= nh_.subscribe (input_cloud_topic_, 10, &PointCloudCapturer::cloud_cb, this);
camera_sub_.subscribe( nh_, input_image_topic_, 1000 );
cloud_sub_.subscribe( nh_, input_cloud_topic_, 1000 );
sync_connection_ = synchronizer_.registerCallback( &PointCloudCapturer::callback, this );
ROS_INFO("[PointCloudColorizer:] Subscribing to cloud topic %s", input_cloud_topic_.c_str());
point_head_client_ = new PointHeadClient("/head_traj_controller/point_head_action", true);
//wait for head controller action server to come up
while(!point_head_client_->waitForServer(ros::Duration(5.0)) && ros::ok())
{
ROS_INFO("Waiting for the point_head_action server to come up");
}
cloud_and_image_received_ = false;
nh_.param("move_offset_y_min", move_offset_y_min_, -1.5);
nh_.param("move_offset_y_max", move_offset_y_max_, 1.5);
nh_.param("step_y", step_y_, 0.3);
nh_.param("move_offset_z_min", move_offset_z_min_, 0.3);
nh_.param("move_offset_z_max", move_offset_z_max_, 1.5);
nh_.param("step_z", step_z_, 0.3);
nh_.param("move_offset_x", move_offset_x_, 1.0);
nh_.param("bag_name", bag_name_, std::string("bosch_kitchen_tr.bag"));
nh_.param("to_frame", to_frame_, std::string("base_link"));
nh_.param("rate", rate_, 1.0);
current_position_x_ = move_offset_x_;
current_position_y_ = move_offset_y_min_;
current_position_z_ = move_offset_z_min_;
move_head("base_link", current_position_x_, current_position_y_, current_position_z_);
EPS = 1e-5;
//thread ROS spinner
spin_thread_ = boost::thread (boost::bind (&ros::spin));
bag_.open(bag_name_, rosbag::bagmode::Write);
}
void handle_cmd(android_app *app, int32_t c) {
if (c == APP_CMD_LOST_FOCUS) {
bag.close();
}
}
~PointCloudCapturer()
{
bag_.close();
delete point_head_client_;
}
