int main(int argc, char* argv[] ) {
    ros::init(argc, argv, "sdf_2d");
    ros::NodeHandle nh("sdf_main");

    ros::NodeHandle private_nh("~");
    bool p_bag_mode;
    private_nh.param("p_bag_mode", p_bag_mode, false);

    sdfslam::SignedDistanceField sdf;

    if (!p_bag_mode)
        ros::spin();
    else {
        bool cont = true;
        unsigned int microseconds = 10000;

        while (ros::ok && cont) {
            cont = sdf.checkTimeout();
            ros::spinOnce();
            usleep(microseconds);
        }
    }
    ROS_INFO("shutting down sdf slam..");

    return 0;
}

void OrientGoal::initialize(std::string name,tf::TransformListener* tf,
    costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap,
    geometry_msgs::Quaternion* goal_orientation ){
  if(!initialized_){
    name_ = name;
    tf_ = tf;
    global_costmap_ = global_costmap;
    local_costmap_ = local_costmap;
    goal_orientation_ = goal_orientation;

    //get some parameters from the parameter server
    ros::NodeHandle private_nh("~/" + name_);
    ros::NodeHandle blp_nh("~/TrajectoryPlannerROS");

    //we'll simulate every degree by default
    private_nh.param("sim_granularity", sim_granularity_, 0.017);
    private_nh.param("frequency", frequency_, 20.0);

    blp_nh.param("acc_lim_th", acc_lim_th_, 1.0);
    blp_nh.param("max_rotational_vel", max_rotational_vel_, 1.0);
    blp_nh.param("min_in_place_rotational_vel", min_rotational_vel_, 0.4);
    blp_nh.param("yaw_goal_tolerance", tolerance_, 0.10);

    local_costmap_->getCostmapCopy(costmap_);
    world_model_ = new base_local_planner::CostmapModel(costmap_);

    initialized_ = true;
  }
  else{
    ROS_ERROR("You should not call initialize twice on this object, doing nothing");
  }
}

void BoxDetectionNode::loadParameters()
{
    ros::NodeHandle private_nh("~");
#define GOP(key, default_value) getOptionalParameter(private_nh, #key, parameters_.key##_, default_value)
#define GRP(key) getRequiredParameter(private_nh, #key, parameters_.key##_)
    GOP(point_cloud_topic, std::string("/camera/depth/points"));
    GOP(target_frame_id, std::string("base_link"));
    GOP(have_action_server_debug_output, true);
    GOP(have_box_detection_debug_output, true);

    GOP(box_plane_points_min, 100);
    GRP(box_plane_size_min);
    GRP(box_plane_size_max);
    GOP(detection_timeout, 30.0);
    GOP(plane_fitting_distance_threshold, 0.02);
    GOP(plane_fitting_max_iterations, 50);
    GOP(downsampling_leaf_size, 0.005);
    GOP(clusterization_tolerance, 0.02);

    GOP(have_plane_publisher, true);
    GOP(plane_publishing_rate, 2.0);

    GOP(have_collision_object_publisher, true);
    GOP(collision_objects_basename, std::string("box"));

    GOP(have_visualization_marker_publisher, true);
    GOP(visualization_marker_namespace, std::string("tedusar_box_detection"));
#undef GOP
#undef GRP
}

int main(int argc, char** argv){
  ros::init(argc, argv, "turtlebot_car");
  
  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");

  //we get the name of the odometry topic
  private_nh.getParam("odometry_topic_name", mOdometry_topic_name);

  //we get paramaters that define the pose of the URG
  //position
  x_laser=0.0;  y_laser=0.0;  z_laser=0.0;
  private_nh.getParam("x_laser", x_laser);private_nh.getParam("y_laser", y_laser);private_nh.getParam("z_laser", z_laser);

  //orientation
  roll_laser=0.0;  pitch_laser=0.0;  yaw_laser=0.0;
  private_nh.getParam("roll_laser", roll_laser);private_nh.getParam("pitch_laser", pitch_laser);private_nh.getParam("yaw_laser", yaw_laser);
  
  //we define the subscriber to odometry
  ros::Subscriber sub = nh.subscribe<nav_msgs::Odometry>(mOdometry_topic_name, 10, &odometryCallback);

  std::cout<<"***************[ STARING turtlebot_car_node]***************** "<<std::endl;

  ros::spin();
  return 0;
};

  void DWAPlannerROS::initialize(
      std::string name,
      tf::TransformListener* tf,
      costmap_2d::Costmap2DROS* costmap_ros) {
    if (! isInitialized()) {

      ros::NodeHandle private_nh("~/" + name);
      g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
      l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
      tf_ = tf;
      costmap_ros_ = costmap_ros;
      costmap_ros_->getRobotPose(current_pose_);

      // make sure to update the costmap we'll use for this cycle
      costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();

      planner_util_.initialize(tf, costmap, costmap_ros_->getGlobalFrameID());

      //create the actual planner that we'll use.. it'll configure itself from the parameter server
      dp_ = boost::shared_ptr<DWAPlanner>(new DWAPlanner(name, &planner_util_));
      
      initialized_ = true;

      dsrv_ = new dynamic_reconfigure::Server<DWAPlannerConfig>(private_nh);
      dynamic_reconfigure::Server<DWAPlannerConfig>::CallbackType cb = boost::bind(&DWAPlannerROS::reconfigureCB, this, _1, _2);
      dsrv_->setCallback(cb);
    }
    else{
      ROS_WARN("This planner has already been initialized, doing nothing.");
    }
  }

int main(int argc, char **argv)
{
  ros::init(argc, argv, "waypoints_marker_publisher");
  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");

  //subscribe traffic light
  ros::Subscriber light_sub = nh.subscribe("light_color", 10, receiveAutoDetection);
  ros::Subscriber light_managed_sub = nh.subscribe("light_color_managed", 10, receiveManualDetection);

  //subscribe global waypoints
  ros::Subscriber lane_array_sub = nh.subscribe("lane_waypoints_array", 10, laneArrayCallback);
  ros::Subscriber traffic_array_sub = nh.subscribe("traffic_waypoints_array", 10, laneArrayCallback);

  //subscribe local waypoints
  ros::Subscriber temporal_sub = nh.subscribe("temporal_waypoints", 10, temporalCallback);
  ros::Subscriber closest_sub = nh.subscribe("closest_waypoint", 10, closestCallback);

  //subscribe config
  ros::Subscriber config_sub = nh.subscribe("config/lane_stop", 10, configParameter);

  g_local_mark_pub = nh.advertise<visualization_msgs::MarkerArray>("local_waypoints_mark", 10, true);
  g_global_mark_pub = nh.advertise<visualization_msgs::MarkerArray>("global_waypoints_mark", 10, true);

  //initialize path color
  _initial_color.b = 1.0;
  _initial_color.g = 0.7;
  _global_color = _initial_color;
  _global_color.a = g_global_alpha;
  g_local_color = _initial_color;
  g_local_color.a = g_local_alpha;

  ros::spin();

}

void JointStatePublisher::init()
{
    ros::NodeHandle private_nh("~");

    std::string yaw_joint_name, pitch_joint_name;
    private_nh.param<std::string>("yaw_joint_name", yaw_joint_name, std::string("sh_yaw_joint"));
    private_nh.param<std::string>("pitch_joint_name", pitch_joint_name, std::string("sh_pitch_joint"));

    double frequency;
    private_nh.param<double>("frequency", frequency, 30.0);

    sh_joint_states_.name.resize(2);
    sh_joint_states_.position.resize(2);
    sh_joint_states_.name[0] = yaw_joint_name;
    sh_joint_states_.position[0] = 0.0;
    sh_joint_states_.name[1] = pitch_joint_name;
    sh_joint_states_.position[1] = 0.0;

    joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>("sh_joint_state", 1);

    servo_1_state_sub_ = nh_.subscribe("servo_yaw_state", 1, &JointStatePublisher::servo1StateMsgCB, this);
    servo_2_state_sub_ = nh_.subscribe("servo_pitch_state", 1, &JointStatePublisher::servo2StateMsgCB, this);

    publish_joint_state_timer_ = nh_.createTimer(ros::Duration(1/frequency), &JointStatePublisher::publishJointStateTimerCB, this, false );
}

int main(int argc, char *argv[])
{
#if defined(USE_POSIX_SHARED_MEMORY)
  attach_ShareMem();

  while (1)
    {
      CvMemStorage *houghStorage = cvCreateMemStorage(0);
      IplImage *frame = getImage_fromSHM();
      process_image_common(frame);
      cvReleaseImage(frame);
    }

  detach_ShareMem();
#else
  ros::init(argc, argv, "line_ocv");
  ros::NodeHandle n;
  ros::NodeHandle private_nh("~");
  std::string image_topic_name;
  private_nh.param<std::string>("image_raw_topic", image_topic_name, "/image_raw");
  ROS_INFO("Setting image topic to %s", image_topic_name.c_str());

  ros::Subscriber subscriber = n.subscribe(image_topic_name, 1, lane_cannyhough_callback);

  image_lane_objects = n.advertise<lane_detector::ImageLaneObjects>("lane_pos_xy", 1);

  ros::spin();
#endif

  return 0;
}

SteeringDriver::SteeringDriver(int argc, char** argv, std::string node_name) {
    // Setup ROS stuff
    ros::init(argc, argv, "steering_driver");
    ros::NodeHandle private_nh("~");
    ros::Rate loop_rate(10);

    // Setup Subscriber(s)
    twist_subscriber =
    private_nh.subscribe("/cmd_vel", 10, &SteeringDriver::twistCallback, this);

    // Get Params
    SB_getParam(private_nh, "port", port, (std::string) "/dev/ttyACM0");
    SB_getParam(private_nh, "max_abs_linear_speed", max_abs_linear_speed, 2.0);
    SB_getParam(
    private_nh, "max_abs_angular_speed", max_abs_angular_speed, 1.0);
    // Ensure that the values given for max absolute speeds are positive
    max_abs_linear_speed  = fabs(max_abs_linear_speed);
    max_abs_angular_speed = fabs(max_abs_angular_speed);

    // Setup Arduino stuff
    // Get the Arduino port from a ros param
    // TODO - Give some indication when we attach/detach from the Steering
    // Controller. Could be as simple as checking how long it was since we last
    // received a message
    // Open the given serial port
    arduino.Open(port);
    arduino.SetBaudRate(LibSerial::SerialStreamBuf::BAUD_9600);
    arduino.SetCharSize(LibSerial::SerialStreamBuf::CHAR_SIZE_8);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "laserscan2costmap");

  tf::TransformListener tf_listener;
  g_tf_listenerp = &tf_listener;

  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");

  private_nh.param<double>("resolution", g_resolution, 0.1);
  private_nh.param<int>("scan_size_x", g_scan_size_x, 1000);
  private_nh.param<int>("scan_size_y", g_scan_size_y, 1000);
  private_nh.param<int>("map_size_x", g_map_size_x, 500);
  private_nh.param<int>("map_size_y", g_map_size_y, 500);
  private_nh.param<std::string>("scan_topic", g_scan_topic, "/scan");
  private_nh.param<std::string>("sensor_frame", g_sensor_frame, "/velodyne");

  ros::Subscriber laserscan_sub = nh.subscribe(g_scan_topic, 1, laserScanCallback);

  g_map_pub = nh.advertise<nav_msgs::OccupancyGrid>("/ring_ogm", 1);

  ros::spin();

  return 0;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "voxel_grid_filter");

  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");

  private_nh.getParam("points_topic", POINTS_TOPIC);
  private_nh.getParam("output_log", _output_log);
  if(_output_log == true){
	  char buffer[80];
	  std::time_t now = std::time(NULL);
	  std::tm *pnow = std::localtime(&now);
	  std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow);
	  filename = "voxel_grid_filter_" + std::string(buffer) + ".csv";
	  ofs.open(filename.c_str(), std::ios::app);
  }

  // Publishers
  filtered_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/filtered_points", 10);
  points_downsampler_info_pub = nh.advertise<points_downsampler::PointsDownsamplerInfo>("/points_downsampler_info", 1000);

  // Subscribers
  ros::Subscriber config_sub = nh.subscribe("config/voxel_grid_filter", 10, config_callback);
  ros::Subscriber scan_sub = nh.subscribe(POINTS_TOPIC, 10, scan_callback);

  ros::spin();

  return 0;
}

WmObjectCapture::WmObjectCapture(ros::NodeHandle private_nh_)
: nh_(),
  objectFrameId_("/object"),
  cameraFrameId_("/camera_link"),
  cameraTopicId_("/camera/depth_registered/points"),
  object_(new pcl::PointCloud<pcl::PointXYZRGB>),
  object_octree_(new pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB>(128.0f)),
  max_z_(0.5),
  making_object_(true),
  unique_shot_(false)
{
	ros::NodeHandle private_nh(private_nh_);
	private_nh.param("objectFrameId", objectFrameId_, objectFrameId_);
	private_nh.param("cameraFrameId", cameraFrameId_, cameraFrameId_);
	private_nh.param("cameraTopicId", cameraTopicId_, cameraTopicId_);
	private_nh.param("pointcloud_max_z", max_z_,max_z_);
	private_nh.param("make_object", making_object_,making_object_);
	private_nh.param("unique_shot", unique_shot_,unique_shot_);

	pointCloudSub_ = new message_filters::Subscriber<sensor_msgs::PointCloud2> (nh_, cameraTopicId_, 5);
	tfPointCloudSub_ = new tf::MessageFilter<sensor_msgs::PointCloud2> (*pointCloudSub_, tfListener_, cameraFrameId_, 5);
	tfPointCloudSub_->registerCallback(boost::bind(&WmObjectCapture::insertCloudCallback, this, _1));

	objectService_ = nh_.advertiseService("object", &WmObjectCapture::objectSrv, this);
	objectPub_ =  nh_.advertise<sensor_msgs::PointCloud2>("/object", 1, true);

	object_octree_->setInputCloud(object_);

	initMarkers();
}

void StepbackRecovery::initialize(std::string name, tf::TransformListener* tf,
                                  costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap) {
    if(!initialized_) {
        name_ = name;
        tf_ = tf;
        global_costmap_ = global_costmap;
        local_costmap_ = local_costmap;

        //get some parameters from the parameter server
        ros::NodeHandle private_nh("~/" + name_);
        ros::NodeHandle blp_nh("~/TrajectoryPlannerROS");

        //Stepping back 10 cm per default
        private_nh.param("stepback_length", stepback_length_, 0.1);
        private_nh.param("frequency", frequency_, 10.0);

        blp_nh.param("acc_lim_th", acc_lim_th_, 3.2);
        blp_nh.param("max_rotational_vel", max_rotational_vel_, 1.0);
        blp_nh.param("min_in_place_rotational_vel", min_rotational_vel_, 0.4);
        blp_nh.param("yaw_goal_tolerance", tolerance_, 0.10);

        world_model_ = new base_local_planner::CostmapModel(*local_costmap_->getCostmap());

        initialized_ = true;
    }
    else {
        ROS_ERROR("You should not call initialize twice on this object, doing nothing");
    }
}

ScanManipulation::ScanManipulation(ros::Rate &publish_retry_rate) :
    publish_retry_rate_(publish_retry_rate)
{
    nh_ = ros::NodeHandle("");
    laser_sub_ = nh_.subscribe<sensor_msgs::LaserScan>("laser_in", 100,
                                                                    &ScanManipulation::laserCB, this);
    pointcloud_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>("pointcloud_in", 10,
                                                                    &ScanManipulation::pointcloudCB, this);

    laser_pub_ = nh_.advertise<sensor_msgs::LaserScan>("laser_out", 100);
    pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("pointcloud_out", 10);

    ros::NodeHandle private_nh("~");
    reconf_server_.reset(new ReconfigureServer(private_nh));
    reconf_server_->setCallback(boost::bind(&ScanManipulation::configCB, this, _1, _2));

    private_nh.param("frame_id_new", frame_id_new_, frame_id_new_);
    private_nh.param("frame_id_to_replace", frame_id_to_replace_, frame_id_to_replace_);
    double msg_delay_in_milliseconds, time_offset_in_milliseconds;
    private_nh.param("msg_delay_milliseconds", msg_delay_in_milliseconds, msg_delay_.toSec() * 1000);
    private_nh.param("time_offset_milliseconds", time_offset_in_milliseconds, time_offset_.toSec() * 1000);
    msg_delay_ = ros::Duration(msg_delay_in_milliseconds/1000);
    time_offset_ = ros::Duration(time_offset_in_milliseconds/1000);
    if (private_nh.hasParam("publish_retry_rate"))
    {
        double retry_rate;
        private_nh.param("publish_retry_rate", retry_rate, 100.0);
        publish_retry_rate_ = ros::Rate(retry_rate);
    }
}

    AppLoader::AppLoader()
    {
        ros::NodeHandle private_nh("~");
        ros::NodeHandle nh;

        rtp_manager_destroy_ = false;

        //get app name and type
        private_nh.param("app_name", app_name_, std::string("app_demo"));
        private_nh.param("app_type", app_type_, std::string("micros_swarm/AppDemo"));

        ros::ServiceClient client = nh.serviceClient<app_loader::AppLoad>("app_loader_load_app");
        app_loader::AppLoad srv;
        srv.request.name = app_name_;
        srv.request.type = app_type_;
        if (client.call(srv))
        {
            ROS_INFO("[AppLoader]: App %s loaded successfully.", app_name_.c_str());
        }
        else
        {
            ROS_ERROR("[AppLoader]: Failed to load App %s.", app_name_.c_str());
        }

        //when the rtp manager was destroyed, automatically unload the apps
        std::string topic_name = "runtime_core_destroy_" + app_name_;
        rtp_manager_destroy_srv_ = nh.advertiseService(topic_name, &AppLoader::rtpManagerDestroyCB, this);
    }

int main(int argc, char **argv)
{
  ros::init(argc, argv, "image_proc");

  // Check for common user errors
  if (ros::names::remap("camera") != "camera")
  {
    ROS_WARN("Remapping 'camera' has no effect! Start image_proc in the "
             "camera namespace instead.\nExample command-line usage:\n"
             "\t$ ROS_NAMESPACE=%s rosrun image_proc image_proc",
             ros::names::remap("camera").c_str());
  }
  if (ros::this_node::getNamespace() == "/")
  {
    ROS_WARN("Started in the global namespace! This is probably wrong. Start image_proc "
             "in the camera namespace.\nExample command-line usage:\n"
             "\t$ ROS_NAMESPACE=my_camera rosrun image_proc image_proc");
  }

  // Shared parameters to be propagated to nodelet private namespaces
  ros::NodeHandle private_nh("~");
  XmlRpc::XmlRpcValue shared_params;
  int queue_size;
  if (private_nh.getParam("queue_size", queue_size))
    shared_params["queue_size"] = queue_size;

  nodelet::Loader manager(false); // Don't bring up the manager ROS API
  nodelet::M_string remappings;
  nodelet::V_string my_argv;

  // Debayer nodelet, image_raw -> image_mono, image_color
  std::string debayer_name = ros::this_node::getName() + "_debayer";
  manager.load(debayer_name, "image_proc/debayer", remappings, my_argv);

  // Rectify nodelet, image_mono -> image_rect
  std::string rectify_mono_name = ros::this_node::getName() + "_rectify_mono";
  if (shared_params.valid())
    ros::param::set(rectify_mono_name, shared_params);
  manager.load(rectify_mono_name, "image_proc/rectify", remappings, my_argv);

  // Rectify nodelet, image_color -> image_rect_color
  // NOTE: Explicitly resolve any global remappings here, so they don't get hidden.
  remappings["image_mono"] = ros::names::resolve("image_color");
  remappings["image_rect"] = ros::names::resolve("image_rect_color");
  std::string rectify_color_name = ros::this_node::getName() + "_rectify_color";
  if (shared_params.valid())
    ros::param::set(rectify_color_name, shared_params);
  manager.load(rectify_color_name, "image_proc/rectify", remappings, my_argv);

  // Check for only the original camera topics
  ros::V_string topics;
  topics.push_back(ros::names::resolve("image_raw"));
  topics.push_back(ros::names::resolve("camera_info"));
  image_proc::AdvertisementChecker check_inputs(ros::NodeHandle(), ros::this_node::getName());
  check_inputs.start(topics, 60.0);
  
  ros::spin();
  return 0;
}

NavfnWithCostmap::NavfnWithCostmap(string name, Costmap2DROS* cmap) : 
  NavfnROS(name, cmap)
{
  ros::NodeHandle private_nh("~");
  cmap_ = cmap;
  make_plan_service_ = private_nh.advertiseService("make_plan", &NavfnWithCostmap::makePlanService, this);
  pose_sub_ = private_nh.subscribe<rm::PoseStamped>("goal", 1, &NavfnWithCostmap::poseCallback, this);
}

  DWAPlanner::DWAPlanner(std::string name, base_local_planner::LocalPlannerUtil *planner_util) :
      planner_util_(planner_util),
      obstacle_costs_(planner_util->getCostmap()),
      path_costs_(planner_util->getCostmap()),
      goal_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
      goal_front_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
      alignment_costs_(planner_util->getCostmap())
  {
    ros::NodeHandle private_nh("~/" + name);

    goal_front_costs_.setStopOnFailure( false );
    alignment_costs_.setStopOnFailure( false );

    //Assuming this planner is being run within the navigation stack, we can
    //just do an upward search for the frequency at which its being run. This
    //also allows the frequency to be overwritten locally.
    std::string controller_frequency_param_name;
    if(!private_nh.searchParam("controller_frequency", controller_frequency_param_name)) {
      sim_period_ = 0.05;
    } else {
      double controller_frequency = 0;
      private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
      if(controller_frequency > 0) {
        sim_period_ = 1.0 / controller_frequency;
      } else {
        ROS_WARN("A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
        sim_period_ = 0.05;
      }
    }
    ROS_INFO("Sim period is set to %.2f", sim_period_);

    oscillation_costs_.resetOscillationFlags();

    private_nh.param("publish_cost_grid_pc", publish_cost_grid_pc_, false);
    map_viz_.initialize(name, boost::bind(&DWAPlanner::getCellCosts, this, _1, _2, _3, _4, _5, _6));

    std::string frame_id;
    private_nh.param("global_frame_id", frame_id, std::string("odom"));

    traj_cloud_.header.frame_id = frame_id;
    traj_cloud_pub_.advertise(private_nh, "trajectory_cloud", 1);

    // set up all the cost functions that will be applied in order
    // (any function returning negative values will abort scoring, so the order can improve performance)
    std::vector<base_local_planner::TrajectoryCostFunction*> critics;
    critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1)
    critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
    critics.push_back(&goal_front_costs_); // prefers trajectories that make the nose go towards (local) nose goal
    critics.push_back(&alignment_costs_); // prefers trajectories that keep the robot nose on nose path
    critics.push_back(&path_costs_); // prefers trajectories on global path
    critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation

    // trajectory generators
    std::vector<base_local_planner::TrajectorySampleGenerator*> generator_list;
    generator_list.push_back(&generator_);

    scored_sampling_planner_ = base_local_planner::SimpleScoredSamplingPlanner(generator_list, critics);
  }

int main(int ac, char **av)
{
  ros::init(ac, av, ros::this_node::getName());
  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");
  mrover::StereoViewer handler(nh, private_nh);
  ros::spin();
  return 0;
}

int main(int argc, char* argv[])
{
  ros::init(argc, argv, "calibration_publisher");
  ros::NodeHandle n;
  ros::NodeHandle private_nh("~");

  if (!private_nh.getParam("register_lidar2camera_tf", isRegister_tf)) {
    isRegister_tf = true;
  }

  if (!private_nh.getParam("publish_extrinsic_mat", isPublish_extrinsic)) {
    isPublish_extrinsic = true; /* publish extrinsic_mat in default */
  }

  if (!private_nh.getParam("publish_camera_info", isPublish_cameraInfo)) {
    isPublish_extrinsic = false; /* doesn't publish camera_info in default */
  }

  if (argc < 2)
    {
      std::cout << "Usage: calibration_publisher <calibration-file>." << std::endl;
      return -1;
    }

  cv::FileStorage fs(argv[1], cv::FileStorage::READ);
  if (!fs.isOpened())
    {
      std::cout << "Cannot open " << argv[1] << std::endl;
      return -1;
    }

  fs["CameraExtrinsicMat"] >> CameraExtrinsicMat;
  fs["CameraMat"] >> CameraMat;
  fs["DistCoeff"] >> DistCoeff;
  fs["ImageSize"] >> ImageSize;

  ros::Subscriber image_sub;
  if (isRegister_tf) {
    image_sub = n.subscribe("/image_raw", 10, image_raw_cb);
  }

  if (isPublish_cameraInfo) {
    camera_info_pub = n.advertise<sensor_msgs::CameraInfo>("/camera/camera_info", 10, true);
    cameraInfo_sender(CameraMat, DistCoeff, ImageSize);
  }

  if (isPublish_extrinsic) {
    projection_matrix_pub = n.advertise<calibration_camera_lidar::projection_matrix>("/projection_matrix", 10, true);
    projectionMatrix_sender(CameraExtrinsicMat);
  }

  ros::spin();

  return 0;

}