C++ NodeHandlePtr::subscribe方法代码示例

int main(int argc, char** argv) {
    ros::init(argc, argv, "batteryInfo");
    nodeHandle = ros::NodeHandlePtr(new ros::NodeHandle("~"));
    getNamePrefix();
    ROS_INFO("Subscribing to /diagnostics ...");
    ros::Subscriber diagnosticsSub = nodeHandle->subscribe("/diagnostics", 1, diagnosticsCallback);
    ros::spin();
    return 0;
}

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

    _handle = ros::NodeHandlePtr(new ros::NodeHandle(""));

    _odom.header.frame_id = "map";
    _x = _y = 0;
    _theta = 0;
    _correct_theta = false;
    _correct_lateral = false;
    _iteration_theta = 0; _iteration_lateral = 0;
    _turn_accum = 0;
    _heading = 0;

    _see_front_plane = false;

    _ringbuffer.reserve(_ringbuffer_max_size);

    ros::Subscriber sub_enc = _handle->subscribe("/arduino/encoders",10,callback_encoders);
    ros::Subscriber sub_turn_angle = _handle->subscribe("/controller/turn/angle",10,callback_turn_angle);
    ros::Subscriber sub_turn_done = _handle->subscribe("/controller/turn/done",10,callback_turn_done);
    ros::Subscriber sub_ir = _handle->subscribe("/perception/ir/distance",10,callback_ir);
    ros::Subscriber sub_planes = _handle->subscribe("/vision/obstacles/planes",10,callback_planes);
    ros::Subscriber sub_crash = _handle->subscribe("/perception/imu/peak", 10, callback_crash);

    _pub_odom = _handle->advertise<nav_msgs::Odometry>("/pose/odometry/",10,(ros::SubscriberStatusCallback)connect_odometry_callback);
    _pub_viz = _handle->advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
    _pub_compass = _handle->advertise<std_msgs::Int8>("/pose/compass", 10, (ros::SubscriberStatusCallback)connect_compass_callback);

    _srv_raycast = _handle->serviceClient<navigation_msgs::Raycast>("/mapping/raycast");

    ros::spin();

    return 0;
}

void subscribe() {
    subscriber = nodeHandle->subscribe(subscriberTopic, subscriberQueueSize,
                                       &callback, subscriberTransportHints);
}

  int init(PhantomState *s)
  {
    if(!s)
    {
      ROS_FATAL("Internal error. PhantomState is NULL.");
      return -1;
    }

    node_ = ros::NodeHandlePtr(new ros::NodeHandle);
    pnode_ = ros::NodeHandlePtr(new ros::NodeHandle("~"));
    pnode_->param(std::string("tf_prefix"), tf_prefix_, std::string(""));

    // Vertical displacement from base_link to link_0. Defaults to Omni offset.
    pnode_->param(std::string("table_offset"), table_offset_, .135);

    // Force feedback damping coefficient
    pnode_->param(std::string("damping_k"), damping_k_, 0.001);

    // On startup device will generate forces to hold end-effector at origin.
    pnode_->param(std::string("locked"), locked_, false);

    // Check calibration status on start up and calibrate if necessary.
    pnode_->param(std::string("calibrate"), calibrate_, false);

    //Frame attached to the base of the phantom (NAME/base_link)
    base_link_name_ = "base_link";

    //Publish on NAME/pose
    std::string pose_topic_name = "pose";
    pose_publisher_ = node_->advertise<geometry_msgs::PoseStamped>(pose_topic_name, 100);

    //Publish button state on NAME/button
    std::string button_topic = "button";
    button_publisher_ = node_->advertise<sensable_phantom::PhantomButtonEvent>(button_topic, 100);

    //Subscribe to NAME/force_feedback
    std::string force_feedback_topic = "force_feedback";
    wrench_sub_ = node_->subscribe(force_feedback_topic, 100, &PhantomROS::wrench_callback, this);

    //Frame of force feedback (NAME/sensable_origin)
    sensable_frame_name_ = "sensable_origin";

    for (int i = 0; i < 7; i++)
    {
      std::ostringstream stream1;
      stream1 << "link_" << i;
      link_names_[i] = std::string(stream1.str());
    }

    state_ = s;
    state_->buttons[0] = 0;
    state_->buttons[1] = 0;
    state_->buttons_prev[0] = 0;
    state_->buttons_prev[1] = 0;
    hduVector3Dd zeros(0, 0, 0);
    state_->velocity = zeros;
    state_->inp_vel1 = zeros; //3x1 history of velocity
    state_->inp_vel2 = zeros; //3x1 history of velocity
    state_->inp_vel3 = zeros; //3x1 history of velocity
    state_->out_vel1 = zeros; //3x1 history of velocity
    state_->out_vel2 = zeros; //3x1 history of velocity
    state_->out_vel3 = zeros; //3x1 history of velocity
    state_->pos_hist1 = zeros; //3x1 history of position
    state_->pos_hist2 = zeros; //3x1 history of position
    state_->lock = locked_;
    state_->lock_pos = zeros;
    state_->hd_cur_transform = hduMatrix::createTranslation(0, 0, 0);

    return 0;
  }