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