Exemplos de param_nh em C++ (Cpp)

Exemplo n.º 1

Arquivo: quadrotor_hardware_gazebo.cpp Projeto: ncos/mipt-airdrone

bool QuadrotorHardwareSim::initSim(
    const std::string& robot_namespace,
    ros::NodeHandle model_nh,
    gazebo::physics::ModelPtr parent_model,
    const urdf::Model *const urdf_model,
    std::vector<transmission_interface::TransmissionInfo> transmissions)
{
  ros::NodeHandle param_nh(model_nh, "controller");

  // store parent model pointer
  model_ = parent_model;
  link_ = model_->GetLink();
  physics_ = model_->GetWorld()->GetPhysicsEngine();

  // subscribe state
  std::string state_topic;
  param_nh.getParam("state_topic", state_topic);
  if (!state_topic.empty()) {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>(state_topic, 1, boost::bind(&QuadrotorHardwareSim::stateCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
    subscriber_state_ = model_nh.subscribe(ops);

    gzlog << "[hector_quadrotor_controller_gazebo] Using topic '" << subscriber_state_.getTopic() << "' as state input for control" << std::endl;
  } else {
    gzlog << "[hector_quadrotor_controller_gazebo] Using ground truth from Gazebo as state input for control" << std::endl;
  }

  // subscribe imu
  std::string imu_topic;
  param_nh.getParam("imu_topic", imu_topic);
  if (!imu_topic.empty()) {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>(imu_topic, 1, boost::bind(&QuadrotorHardwareSim::imuCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
    subscriber_imu_ = model_nh.subscribe(ops);
    gzlog << "[hector_quadrotor_controller_gazebo] Using topic '" << subscriber_imu_.getTopic() << "' as imu input for control" << std::endl;
  } else {
    gzlog << "[hector_quadrotor_controller_gazebo] Using ground truth from Gazebo as imu input for control" << std::endl;
  }

  // subscribe motor_status
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<airdrone_gazebo::MotorStatus>("motor_status", 1, boost::bind(&QuadrotorHardwareSim::motorStatusCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
    subscriber_motor_status_ = model_nh.subscribe(ops);
  }

  // publish wrench
  {
    ros::AdvertiseOptions ops = ros::AdvertiseOptions::create<geometry_msgs::WrenchStamped>("command/wrench", 1, ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_);
    publisher_wrench_command_ = model_nh.advertise(ops);
  }

  // publish motor command
  {
    ros::AdvertiseOptions ops = ros::AdvertiseOptions::create<airdrone_gazebo::MotorCommand>("command/motor", 1, ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_);
    publisher_motor_command_ = model_nh.advertise(ops);
  }

  return true;
}

Exemplo n.º 2

Arquivo: cortex_bridge.cpp Projeto: unr-arl/cortex_ros_bridge

// main driver
int main(int argc, char* argv[])
{
	// initialize ros / variables
	ros::init(argc, argv, "cortex_bridge");
	ros::NodeHandle nh;
	ros::ServiceServer service;
	glob = ros::Time::now();
	std::string myIP, cortexIP;

	// Get IPs from param file
	ros::NodeHandle param_nh("~");
	if ( !param_nh.getParam("local_ip", myIP) )
		ROS_ERROR ("Could not find IP of this machine in launch file.");
	if ( !param_nh.getParam("cortex_ip", cortexIP) )
		ROS_ERROR ("Could not find IP of cortex machine in launch file.");

	// publisher for markers
#ifdef PUBLISH_VISUALIZATION_MARKERS
	Cortex_markers = nh.advertise<visualization_msgs::MarkerArray>("vis_markers", 1000);
#else
	Cortex_markers = nh.advertise<cortex_bridge::Markers>("novis_markers", 1000);
#endif

	// broadcaster for transforms
	Cortex_broadcaster = new tf::TransformBroadcaster();

	// service to set origin for body
	service = nh.advertiseService("cortexSetOrigin", setOriginCallback);

	// set loop rate
	ros::Rate loop_rate(150);	

	// Initialize cortex connection
	if ( InitializeCortexConnection ( myIP.c_str(), cortexIP.c_str() ) != 0 )
	{
		ROS_INFO ( "Error: Unable to initialize ethernet communication" );
		return -1;
	}

	// get all the possible bodies to track
	NumBodiesOfInterest = GetKnownBodies ( bodyOfInterest );
	if ( NumBodiesOfInterest < 0 )
	{
		// clean up cortex connection
		Cortex_Exit();
		ROS_ERROR ("Are you sure you added body defs in Cortex?");
		return -1;
	}

	// init body offsets
	bodyOriginOffset = new float*[NumBodiesOfInterest];
	for ( int i = 0; i < NumBodiesOfInterest; ++i )
	{
		bodyOriginOffset[i] = new float[7];
		for ( int j = 0; j < 7; ++j )
			bodyOriginOffset[i][j] = 0.0;
	}

	// get cortex frame rate information
	GetCortexFrameRate();

	// set callbacks and handlers
	InitializeCortexHandlers();
	Cortex_SetDataHandlerFunc(newFrameCallback);

	// call callbacks while ros::ok == true
	ros::spin();

	// clean up cortex connection
	ROS_INFO ("Cortex_Exit");
	Cortex_Exit();

	// clean up memory
	for ( int i = 0; i < NumBodiesOfInterest; ++i )
	{
		delete bodyOfInterest[i];
		delete bodyOriginOffset[i];
	}
	delete bodyOfInterest;
	delete bodyOriginOffset;

	return 0;
}