C++ ROS_WARN_STREAM函数代码示例

Mesh* createMeshFromAsset(const aiScene* scene, const Eigen::Vector3d &scale, const std::string &resource_name)
{
  if (!scene->HasMeshes())
  {
    ROS_WARN_STREAM("Assimp reports scene in " << resource_name << " has no meshes");
    return NULL;
  }
  EigenSTL::vector_Vector3d vertices;
  std::vector<unsigned int> triangles;
  extractMeshData(scene, scene->mRootNode, aiMatrix4x4(), scale, vertices, triangles);
  if (vertices.empty())
  {
    ROS_WARN_STREAM("There are no vertices in the scene " << resource_name);
    return NULL;
  }
  if (triangles.empty())
  {
    ROS_WARN_STREAM("There are no triangles in the scene " << resource_name);
    return NULL;
  }
  
  return createMeshFromVertices(vertices, triangles);
}

collision_space::EnvironmentModel::AllowedCollisionMatrix 
planning_environment::convertFromACMMsgToACM(const arm_navigation_msgs::AllowedCollisionMatrix& matrix)
{
  std::map<std::string, unsigned int> indices;
  std::vector<std::vector<bool> > vecs;
  unsigned int ns = matrix.link_names.size();
  if(matrix.entries.size() != ns) {
    ROS_WARN_STREAM("Matrix messed up");
  }
  vecs.resize(ns);
  for(unsigned int i = 0; i < std::min(ns, (unsigned int) matrix.entries.size()); i++) {
    indices[matrix.link_names[i]] = i;
    if(matrix.entries[i].enabled.size() != ns) {
      ROS_WARN_STREAM("Matrix messed up");
    }
    vecs[i].resize(ns);
    for(unsigned int j = 0; j < std::min(ns, (unsigned int) matrix.entries[i].enabled.size()); j++) {
      vecs[i][j] = matrix.entries[i].enabled[j];
    }
  }
  collision_space::EnvironmentModel::AllowedCollisionMatrix acm(vecs,indices);
  return acm;
}

void CSrf10Controller::timerCallback(const ros::TimerEvent& e)
{
    ros::Time now=ros::Time::now();
    std::map<uint8_t,unsigned short> updatedDistances;
    int code=device_p_->getDistanceSensors(updatedDistances);
    if (code<0)
        ROS_ERROR("Unable to get srf10 sensor distances from the base control board");
    else
    {
        std::map< uint8_t,CDistanceSensor * >::iterator it;
        for (it=srf10Sensors_.begin();it!=srf10Sensors_.end();it++)
        {
            if (updatedDistances.count((*it).first)>0)
            {
                ROS_DEBUG_STREAM("Obtained distance " << updatedDistances[(*it).first] << " for srf10 sensor " << (*it).second->getName() << " from the base control board");

//do we publish al lvalues or only when we see something ? (0.0 values means no obstacle)
                bool publish_if_obstacle;

                nh.getParam("controllers/"+getName()+"/sensors/front/"+(*it).second->getName()+"/publish_if_obstacle", publish_if_obstacle);
                if( publish_if_obstacle){
                    if( updatedDistances[(*it).first]>0)
                       (*it).second->publish((float)updatedDistances[(*it).first],now);
}
                else{
                     (*it).second->publish((float)updatedDistances[(*it).first],now);
}
            }
            else
                ROS_WARN_STREAM("Could not obtain distance of srf10 sensor " << (int)((*it).first) << " from base control board");
        }
    }
    std::vector<unsigned int> adcReads;
    code=device_p_->getAdcReads(adcSensorsAddresses_,adcReads);
    if (code<0)
        ROS_ERROR("Unable to get adc sensor reads from the base control board");
    else
    {
        if(adcReads.size()!=adcSensorsAddresses_.size())
            ROS_ERROR("The asked addreses and the returned reads for the adc sensors do not match");
        else
        {
            for (int i=0;i<adcSensorsAddresses_.size();i++)
            {
                ROS_DEBUG_STREAM("Obtained distance " << adcReads[i] << " for adc sensor " << adcSensors_[adcSensorsAddresses_[i]]->getName() << " from the base control board");
                adcSensors_[adcSensorsAddresses_[i]]->publish(adcReads[i],now);
            }
        }
    }
}

void planning_models::KinematicState::JointStateGroup::getKinematicStateValues(std::map<std::string,double>& joint_state_values) const {
  joint_state_values.clear();
  for(unsigned int i = 0; i < joint_state_vector_.size(); i++) {
    unsigned int dim = joint_state_vector_[i]->getDimension();  
    if(dim != 0) {
      for(unsigned int j = 0; j < joint_state_vector_[i]->getJointStateValues().size(); j++) {
        joint_state_values[joint_state_vector_[i]->getJointStateNameOrder()[j]] = joint_state_vector_[i]->getJointStateValues()[j]; 
      }
    }
  }
  if(joint_state_values.size() != dimension_) {
    ROS_WARN_STREAM("Some problems with group map dimension values " << joint_state_values.size() << " dimension is " << dimension_);
  }
}

void LagrangeAllocator::setWeights(const Eigen::MatrixXd &W_new)
{
  bool isCorrectDimensions = ( W_new.rows() == r && W_new.cols() == r );
  if (!isCorrectDimensions)
  {
    ROS_WARN_STREAM("Attempt to set weight matrix in LagrangeAllocator with wrong dimensions " << W_new.rows() << "*" << W_new.cols() << ".");
    return;
  }

  W = W_new;

  // New weights require recomputing the generalized inverse of the thrust config matrix
  computeGeneralizedInverse();
}

void MotionPlanningFrame::pathConstraintsIndexChanged(int index)
{
  if (move_group_)
  {
    if (index > 0)
    {
      std::string c = ui_->path_constraints_combo_box->itemText(index).toStdString();
      if (!move_group_->setPathConstraints(c))
        ROS_WARN_STREAM("Unable to set the path constraints: " << c);
    }
    else
      move_group_->clearPathConstraints();
  }
}

double leatherman::getColladaFileScale(std::string resource)
{
  static std::map<std::string, float> rescale_cache;

  // Try to read unit to meter conversion ratio from mesh. Only valid in Collada XML formats. 
  TiXmlDocument xmlDoc;
  float unit_scale(1.0);
  resource_retriever::Retriever retriever;
  resource_retriever::MemoryResource res;
  try
  {
    res = retriever.get(resource);
  }
  catch (resource_retriever::Exception& e)
  {
    ROS_ERROR("%s", e.what());
    return unit_scale;
  }

  if (res.size == 0)
  {
    return unit_scale;
  }


  // Use the resource retriever to get the data.
  const char * data = reinterpret_cast<const char * > (res.data.get());
  xmlDoc.Parse(data);

  // Find the appropriate element if it exists
  if(!xmlDoc.Error())
  {
    TiXmlElement * colladaXml = xmlDoc.FirstChildElement("COLLADA");
    if(colladaXml)
    {
      TiXmlElement *assetXml = colladaXml->FirstChildElement("asset");
      if(assetXml)
      {
        TiXmlElement *unitXml = assetXml->FirstChildElement("unit");
        if (unitXml && unitXml->Attribute("meter"))
        {
          // Failing to convert leaves unit_scale as the default.
          if(unitXml->QueryFloatAttribute("meter", &unit_scale) != 0)
            ROS_WARN_STREAM("getMeshUnitRescale::Failed to convert unit element meter attribute to determine scaling. unit element: " << *unitXml);
        }
      }
    }
  }
  return unit_scale;
}

void Channel::cmdCallback(const roboteq_msgs::Command& command)
{
  // Reset command timeout.
  timeout_timer_.stop();
  timeout_timer_.start();
 ROS_WARN_STREAM("Got command " << command.mode << " with " << command.setpoint);
  // Update mode of motor driver. We send this on each command for redundancy against a
  // lost message, and the MBS script keeps track of changes and updates the control
  // constants accordingly.
  controller_->command << "VAR" << channel_num_ << static_cast<int>(command.mode) << controller_->send;

  if (command.mode == roboteq_msgs::Command::MODE_VELOCITY)
  {
    // Get a -1000 .. 1000 command as a proportion of the maximum RPM.
    int roboteq_velocity = to_rpm(command.setpoint) / max_rpm_ * 1000.0;
    ROS_WARN_STREAM("Commanding " << roboteq_velocity << " velocity to motor driver.");

    // Write mode and command to the motor driver.
    controller_->command << "G" << channel_num_ << roboteq_velocity << controller_->send;
  }
  else if (command.mode == roboteq_msgs::Command::MODE_POSITION)
  {
    // Convert the commanded position in rads to encoder ticks.
    int roboteq_position = to_encoder_ticks(command.setpoint);
    ROS_DEBUG_STREAM("Commanding " << roboteq_position << " position to motor driver.");

    // Write command to the motor driver.
    controller_->command << "P" << channel_num_ << roboteq_position << controller_->send;
  }
  else
  {
    ROS_WARN_STREAM("Command received with unknown mode number, dropping.");
  }

  controller_->flush();
  last_mode_ = command.mode;
}

bool VectorVisualization::visualize(const grid_map::GridMap& map)
{
  if (!isActive()) return true;

  for (const auto& type : types_) {
    if (!map.exists(type)) {
      ROS_WARN_STREAM(
          "VectorVisualization::visualize: No grid map layer with name '" << type << "' found.");
      return false;
    }
  }

  // Set marker info.
  marker_.header.frame_id = map.getFrameId();
  marker_.header.stamp.fromNSec(map.getTimestamp());

  // Clear points.
  marker_.points.clear();
  marker_.colors.clear();

  for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator)
  {
    if (!map.isValid(*iterator, positionLayer_) || !map.isValid(*iterator, types_)) continue;
    geometry_msgs::Vector3 vector;
    vector.x = map.at(types_[0], *iterator); // FIXME(cfo): segfaults when types is not set
    vector.y = map.at(types_[1], *iterator);
    vector.z = map.at(types_[2], *iterator);

    Eigen::Vector3d position;
    map.getPosition3(positionLayer_, *iterator, position);
    geometry_msgs::Point startPoint;
    startPoint.x = position.x();
    startPoint.y = position.y();
    startPoint.z = position.z();
    marker_.points.push_back(startPoint);

    geometry_msgs::Point endPoint;
    endPoint.x = startPoint.x + scale_ * vector.x;
    endPoint.y = startPoint.y + scale_ * vector.y;
    endPoint.z = startPoint.z + scale_ * vector.z;
    marker_.points.push_back(endPoint);

    marker_.colors.push_back(color_); // Each vertex needs a color.
    marker_.colors.push_back(color_);
  }

  publisher_.publish(marker_);
  return true;
}

bool constraint_samplers::JointConstraintSampler::setup(const std::vector<kinematic_constraints::JointConstraint> &jc)
{
  if (!jmg_)
  {
    ROS_FATAL("NULL group specified for constraint sampler");
    return false;
  }
  
  // find and keep the constraints that operate on the group we sample
  // also keep bounds for joints as convenient
  const std::map<std::string, unsigned int> &vim = jmg_->getJointVariablesIndexMap();
  std::set<const planning_models::KinematicModel::JointModel*> bounded;
  for (std::size_t i = 0 ; i < jc.size() ; ++i)
  {
    if (!jc[i].enabled())
      continue;
    
    const planning_models::KinematicModel::JointModel *jm = jc[i].getJointModel();
    if (!jmg_->hasJointModel(jm->getName()))
      continue;
    
    std::pair<double, double> bounds;
    jm->getVariableBounds(jc[i].getJointVariableName(), bounds);

    bounds.first = std::max(bounds.first, jc[i].getDesiredJointPosition() - jc[i].getJointToleranceBelow());
    bounds.second = std::min(bounds.second, jc[i].getDesiredJointPosition() + jc[i].getJointToleranceAbove());
    if (bounds.first > bounds.second)
    {
      std::stringstream cs; jc[i].print(cs);
      ROS_WARN_STREAM("The constraints for joint '" << jm->getName() << "' are such that there are no possible values for this joint. Ignoring constraint: \n" << cs.str());
      continue;
    }
    if (jm->getVariableCount() == 1)
      bounded.insert(jm);
    bounds_.push_back(bounds);
    index_.push_back(vim.find(jc[i].getJointVariableName())->second);
  }
  
  // get a separate list of joints that are not bounded; we will sample these randomly
  const std::vector<const planning_models::KinematicModel::JointModel*> &joints = jmg_->getJointModels();
  for (std::size_t i = 0 ; i < joints.size() ; ++i)
    if (bounded.find(joints[i]) == bounded.end())
    {
      unbounded_.push_back(joints[i]);
      uindex_.push_back(vim.find(joints[i]->getName())->second);
    } 
  values_.resize(jmg_->getVariableCount());
  return true;
}

	/** Open the camera device.
	 *
	 * @param newconfig configuration parameters
	 * @return true, if successful
	 *
	 * if successful:
	 *   state_ is Driver::OPENED
	 *   camera_name_ set to camera_name string
	 */
	bool openCamera(Config &newconfig)
	{
		bool success = true;

		try
		{
			ROS_INFO("opening uvc_cam at %dx%d, %f fps", newconfig.width, newconfig.height, newconfig.frame_rate);
                        uvc_cam::Cam::mode_t mode = uvc_cam::Cam::MODE_RGB;
                        switch (newconfig.format_mode) {
                                case 1:
                                  mode = uvc_cam::Cam::MODE_RGB;
                                case 2:
                                  mode = uvc_cam::Cam::MODE_YUYV;
                                case 3:
                                  mode = uvc_cam::Cam::MODE_MJPG;
                                default:
                                  mode = uvc_cam::Cam::MODE_RGB;
                        }
			cam_ = new uvc_cam::Cam(newconfig.device.c_str(), mode, newconfig.width, newconfig.height, newconfig.frame_rate);
			if (camera_name_ != camera_name_)
			{
				camera_name_ = camera_name_;
				if (!cinfo_.setCameraName(camera_name_))
				{
					// GUID is 16 hex digits, which should be valid.
					// If not, use it for log messages anyway.
					ROS_WARN_STREAM("[" << camera_name_ << "] name not valid"
							<< " for camera_info_manger");
				}
			}
			//			ROS_INFO_STREAM("[" << camera_name_
			//					<< "] opened: " << newconfig.video_mode << ", "
			//					<< newconfig.frame_rate << " fps, "
			//					<< newconfig.iso_speed << " Mb/s");
			state_ = Driver::OPENED;
			calibration_matches_ = true;

		//	cam_->display_formats_supported();

		}
		catch (uvc_cam::Exception& e)
		{
			ROS_FATAL_STREAM("[" << camera_name_
					<< "] exception opening device: " << e.what());
			success = false;
		}

		return success;
	}

/**
 * @brief If not already maxxed, increment the angular velocities..
 */
void KeyOp::incrementAngularVelocity()
{
  if (power_status_)
  {
    if (cmd_->angular.z <= angular_vel_max_)
    {
      cmd_->angular.z += angular_vel_step_;
    }
    ROS_INFO_STREAM("KeyOp: angular velocity incremented [" << cmd_->linear.x << "|" << cmd_->angular.z << "]");
  }
  else
  {
    ROS_WARN_STREAM("KeyOp: motors are not yet powered up.");
  }
}

  void PredictionModel::new_measurement(double x, double y, double z)
  {
    if( (x != x) | (y != y) | (z != z) )
    {
      ROS_WARN_STREAM("We received NaN, ignoring the new measurement: " << x << " " << y << " " << z );
      return;
    }

    measurement_(1) = x;
    measurement_(2) = y;
    measurement_(3) = z;

    //we have received a new measurement
    meas_used_ = false;
  }

/**
 * @brief If not already mined, decrement the angular velocities..
 */
void KeyOpCore::decrementAngularVelocity()
{
  if (power_status)
  {
    if (cmd->angular.z >= -angular_vel_max)
    {
      cmd->angular.z -= angular_vel_step;
    }
    ROS_INFO_STREAM("KeyOp: angular velocity decremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
  }
  else
  {
    ROS_WARN_STREAM("KeyOp: motors are not yet powered up.");
  }
}

/**
 * Checks that we have ended inside the goal constraints.
 */
bool JointTrajectoryActionController::goalReached()
{
  for (size_t i = 0; i < joints_.size(); i++)
  {
    double error = current_trajectory_->back().splines[i].target_position - katana_->getMotorAngles()[i];
    if (goal_constraints_[i] > 0 && fabs(error) > goal_constraints_[i])
    {
      ROS_WARN_STREAM("Joint " << i << " did not reach its goal. target position: "
          << current_trajectory_->back().splines[i].target_position << " actual position: "
          << katana_->getMotorAngles()[i] << std::endl);
      return false;
    }
  }
  return true;
}