C++ NodeHandle类代码示例

MainWindow::MainWindow(NodeHandle& nh, QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    startTimer(1000);

    // Create model
    guides_model_ = new GuidesModel(nh,this);

    ui->listView->setModel(guides_model_);

    // Add additional feature so that
    // we can manually modify the data in ListView
    // It may be triggered by hitting any key or double-click etc.
    ui->listView->setEditTriggers(QAbstractItemView::AnyKeyPressed |
                                  QAbstractItemView::DoubleClicked );

    sub_ = new Subscriber(nh.subscribe("rosout", 1000, &MainWindow::loggerCallback, this));

    spinner_ptr_ = new AsyncSpinner(1); // Use one thread to keep the ros magic alive
    spinner_ptr_->start();

    // TextBox is read only
    ui->consoleText->setReadOnly(true);
    ui->consoleText->setTextInteractionFlags(0); //0 == Qt::TextInteractionFlag::NoTextInteraction

    //QColor color = QColorDialog::getColor(Qt::white,this); // in here your color pallete will open..

    QPalette p = ui->consoleText->palette(); // define pallete for textEdit..
    p.setColor(QPalette::Base, Qt::black); // set color "Red" for textedit base
    //p.setColor(QPalette::Text, Qt::white); // set text color which is selected from color pallete
    ui->consoleText->setPalette(p); // change textedit palette

    // Explanation of this hack:
    // Qt and Ros don't like each other, or better, their respective thread workers don't like each other.
    // You should not handle qt objects with ROS threads.
    // So we use the ros callback to trigger the qt callback :)
    QObject::connect(this, SIGNAL(requestUpdateConsole(const QString&,int)),
                     this, SLOT(updateConsole(const QString&,int)));

    // SlideBar
    slidebar_res_ = 100;
    ui->mergeSlider->setMinimum(0);
    ui->mergeSlider->setMaximum(slidebar_res_);

    //Refresh on start
    on_refreshButton_clicked(); // click a virtual button! :D
}

int main(int argc, char** argv)
{	
   ROS_INFO("Starting %s ...", NODE_NAME.c_str());	
   init(argc, argv, NODE_NAME);
   NodeHandle n;
   Subscriber dataSub = n.subscribe(SUBSCRIBE_TOPIC, MSG_QUEUE_SIZE, irCallBack);
   dataPub = n.advertise<geometry_msgs::Twist>(PUBLISH_TOPIC, MSG_QUEUE_SIZE);

  if(argc != 4){
    std::cout << "Usage: Nodename <FieldView> <MaxDistance> <maxSteering>" << std::endl;
    return -1;
  }

  FIELD = atoi(argv[1]);
  MAX_DISTANCE = atoi(argv[2]);
  MAX_STEERING = atoi(argv[3]);

   while (ok())
       spin();

   ROS_INFO("Shutting down %s!", NODE_NAME.c_str());
   delete lidar_ptr;
}

void
Dbtux::nodePushUpScans(NodeHandle& node, unsigned pos)
{
  const unsigned occup = node.getOccup();
  ScanOpPtr scanPtr;
  scanPtr.i = node.getNodeScan();
  do {
    jam();
    c_scanOpPool.getPtr(scanPtr);
    TreePos& scanPos = scanPtr.p->m_scanPos;
    ndbrequire(scanPos.m_loc == node.m_loc && scanPos.m_pos < occup);
    if (scanPos.m_pos >= pos) {
      jam();
#ifdef VM_TRACE
      if (debugFlags & DebugScan) {
        debugOut << "Fix scan " << scanPtr.i << " " << *scanPtr.p << endl;
        debugOut << "At pushUp pos=" << pos << " " << node << endl;
      }
#endif
      scanPos.m_pos++;
    }
    scanPtr.i = scanPtr.p->m_nodeScan;
  } while (scanPtr.i != RNIL);
}

/*
 * Set handle to point to new node.  Uses a pre-allocated node.
 */
void
Dbtux::insertNode(NodeHandle& node)
{
  Frag& frag = node.m_frag;
  // use up pre-allocated node
  selectNode(node, frag.m_freeLoc);
  frag.m_freeLoc = NullTupLoc;
  new (node.m_node) TreeNode();
#ifdef VM_TRACE
  TreeHead& tree = frag.m_tree;
  memset(node.getPref(), DataFillByte, tree.m_prefSize << 2);
  TreeEnt* entList = tree.getEntList(node.m_node);
  memset(entList, NodeFillByte, tree.m_maxOccup * (TreeEntSize << 2));
#endif
}

/*
 * Check if a scan is linked to this node.  Only for ndbrequire.
 */
bool
Dbtux::islinkScan(NodeHandle& node, ScanOpPtr scanPtr)
{
  ScanOpPtr currPtr;
  currPtr.i = node.getNodeScan();
  while (currPtr.i != RNIL) {
    jam();
    c_scanOpPool.getPtr(currPtr);
    if (currPtr.i == scanPtr.i) {
      jam();
      return true;
    }
    currPtr.i = currPtr.p->m_nodeScan;
  }
  return false;
}

int main(int argc, char **argv)
{
	setFreqs(0,0);

	init(argc,argv,"motors");
	NodeHandle n;

	std::string onoff;
	if(argc > 1)
		onoff = argv[1];

	setPower(onoff == "on");

	signal(SIGINT, onSigint);

	last_cmdvel = Time::now();
	cur_time = Time::now();
	send_time = Time::now();

	ServiceServer srv_on = n.advertiseService("motor_on", callbackOn);
	ServiceServer srv_off = n.advertiseService("motor_off", callbackOff);
	ServiceServer srv_tm = n.advertiseService("timed_motion", callbackTimedMotion); 
	
	Subscriber sub_raw = n.subscribe("motor_raw", 10, callbackRaw);
	Subscriber sub_cmdvel = n.subscribe("cmd_vel", 10, callbackCmdvel);
	Subscriber sub_9axis = n.subscribe("/imu/data_raw", 10, callback9Axis);

	Publisher pub_odom = n.advertise<nav_msgs::Odometry>("odom", 10);
	odom_x = 0.0;
	odom_y = 0.0;
	odom_theta = 0.0;

	send_time = Time::now();

	Rate loop_rate(10);
	while(ok()){
		if(in_cmdvel and Time::now().toSec() - last_cmdvel.toSec() >= 1.0)
			setFreqs(0,0);

		pub_odom.publish(send_odom());
		spinOnce();
		loop_rate.sleep();
	}
	
	exit(0);
}

/*
 * Delete existing node.  Make it the pre-allocated free node if there
 * is none.  Otherwise return it to fragment's free list.
 */
void
Dbtux::deleteNode(NodeHandle& node)
{
  Frag& frag = node.m_frag;
  ndbrequire(node.getOccup() == 0);
  if (frag.m_freeLoc == NullTupLoc)
  {
    jam();
    frag.m_freeLoc = node.m_loc;
    // invalidate the handle
    node.m_loc = NullTupLoc;
    node.m_node = 0;
  }
  else
  {
    jam();
    freeNode(node);
  }
}

void getAllPointsRecursiveAsync(MegaTree &tree, NodeHandle& node, double resolution,
                                std::vector<double> &results, std::vector<double> &colors, megatree::List<NodeHandle*>& nodes_to_load)
{
  //printf("Get all points recursive for node %s with frame offset %f %f %f\n",
  //       node.getId().toString().c_str(), frame_offset[0], frame_offset[1], frame_offset[2]);

  assert(!node.isEmpty());

  // we hit a leaf
  if (node.isLeaf() || node.getNodeGeometry().getSize() <= resolution) 
  {
    double point[3];
    node.getPoint(point);
    results.push_back(point[0]);
    results.push_back(point[1]);
    results.push_back(point[2]);

    double color[3];
    node.getColor(color);
    colors.push_back(color[0]);
    colors.push_back(color[1]);
    colors.push_back(color[2]);
  }

  // need to descend further
  else 
  {
    for(int i = 0; i < 8; ++i)
    {
      if(node.hasChild(i))
      {
        megatree::NodeHandle* child = new megatree::NodeHandle;
        tree.getChildNode(node, i, *child);

        //if the child isn't valid, we'll push it onto our nodes to load list
        if(!child->isValid())
        {
          nodes_to_load.push_back(child);
          process_queue_size++;
        }
        else
        {
          getAllPointsRecursiveAsync(tree, *child, resolution, results, colors, nodes_to_load);
          tree.releaseNode(*child);
          delete child;
        }
      }
    }
  }
}

MechanismManagerServer::MechanismManagerServer(MechanismManagerInterface* mm_interface, NodeHandle& nh)
    : spinner_ptr_(NULL)
{
    assert(mm_interface!=NULL);

    mm_interface_ = mm_interface;

    if(master::check())
    {
        ss_ = nh.advertiseService("mechanism_manager_interaction",
                                  &MechanismManagerServer::CallBack, this);

        spinner_ptr_ = new AsyncSpinner(1); // Use one thread to keep the ros magic alive
        spinner_ptr_->start();
    }
    else
    {
        std::string err("Can not start the action server, did you start roscore?");
        throw std::runtime_error(err);
    }
}

int main(int argc, char** argv) {
	init(argc, argv, "JointUpdateNode");
	NodeHandle nh;

	// get the filenames
	nh.param(string("joint_offsets_filename_suffix"), jointOffsetsFilename,
			string("calibration_joint_offsets.xacro"));
	nh.param(string("camera_transform_filename_suffix"),
			cameraTransformFilename,
			string("calibration_camera_transform.xacro"));
	nh.param(string("urdf_filename_suffix"), urdfFilename,
			string("robot_model_calibrated.xml"));
	nh.param(string("marker_transforms_filename_suffix"),
			markerTransformsFilename,
			string("calibration_marker_transformations.xacro"));
	nh.param(string("camera_intrnsics_filename"), cameraIntrnsicsFilename,
			string("nao_bottom_640x480.yaml"));

	ModelLoader modelLoader;
	modelLoader.initializeFromRos();

	urdf::Model model;
	modelLoader.getUrdfModel(model);

	// get the robot name and prepend to the filenames
	string filePrefix = model.getName();
	jointOffsetsFilename = filePrefix + "_" + jointOffsetsFilename;
	cameraTransformFilename = filePrefix + "_" + cameraTransformFilename;
	urdfFilename = filePrefix + "_" + urdfFilename;
	markerTransformsFilename = filePrefix + "_" + markerTransformsFilename;

    Subscriber sub = nh.subscribe("/onlineCalibration/calibration_result",
			1, resultCb);
	spin();
	return 0;
}

int main(int argc, char* argv[]) {
    gengetopt_args_info args;
    cmdline_parser(argc,argv,&args);
    float coeffs[6];
    coeffs[0] = args.a1_arg;
    coeffs[1] = args.a2_arg;
    coeffs[2] = args.a3_arg;
    coeffs[3] = args.a4_arg;
    coeffs[4] = args.a5_arg;
    coeffs[5] = args.a6_arg;
    sigmaHit = args.sigma_arg;
    angleMin = args.angleMin_arg;
    angleMax = args.angleMax_arg;
    angleMin *= (M_PI/180.0);
    angleMax *= (M_PI/180.0);
    angleIncrement = (angleMax-angleMin)/args.numBeams_arg;
    numBeams = args.numBeams_arg;
    float trueDistances[numBeams];

    float noisyDistances[numBeams];

    float commandReal[2];

    init(argc, argv, "Odom");
    nav_msgs::Odometry msg;
    sensor_msgs::LaserScan msg2;
    NodeHandle n;
    Publisher pub = n.advertise<nav_msgs::Odometry>("odom", 1);
    Publisher las = n.advertise<sensor_msgs::LaserScan>("/scan", 1);
    Subscriber sub = n.subscribe("navi", 1000, cmmdUpdate);  // update the command velocities
    Subscriber rst = n.subscribe("/mobile_base/commands/reset_odometry", 1000, reset);
    ros::Duration(1.3).sleep();
    ros::Rate loop_rate(10);
    while (ros::ok()) {
        sampleMotionModel(command, commandReal, pose, coeffs, 0.1);
        yawToQuaternion(pose[2], quaternion);
        msg.pose.pose.position.x = pose[0];
        msg.pose.pose.position.y = pose[1];
        msg.pose.pose.orientation.w = quaternion[0];
        msg.pose.pose.orientation.x = quaternion[1];
        msg.pose.pose.orientation.y = quaternion[2];
        msg.pose.pose.orientation.z = quaternion[3];
        msg.twist.twist.linear.x = commandReal[0];
        msg.twist.twist.angular.z = commandReal[1];

        // Laser scanning section
        calcTrueDistance(trueDistances, numBeams, angleIncrement);
        calcNoisyDistance(noisyDistances, trueDistances, sigmaHit, numBeams);
        msg2.angle_max = angleMax;
        msg2.angle_min = angleMin;
        msg2.time_increment = 0;
        msg2.angle_increment = angleIncrement;
        msg2.scan_time = 0.1;
        msg2.range_min = 0;
        msg2.range_max = maxRange;
        msg2.ranges.resize(numBeams);
        for (int i = 0; i < numBeams; i++) {
            msg2.ranges[i] = noisyDistances[i];
        }
        ros::spinOnce();
        pub.publish(msg);
        las.publish(msg2);
        loop_rate.sleep();
    }
    return 0;
}

int main(int argc, char **argv)
{
    srand(time(0));
    ros::init(argc ,argv, "ROS_Publisher");
    NodeHandle node;
    image_transport::ImageTransport it(node);

    std::cout << "Starting image load" << endl;
    loadImages();
    cout << "Done Loading Images" << endl;
    getchar();

    mcl_data_subscriber = node.subscribe(mcl_data_publisher_name, 4, MyDataCallback);

    time_t temptime = time(0);
    std::cout << "Waiting for Handshake from Program .." << std::endl;
    while(!handshake_recieved && (time(0) - temptime) < 20)
    {
        ros::spinOnce();
    }
    if(handshake_recieved)
        std::cout << "Handshake recieved" << std::endl;
    else
    {
        std::cout << "No handshake recieved";
        return -1;
    }
    
    movement_publisher = node.advertise<std_msgs::String>("ROBOT_MOVEMENT_PUBLISHER", 4);

    data_publisher = it.advertise(publish_image_data_under, 4, true);


    char key = 'k';
    // namedWindow("Robot Image");
    // namedWindow("Top Match");

    while(ros::ok() && key != 'q')
    {
        ros::spinOnce();
        int i = rand()%image_names.size();
        cv_bridge::CvImage out_msg;
        ros::Time scan_time = ros::Time::now();
        out_msg.header.stamp = scan_time;
        out_msg.header.frame_id = "robot_image";
        out_msg.encoding = sensor_msgs::image_encodings::BGR8;
        out_msg.image = image_list[current_image];
        
        if(!out_msg.image.empty())
            data_publisher.publish(out_msg.toImageMsg());
        ros::spinOnce();

        std::cout << "hererer" << std::endl;
        // imshow("Robot Image", image_list[current_image]);
        // imshow("Top Match", BestGuessImage);

        // key = cv::waitKsey(2);
        if(key == ' ')
            current_image++;
        if(current_image == image_list.size())
            current_image = 0;
        //ros::Duration(0.1).sleep();
    }

    std_msgs::String msg;
    stringstream ss;
    ss << killflag << "_";
    msg.data = ss.str();
    movement_publisher.publish(msg);

    ros::spinOnce();
    ros::spinOnce();

    ros::Duration(0.1).sleep();
    ros::spinOnce();
    ros::shutdown();

}

FeatherNodesFixture::~FeatherNodesFixture() {
    FeatherNodeFactory::instance().reset();
}

void FeatherNodesFixture::clearAuxNodes() {
    FeatherNodeFactory::instance().clearAuxNodes();
}

void FeatherNodesFixture::setContextForAuxNodes() {
    FeatherNodeFactory::instance().setContextForAuxNodes(globalContext_);
}

TEST_CASE_METHOD(FeatherNodesFixture, "Testing Feather expressions generation") {
    rc::prop("Test expected type", [=](Nest::TypeWithStorage expectedType) {
        clearAuxNodes();
        NodeHandle node = *FeatherNodeFactory::instance().arbExp(expectedType);
        setContextForAuxNodes();
        node.setContext(globalContext_);
        auto t = node.computeType();
        REQUIRE(t);
        if (expectedType.kind() != Feather_getFunctionTypeKind())
            REQUIRE(sameTypeIgnoreMode(t, expectedType));
    });
}

TEST_CASE_METHOD(FeatherNodesFixture, "Testing Feather::Nop node") {
    SECTION("Has type Void") {
        clearAuxNodes();
        auto nop = Nop::create(createLocation());
        setContextForAuxNodes();
        nop.setContext(globalContext_);

NodeHandle FileEntry::create_hidden_node( NodeHandle n ) {
    NodeHandle r = StringEntry::create_hidden_node(n);
    r->add_attribute( default_extension );
    return r;
}

void getParameters(NodeHandle &nh)
{
	////////////////////////////////////////////////////////////
	// Get robot description
	////////////////////////////////////////////////////////////
	string description;
	if(!nh.getParam("/robot_description", description))
	{
		ROS_ERROR("Parameter not set: /robot_description");
		exit(-1);
	}

	////////////////////////////////////////////////////////////
	// Create robot model
	////////////////////////////////////////////////////////////
	urdf::Model model;
	if(!model.initString(description))
	{
		ROS_ERROR("Could not initialize robot model");
		exit(-1);
	}

	////////////////////////////////////////////////////////////
	// Get tip link name and verify it exists in the model
	////////////////////////////////////////////////////////////
	string tip_link_name;
	if(!nh.getParam("tip_link_name", tip_link_name))
	{
		ROS_ERROR("Parameter not set: %s/tip_link_name", nh.getNamespace().c_str());
		exit(-1);
	}
	if(model.links_.find(tip_link_name) == model.links_.end())
	{
		ROS_ERROR("The link specified in %s/tip_link_name does not exist", nh.getNamespace().c_str());
		exit(-1);
	}

	////////////////////////////////////////////////////////////
	// Get revolute joints
	////////////////////////////////////////////////////////////
	vector<boost::shared_ptr<urdf::Joint> > model_joints;

	for(boost::shared_ptr<const urdf::Link> current_link = model.getLink(tip_link_name); current_link->parent_joint != NULL; current_link = current_link->getParent())
	{
		if(current_link->parent_joint->type == urdf::Joint::REVOLUTE)
		{
			model_joints.insert(model_joints.begin(), current_link->parent_joint);
		}
	}

	if(model_joints.size() != NUM_SERVOS)
	{
		ROS_ERROR("The robot model must have %d revolute joints, found %d", NUM_SERVOS, (int)model_joints.size());
		exit(-1);
	}

	////////////////////////////////////////////////////////////
	// Configure servo limits
	////////////////////////////////////////////////////////////
	for(int joint_i = 0; joint_i < NUM_SERVOS; joint_i++)
	{
		float lowerLimit = model_joints[joint_i]->limits->lower;
		float upperLimit = model_joints[joint_i]->limits->upper;

		RSSerialController::setServoLimits(joint_i, lowerLimit, upperLimit);
	}

	////////////////////////////////////////////////////////////
	// Store joint names
	////////////////////////////////////////////////////////////
	for(int joint_i = 0; joint_i < NUM_SERVOS; joint_i++)
	{
		joint_names.push_back(model_joints[joint_i]->name);
	}

	////////////////////////////////////////////////////////////
	// Create KDL tree
	////////////////////////////////////////////////////////////
	if(!kdl_parser::treeFromString(description, tree))
	{
		ROS_ERROR("Failed to construct kdl tree");
		exit(-1);
	}

	if(!tree.getChain(model.getRoot()->name, tip_link_name, chain))
	{
		ROS_ERROR("Failed to make chain from tree");
		exit(-1);
	}
}