//  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;
    }
  }
Exemple #2
0
 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++;

}
Exemple #5
0
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;
}
Exemple #7
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);
  }
Exemple #13
0
void handle_cmd(android_app *app, int32_t c) {
    if (c == APP_CMD_LOST_FOCUS) {
        bag.close();
    }
}
 ~PointCloudCapturer()
 {
   bag_.close();
   delete point_head_client_;
 }