C++ ROS_INFO_STREAM_NAMED函数代码示例

int main(int argc, char** argv)
{
  ros::init(argc, argv, "SHORT_NAME");
  ROS_INFO_STREAM_NAMED("main", "Starting CLASS_NAME...");

  // Allow the action server to recieve and send ros messages
  ros::AsyncSpinner spinner(2);
  spinner.start();

  // Check for verbose flag
  bool verbose = false;
  if (argc > 1)
  {
    for (int i = 0; i < argc; ++i)
    {
      if (strcmp(argv[i], "--verbose") == 0)
      {
        ROS_INFO_STREAM_NAMED("main","Running in VERBOSE mode (slower)");
        verbose = true;
        continue;
      }
    }
  }

  PACKAGE_NAME::CLASS_NAME server();

  ROS_INFO_STREAM_NAMED("main", "Shutting down.");
  ros::shutdown();

  return 0;
}

bool Action::graspPlan(MetaBlock *block, const std::string surface_name) //computePlanButtonClicked
{
  bool success(false);

  if (verbose_)
    ROS_INFO_STREAM_NAMED("pick_place:","Planning " << block->name << " at pose " << block->start_pose);

  move_group_->setGoalTolerance(0.1);//0.05 //TODO

  // Prevent collision with table
  if (!surface_name.empty())
    move_group_->setSupportSurfaceName(surface_name);

  if (!move_group_)
    return false;

  //move_group_->setApproximateJointValueTargets(target, move_group_->getEndEffectorLink().c_str());
  move_group_->setPoseTargets(configureForPlanning(generateGrasps(block)), move_group_->getEndEffectorLink().c_str());

  current_plan_.reset(new moveit::planning_interface::MoveGroup::Plan());
  success = move_group_->plan(*current_plan_);
  if (!success)
    current_plan_.reset();

  if (verbose_ && success)
      ROS_INFO_STREAM_NAMED("pick_place","Grasp plannin success! \n\n");

  return success;
}

  // Execute series of tasks for pick/place
  bool pickAndPlace(const geometry_msgs::Pose& start_block_pose, const geometry_msgs::Pose& end_block_pose)
  {
    ROS_INFO_STREAM_NAMED("pick_place","Pick and place started");

    // ---------------------------------------------------------------------------------------------
    // Visualize the two blocks
    rviz_tools_->publishBlock(start_block_pose);
    rviz_tools_->publishBlock(end_block_pose);

    // ---------------------------------------------------------------------------------------------
    // Generate graps
    ROS_INFO_STREAM_NAMED("pick_place_moveit","Generating grasps for pick and place");

    bool rviz_verbose = true;
    moveit_simple_grasps::SimpleGrasps grasp_generator(rviz_tools_);

    // Pick grasp
    std::vector<moveit_msgs::Grasp> possible_grasps;
    grasp_generator.generateBlockGrasps( start_block_pose, grasp_data_, possible_grasps );

    // Filter grasp poses
    //moveit_simple_grasps::GraspFilter grasp_filter( planning_scene_monitor_->getPlanningScene()->getCurrentState() ...
    //if( !grasp_generator.filterGrasps( possible_grasps ) )
    //return false;


    // Send pick command to move_group
    executeGrasps(possible_grasps, start_block_pose);


    return true;
  }

  // Execute series of tasks for pick/place
  bool pickAndPlace(const geometry_msgs::Pose& start_block_pose, const geometry_msgs::Pose& end_block_pose)
  {
    ROS_INFO_STREAM_NAMED("pick_place","Pick and place started");

    // ---------------------------------------------------------------------------------------------
    // Visualize the two blocks
    rviz_tools_->publishBlock(start_block_pose, BLOCK_SIZE, true);
    rviz_tools_->publishBlock(end_block_pose, BLOCK_SIZE, false);

    // ---------------------------------------------------------------------------------------------
    // Generate graps
    ROS_INFO_STREAM_NAMED("pick_place_moveit","Generating grasps for pick and place");

    bool rviz_verbose = true;
    block_grasp_generator::GraspGenerator grasp_generator( base_link_, PLANNING_GROUP_NAME, rviz_tools_, rviz_verbose);

    // Pick grasp
    std::vector<manipulation_msgs::Grasp> possible_grasps;
    grasp_generator.generateGrasps( start_block_pose, possible_grasps );

    // Filter grasp poses
    //block_grasp_generator::GraspFilter grasp_filter( planning_scene_monitor_->getPlanningScene()->getCurrentState() ...
    //if( !grasp_generator.filterGrasps( possible_grasps ) )
    //return false;


    // Send pick command to move_group
    executeGrasps(possible_grasps, start_block_pose);


    return true;
  }

void GenericHWInterface::loadURDF(ros::NodeHandle &nh, std::string param_name)
{
  std::string urdf_string;
  urdf_model_ = new urdf::Model();

  // search and wait for robot_description on param server
  while (urdf_string.empty() && ros::ok())
  {
    std::string search_param_name;
    if (nh.searchParam(param_name, search_param_name))
    {
      ROS_INFO_STREAM_NAMED("generic_hw_interface", "Waiting for model URDF on the ROS param server at location: " <<
                            nh.getNamespace() << search_param_name);
      nh.getParam(search_param_name, urdf_string);
    }
    else
    {
      ROS_INFO_STREAM_NAMED("generic_hw_interface", "Waiting for model URDF on the ROS param server at location: " <<
                            nh.getNamespace() << param_name);
      nh.getParam(param_name, urdf_string);
    }

    usleep(100000);
  }

  if (!urdf_model_->initString(urdf_string))
    ROS_ERROR_STREAM_NAMED("generic_hw_interface", "Unable to load URDF model");
  else
    ROS_DEBUG_STREAM_NAMED("generic_hw_interface", "Received URDF from param server");
}

  void createCollisionObject(const geometry_msgs::Pose& block_pose, moveit_msgs::CollisionObject& block_object)
  {
    if( block_published_ )
    {
      return; // only publish the block once!
    }

    ROS_INFO_STREAM_NAMED("pick_place_moveit","Creating the collision object");
    // ---------------------------------------------------------------------------------------------
    // Create Solid Primitive
    shape_msgs::SolidPrimitive block_shape;

    // type of the shape
    block_shape.type = shape_msgs::SolidPrimitive::BOX;

    // dimensions of the shape
    //    block_shape.dimensions.resize(3);
    block_shape.dimensions.push_back(BLOCK_SIZE); // x
    block_shape.dimensions.push_back(BLOCK_SIZE); // y
    block_shape.dimensions.push_back(BLOCK_SIZE); // z

    // ---------------------------------------------------------------------------------------------
    // Add the block to the collision environment

    // a header, used for interpreting the poses
    block_object.header.frame_id = base_link_;
    block_object.header.stamp = ros::Time::now();

    // the id of the object
    static int block_id = 0;
    block_object.id = "Block" + boost::lexical_cast<std::string>(block_id);

    // the the collision geometries associated with the object;
    // their poses are with respect to the specified header

    // solid geometric primitives
    //shape_msgs/SolidPrimitive[] primitives // TODO?
    block_object.primitives.push_back(block_shape);

    //geometry_msgs/Pose[] primitive_poses
    block_object.primitive_poses.push_back( block_pose );

    // meshes
    //shape_msgs/Mesh[] meshes
    //geometry_msgs/Pose[] mesh_poses

    // bounding planes (equation is specified, but the plane can be oriented using an additional pose)
    //shape_msgs/Plane[] planes
    //geometry_msgs/Pose[] plane_poses

    // Operation to be performed
    block_object.operation = moveit_msgs::CollisionObject::ADD; // Puts the object into the environment or updates the object if already added

    // Send the object
    collision_obj_pub_.publish(block_object);
    block_published_ = true;
    ROS_INFO_STREAM_NAMED("pick_place_moveit","Collision object published for addition");
  }

    MovePlatformAction() :
        as_(n_, CARLOS_MOVE_ACTION, false), //movePlatform action SERVER
        ac_planner_("/plan_action", true), // Planner action CLIENT
        ac_control_("/control_action", true) // Controller action CLIENT
    {

        n_.param("/move_platform_server/debug", debug_, false);

        std::string name = ROSCONSOLE_DEFAULT_NAME; //ros.carlos_motion_action_server
        name = name  + ".debug";
        logger_name_ = "debug";
        //logger is ros.carlos_motion_action_server.debug

        if (debug_)
        {
            // if we use ROSCONSOLE_DEFAULT_NAME we'll get a ton of debug messages from actionlib which is annoying!!!
            // so for debug we'll use a named logger
            if(ros::console::set_logger_level(name, ros::console::levels::Debug)) //name
                ros::console::notifyLoggerLevelsChanged();
        }
        else // if not DEBUG we want INFO
        {
            if(ros::console::set_logger_level(name, ros::console::levels::Info)) //name
                ros::console::notifyLoggerLevelsChanged();
        }

        ROS_DEBUG_NAMED(logger_name_, "Starting Move Platform Server");

        as_.registerGoalCallback(boost::bind(&MovePlatformAction::moveGoalCB, this));
        as_.registerPreemptCallback(boost::bind(&MovePlatformAction::movePreemptCB, this));

        //start the move server
        as_.start();
        ROS_DEBUG_NAMED(logger_name_, "Move Platform Action Server Started");

        // now wait for the other servers (planner + controller) to start
        ROS_WARN_NAMED(logger_name_, "Waiting for planner server to start");
        ac_planner_.waitForServer();
        ROS_INFO_STREAM_NAMED(logger_name_, "Planner server started: " <<  ac_planner_.isServerConnected());

        ROS_WARN_NAMED(logger_name_, "Waiting for controller server to start");
        ac_control_.waitForServer();
        ROS_INFO_STREAM_NAMED(logger_name_, "Controller server started: " <<  ac_control_.isServerConnected());

        n_.param("/carlos/fsm_frequency", frequency_, DEFAULT_STATE_FREQ);
        state_pub_timer_ = n_.createTimer(frequency_, &MovePlatformAction::state_pub_timerCB, this);
        state_pub_ = n_.advertise<mission_ctrl_msgs::hardware_state>(CARLOS_BASE_STATE_MSG,1);

        planning_ = false;
        controlling_ = false;
        //set_terminal_state_;
        ctrl_state_sub = n_.subscribe<std_msgs::String>("/oea_controller/controller_state", 5, &MovePlatformAction::control_stateCB, this);
        planner_state_sub = n_.subscribe<std_msgs::UInt8>("/oea_planner/state", 5, &MovePlatformAction::planner_stateCB, this);

    }

void OpenNIListener::setupSubscribers(){
  ParameterServer* ps = ParameterServer::instance();
  int q = ps->get<int>("subscriber_queue_size");
  ros::NodeHandle nh;
  tflistener_ = new tf::TransformListener(nh);
  std::string bagfile_name = ps->get<std::string>("bagfile_name");
  if(bagfile_name.empty()){
    std::string visua_tpc = ps->get<std::string>("topic_image_mono");
    std::string depth_tpc = ps->get<std::string>("topic_image_depth");
    std::string cinfo_tpc = ps->get<std::string>("camera_info_topic");
    std::string cloud_tpc = ps->get<std::string>("topic_points");
    std::string widev_tpc = ps->get<std::string>("wide_topic");
    std::string widec_tpc = ps->get<std::string>("wide_cloud_topic");

    //All information from Kinect
    if(!visua_tpc.empty() && !depth_tpc.empty() && !cloud_tpc.empty())
    {   
        visua_sub_ = new image_sub_type(nh, visua_tpc, q);
        depth_sub_ = new image_sub_type (nh, depth_tpc, q);
        cloud_sub_ = new pc_sub_type (nh, cloud_tpc, q);  
        kinect_sync_ = new message_filters::Synchronizer<KinectSyncPolicy>(KinectSyncPolicy(q),  *visua_sub_, *depth_sub_, *cloud_sub_),
        kinect_sync_->registerCallback(boost::bind(&OpenNIListener::kinectCallback, this, _1, _2, _3));
        ROS_INFO_STREAM_NAMED("OpenNIListener", "Listening to " << visua_tpc << ", " << depth_tpc << " and " << cloud_tpc);
    } 
    //No cloud, but visual image and depth
    else if(!visua_tpc.empty() && !depth_tpc.empty() && !cinfo_tpc.empty() && cloud_tpc.empty())
    {   
        visua_sub_ = new image_sub_type(nh, visua_tpc, q);
        depth_sub_ = new image_sub_type(nh, depth_tpc, q);
        cinfo_sub_ = new cinfo_sub_type(nh, cinfo_tpc, q);
        no_cloud_sync_ = new message_filters::Synchronizer<NoCloudSyncPolicy>(NoCloudSyncPolicy(q),  *visua_sub_, *depth_sub_, *cinfo_sub_);
        no_cloud_sync_->registerCallback(boost::bind(&OpenNIListener::noCloudCallback, this, _1, _2, _3));
        ROS_INFO_STREAM_NAMED("OpenNIListener", "Listening to " << visua_tpc << " and " << depth_tpc);
    } 

    //All information from stereo                                               
    if(!widec_tpc.empty() && !widev_tpc.empty())
    {   
      visua_sub_ = new image_sub_type(nh, widev_tpc, q);
      cloud_sub_ = new pc_sub_type(nh, widec_tpc, q);
      stereo_sync_ = new message_filters::Synchronizer<StereoSyncPolicy>(StereoSyncPolicy(q), *visua_sub_, *cloud_sub_);
      stereo_sync_->registerCallback(boost::bind(&OpenNIListener::stereoCallback, this, _1, _2));
      ROS_INFO_STREAM_NAMED("OpenNIListener", "Listening to " << widev_tpc << " and " << widec_tpc );
    } 
    if(ps->get<bool>("use_robot_odom")){
    	odom_sub_= new odom_sub_type(nh, ps->get<std::string>("odometry_tpc"), 3);
      ROS_INFO_STREAM_NAMED("OpenNIListener", "Listening to odometry on " << ps->get<std::string>("odometry_tpc"));
    	odom_sub_->registerCallback(boost::bind(&OpenNIListener::odomCallback,this,_1));
    }
  } 
  else {
    ROS_WARN("RGBDSLAM loads a bagfile - therefore doesn't subscribe to topics.");
  }
}

  // Skip perception
  void fake_goalCB()
  {
    ROS_INFO_STREAM_NAMED("pick_place_moveit","Received fake goal ----------------------------------------");

    // Position
    geometry_msgs::Pose start_block_pose;
    geometry_msgs::Pose end_block_pose;

    // Does not work
    start_block_pose.position.x = 0.35;
    start_block_pose.position.y = 0.2;
    start_block_pose.position.z = 0.02;

    // Works - close
    start_block_pose.position.x = 0.2;
    start_block_pose.position.y = 0.0;
    start_block_pose.position.z = 0.02;

    // 3rd try
    start_block_pose.position.x = 0.35;
    start_block_pose.position.y = 0.1;
    start_block_pose.position.z = 0.02;

    nh_.param<double>("/block_pick_place_server/block_x", start_block_pose.position.x, 0.2);
    nh_.param<double>("/block_pick_place_server/block_y", start_block_pose.position.y, 0.0);
    nh_.param<double>("/block_pick_place_server/block_z", start_block_pose.position.z, 0.02);

    ROS_INFO_STREAM_NAMED("pick_place_moveit","start block is \n" << start_block_pose.position);


    end_block_pose.position.x = 0.25;
    end_block_pose.position.y = 0.15;
    end_block_pose.position.z = 0.02;

    // Orientation
    double angle = M_PI / 1.5;
    Eigen::Quaterniond quat(Eigen::AngleAxis<double>(double(angle), Eigen::Vector3d::UnitZ()));
    start_block_pose.orientation.x = quat.x();
    start_block_pose.orientation.y = quat.y();
    start_block_pose.orientation.z = quat.z();
    start_block_pose.orientation.w = quat.w();

    angle = M_PI / 1.1;
    quat = Eigen::Quaterniond(Eigen::AngleAxis<double>(double(angle), Eigen::Vector3d::UnitZ()));
    end_block_pose.orientation.x = quat.x();
    end_block_pose.orientation.y = quat.y();
    end_block_pose.orientation.z = quat.z();
    end_block_pose.orientation.w = quat.w();

    // Fill goal
    base_link_ = "base_link";

    processGoal(start_block_pose, end_block_pose);
  }

  BlockPerceptionServer(const std::string name) :
    nh_("~"),
    action_server_(name, false),
    action_name_(name)
  {
    // Publish a point cloud of filtered data that was not part of table
    filtered_pub_ = nh_.advertise< pcl::PointCloud<pcl::PointXYZRGB> >("block_output", 1);

    // Publish a point cloud of data that was considered part of the plane
    plane_pub_ = nh_.advertise< pcl::PointCloud<pcl::PointXYZRGB> >("plane_output", 1);

    // Publish interactive markers for blocks
    block_pose_pub_ = nh_.advertise< geometry_msgs::PoseArray >("block_orientation", 1, true);

    // Publish markers to highlight blocks
    block_marker_pub_ = nh_.advertise<visualization_msgs::Marker>("block_marker", 1);

    // Setup OpenCV stuff
    canny_threshold = 100;

    hough_rho = 2; // Distance resolution of the accumulator in pixels.
    hough_theta = 1; // Angle resolution of the accumulator in fraction of degress (so 1/theta degrees). to be converted to radians
    hough_threshold = 14; // Accumulator threshold parameter. Only those lines are returned that get enough votes
    hough_minLineLength = 13; //10; // Minimum line length. Line segments shorter than that are rejected.
    hough_maxLineGap = 16; // Maximum allowed gap between points on the same line to link them.

    // Initialize how often we process images
    process_count_ = PROCESS_EVERY_NTH;

    // TODO: move this, should be brought in from action goal. temporary!
    base_link = "/base_link";
    camera_link = "/camera_rgb_frame";
    //    camera_link = "/camera_rgb_optical_frame";
    block_size = 0.04;
    //    table_height = 0.001;
    table_height = 0.0;

    // Subscribe to point cloud
    point_cloud_sub_ = nh_.subscribe("/camera/depth_registered/points", 1, &BlockPerceptionServer::pointCloudCallback, this);

    // Register the goal and feeback callbacks.
    action_server_.registerGoalCallback(boost::bind(&BlockPerceptionServer::goalCB, this));
    action_server_.registerPreemptCallback(boost::bind(&BlockPerceptionServer::preemptCB, this));

    action_server_.start();

    // Announce state
    ROS_INFO_STREAM_NAMED("perception", "Server ready.");
    ROS_INFO_STREAM_NAMED("perception", "Waiting for point clouds...");
  }

  void SplineFitting::calcSimpleDerivative()
  {
    ROS_INFO_STREAM_NAMED(name_, "Calculating simple derivatives");
    joint_velocities_.resize(num_joints_);

    bool verbose = false;

    // For each joint
    for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
    {
      // For each waypoint
      for (std::size_t i = 1; i < joint_positions_[joint_id].size(); ++i)
      {
        double pos_diff = joint_positions_[joint_id][i] - joint_positions_[joint_id][i - 1];
        double t_diff = timestamps_[i] - timestamps_[i - 1];
        double vel = pos_diff / t_diff;
        if (verbose)
          std::cout << "joint_id: " << joint_id << " i: " << std::setfill('0') << std::setw(2) << i
                    << " pos_diff: " << std::fixed << pos_diff << " \tt_diff: " << t_diff << " \tvel: " << vel
                    << std::endl;

        joint_velocities_[joint_id].push_back(vel);
      }
      joint_velocities_[joint_id].push_back(0.0);  // TODO(davetcoleman): how to calculate final derivative?
    }
  }

	/**
	 * @brief Send a safety zone (volume), which is defined by two corners of a cube,
	 * to the FCU.
	 *
	 * @note ENU frame.
	 */
	void send_safety_set_allowed_area(Eigen::Vector3d p1, Eigen::Vector3d p2)
	{
		ROS_INFO_STREAM_NAMED("safetyarea", "SA: Set safty area: P1 " << p1 << " P2 " << p2);

		p1 = ftf::transform_frame_enu_ned(p1);
		p2 = ftf::transform_frame_enu_ned(p2);

		mavlink::common::msg::SAFETY_SET_ALLOWED_AREA s;
		m_uas->msg_set_target(s);

		// TODO: use enum from lib
		s.frame = utils::enum_value(mavlink::common::MAV_FRAME::LOCAL_NED);

		// [[[cog:
		// for p in range(1, 3):
		//     for v in ('x', 'y', 'z'):
		//         cog.outl("s.p%d%s = p%d.%s();" % (p, v, p, v))
		// ]]]
		s.p1x = p1.x();
		s.p1y = p1.y();
		s.p1z = p1.z();
		s.p2x = p2.x();
		s.p2y = p2.y();
		s.p2z = p2.z();
		// [[[end]]] (checksum: c996a362f338fcc6b714c8be583c3be0)

		UAS_FCU(m_uas)->send_message_ignore_drop(s);
	}

Chroma_processing::Chroma_processing()
: it_(nh_)
{
	//Getting the parameters specified by the launch file 
	ros::NodeHandle local_nh("~");
	local_nh.param("image_topic", image_topic, string("/camera/rgb/image_raw"));
	local_nh.param("image_out_topic", image_out_topic, string("/chroma_proc/image"));
	local_nh.param("image_out_dif_topic", image_out_dif_topic, string("/chroma_proc/image_dif"));
	local_nh.param("project_path",path_, string(""));
	local_nh.param("playback_topics", playback_topics, false);
	local_nh.param("display", display, false);
	
	if(playback_topics)
	{
		ROS_INFO_STREAM_NAMED("Chroma_processing","Subscribing at compressed topics \n"); 
		
		image_sub = it_.subscribe(image_topic, 1, 
		  &Chroma_processing::imageCb, this, image_transport::TransportHints("compressed"));
    }
    else
    {
		// Subscribe to input video feed 
		image_sub = it_.subscribe(image_topic, 1, &Chroma_processing::imageCb, this);
		
	} 
	
	image_pub = it_.advertise(image_out_topic, 1); 
	image_pub_dif = it_.advertise(image_out_dif_topic, 1); 
}

int main( int argc, char **argv )
{

  ros::init( argc, argv, "example3" );

  ros::NodeHandle n;

  ros::Rate rate( 1 );
  while( ros::ok() ) {

    ROS_DEBUG_STREAM( "DEBUG message." );
    ROS_INFO_STREAM ( "INFO message."  );
    ROS_WARN_STREAM ( "WARN message."  );
    ROS_ERROR_STREAM( "ERROR message." );
    ROS_FATAL_STREAM( "FATAL message." );

    ROS_INFO_STREAM_NAMED( "named_msg", "INFO named message." );

    ROS_INFO_STREAM_THROTTLE( 2, "INFO throttle message." );

    ros::spinOnce();
    rate.sleep();
  }

  return EXIT_SUCCESS;

}

	void initialize(UAS &uas_)
	{
		bool tf_listen;

		uas = &uas_;

		// main params
		sp_nh.param("reverse_throttle", reverse_throttle, false);
		// tf params
		sp_nh.param("tf/listen", tf_listen, false);
		sp_nh.param<std::string>("tf/frame_id", tf_frame_id, "local_origin");
		sp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "attitude");
		sp_nh.param("tf/rate_limit", tf_rate, 10.0);

		if (tf_listen) {
			ROS_INFO_STREAM_NAMED("attitude", "Listen to desired attitude transform " << tf_frame_id
					<< " -> " << tf_child_frame_id);
			tf2_start("AttitudeSpTF", &SetpointAttitudePlugin::transform_cb);
		}
		else {
			twist_sub = sp_nh.subscribe("cmd_vel", 10, &SetpointAttitudePlugin::twist_cb, this);
			pose_sub = sp_nh.subscribe("attitude", 10, &SetpointAttitudePlugin::pose_cb, this);
		}

		throttle_sub = sp_nh.subscribe("att_throttle", 10, &SetpointAttitudePlugin::throttle_cb, this);
	}