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