C++ ROS_ERROR_STREAM函数代码示例

	void GroupCommandController::commandCB(const std_msgs::Float64MultiArrayConstPtr& msg)
    {
		 #if TRACE_GroupCommandController_ACTIVATED
			ROS_INFO("GroupCommandController: Start commandCB of robot %s!",robot_namespace_.c_str());
		 #endif
		 
		
	     if(msg->data.size()!=joint_handles_.size())
	     { 
	       ROS_ERROR_STREAM("GroupCommandController: Dimension (of robot " << robot_namespace_.c_str() << ") of command (" << msg->data.size() << ") does not match number of joints (" << joint_handles_.size() << ")! Not executing!");
	       cmd_flag_ = 0;
	       return; 
	     }
	     
	     // clear buffers for initial values
	     commands_buffer_.clear();
	     goal_buffer_.clear();
	     goal_factor_.clear();
	     
	     for (size_t i=0; i<joint_handles_.size(); ++i)
	     {
			commands_buffer_.push_back(msg->data[i]);
			
			// set the factor of increment/decrement depending of the current joint value and the desired one
			if (joint_handles_[i].getPosition() > msg->data[i]) 
				goal_factor_.push_back(-1);
			else 
				goal_factor_.push_back(1);
		 }
		 
	     cmd_flag_ = 1; // set this flag to 1 to run the update method
	     
	     #if TRACE_GroupCommandController_ACTIVATED
			ROS_INFO("GroupCommandController: Finish commandCB of robot %s !",robot_namespace_.c_str());
			ROS_INFO("GroupCommandController: of robot %s -> j0=%f, j1=%f, j2=%f, j3=%f, j4=%f, j5=%f, j6=%f",robot_namespace_.c_str(),commands_buffer_[0],commands_buffer_[1],commands_buffer_[2],commands_buffer_[3],commands_buffer_[4],commands_buffer_[5],commands_buffer_[6]);

		 #endif
		 
	}

void
KalmanDetectionFilter::drawFilterStates()
{
    if(!current_filter_)
        return;
    geometry_msgs::PointStamped temp_msg, temp_msg_base_link;
    temp_msg.header.frame_id = "odom";
    temp_msg.header.stamp = ros::Time(0);

    cv::Mat img = cv::Mat::zeros(500, 500, CV_8UC3);
    float px_per_meter = 50.0;
    float offset = 250;
    temp_msg.point.x = current_filter_->statePost.at<float>(0);
    temp_msg.point.y = current_filter_->statePost.at<float>(1);
    temp_msg.point.z = 0.0;

    if( listener_.canTransform( "base_link", temp_msg.header.frame_id, temp_msg.header.stamp))
    {
        listener_.transformPoint("base_link",temp_msg,temp_msg_base_link);
    }
    else
    {
        ROS_ERROR_STREAM("cannot transform filter from odom to base_link");
        return;
    }

    cv::Point mean(temp_msg_base_link.point.x * px_per_meter,
            temp_msg_base_link.point.y * px_per_meter);
    float rad_x = current_filter_->errorCovPost.at<float>(0,0) * px_per_meter;
    float rad_y = current_filter_->errorCovPost.at<float>(1,1) * px_per_meter;
    cv::circle(img, mean+cv::Point(0,offset), 5, cv::Scalar(255,0,0));
    cv::ellipse(img, mean+cv::Point(0,offset), cv::Size(rad_x, rad_y), 0, 0, 360, cv::Scalar(0,255,0));

    //printFilterState();
    std_msgs::Header header;
    sensor_msgs::ImagePtr debug_img_msg = cv_bridge::CvImage(header,"rgb8",img).toImageMsg();
    pub_debug_img.publish(debug_img_msg);
}

bool MCU::rcvMCUMsg(double & angle)
{
    bool result = false;
    char b0, b1, b2, b3, b4, b5;

    if (!serialPort.get(b0)) return false;
    if (b0 != MCU_RESP_START_BYTE) return false;
    if (!serialPort.get(b1)) return false;
    if (!serialPort.get(b2)) return false;
    if (!serialPort.get(b3)) return false;
    if (!serialPort.get(b4)) return false;
    if (!serialPort.get(b5)) return false;

    // Perform checksum
    char checksum;
    checksum = b0 ^ b1 ^ b2 ^ b3  ^ b4;
    if (checksum == b5)
    {
        int stepPosition = (b1 << 24) + (b2 << 16) + (b3 << 8) + (b4 & 0xFF);
        angle = static_cast<double>(stepPosition) / STEPS_PER_REV * 360;

        // ROS_INFO_STREAM("MCU::rcvMCUMsg: Received current angle: " << currAngle << ", stepPosition: " << stepPosition << "\n"
        //     << "b0 = " << std::hex << "0x" << static_cast<int>(b0) << "\n"
        //     << "b1 = " << std::hex << "0x" << static_cast<int>(b1) << "\n"
        //     << "b2 = " << std::hex << "0x" << static_cast<int>(b2) << "\n"
        //     << "b3 = " << std::hex << "0x" << static_cast<int>(b3) << "\n"
        //     << "b4 = " << std::hex << "0x" << static_cast<int>(b4) << "\n"
        //     << "b5 = " << std::hex << "0x" << static_cast<int>(b5));
        result = true;
    }
    else
    {
        ROS_ERROR_STREAM("MCU::rcvMCUMsg: Bad checksum of 0x" << std::hex << checksum << ", expected 0x" << b5);
        result = false;
    }

    return result;
}

bool ROSRuntimeUtils::store_tf_broadcasters(std::string &package_path, std::string &file_name)
{
  std::string filepath = package_path+file_name;
  std::ofstream output_file(filepath.c_str(), std::ios::out);// | std::ios::app);
  if (output_file.is_open())
  {
    ROS_INFO_STREAM("Storing results in: "<<filepath);
  }
  else
  {
    ROS_ERROR_STREAM("Unable to open file");
    return false;
  }
  output_file << "<launch>";
  //have calibrated transforms
  double roll, pitch, yaw, x_position, y_position, z_position;
  tf::Vector3 origin;
  tf::Matrix3x3 orientation;
  for (int i=0; i<calibrated_transforms_.size(); i++)
  {
    origin = calibrated_transforms_.at(i).getOrigin();
    x_position=origin.getX();
    y_position=origin.getY();
    z_position=origin.getZ();
    orientation = calibrated_transforms_.at(i).getBasis();
    orientation.getEulerYPR(yaw, pitch, roll);
    output_file<<"\n";
    output_file<<" <node pkg=\"tf\" type=\"static_transform_publisher\" name=\"world_to_camera"<<i<<"\" args=\"";
    //tranform publisher launch files requires x y z yaw pitch roll
    output_file<<x_position<< ' '<<y_position<< ' '<<z_position<< ' '<<yaw<< ' '<<pitch<< ' '<<roll ;
    output_file<<" "<<world_frame_;
    output_file<<" "<<camera_intermediate_frame_[i];
    output_file<<" 100\" />";
  }
  output_file<<"\n";
  output_file << "</launch>";
  return true;
}

std::vector<urdf_traverser::JointPtr>
urdf_traverser::getChain(const LinkConstPtr& from_link, const LinkConstPtr& to_link)
{
    assert(from_link);
    assert(to_link);
    std::vector<JointPtr> chain;

    if (to_link->name == from_link->name) return chain;

    LinkConstPtr curr = to_link;
    LinkConstPtr pl = to_link->getParent();

    while (curr && (curr->name != from_link->name))
    {
        // ROS_INFO_STREAM("Chain: "<<curr->name);
        JointPtr pj = curr->parent_joint;
        if (!pj)
        {
            ROS_ERROR("UrdfTraverser: End of chain at link '%s'", curr->name.c_str());
            return std::vector<JointPtr>();
        }
        chain.push_back(pj);
        curr = pl;
        pl = curr->getParent();
        // if (pl) ROS_INFO_STREAM("Parent: "<<pl->name);
    }
    if (curr->name != from_link->name)
    {
        ROS_ERROR_STREAM("UrdfTraverser: could not find link " <<
                         from_link->name << " while traversing up the chain starting from " <<
                         to_link->name << ". Failed to find parent chain!");
        return std::vector<JointPtr>();
    }

    std::reverse(chain.begin(), chain.end());

    return chain;
}

enum uvc_frame_format CameraDriver::GetVideoMode(std::string vmode){
  if(vmode == "uncompressed") {
    return UVC_COLOR_FORMAT_UNCOMPRESSED;
  } else if (vmode == "compressed") {
    return UVC_COLOR_FORMAT_COMPRESSED;
  } else if (vmode == "yuyv") {
    return UVC_COLOR_FORMAT_YUYV;
  } else if (vmode == "uyvy") {
    return UVC_COLOR_FORMAT_UYVY;
  } else if (vmode == "rgb") {
    return UVC_COLOR_FORMAT_RGB;
  } else if (vmode == "bgr") {
    return UVC_COLOR_FORMAT_BGR;
  } else if (vmode == "mjpeg") {
    return UVC_COLOR_FORMAT_MJPEG;
  } else if (vmode == "gray8") {
    return UVC_COLOR_FORMAT_GRAY8;
  } else {
    ROS_ERROR_STREAM("Invalid Video Mode: " << vmode);
    ROS_WARN_STREAM("Continue using video mode: uncompressed");
    return UVC_COLOR_FORMAT_UNCOMPRESSED;
  }
};

    void DisableLinkPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
    {
        model_ = _parent;
        world_ = model_->GetWorld();

        // Check for link element
        if (!_sdf->HasElement("link")) {
            ROS_ERROR("No link element present. DisableLinkPlugin could not be loaded.");
            return;
        }

        link_name_ = _sdf->GetElement("link")->Get<std::string>();

        // Get pointers to joints
        link_ = model_->GetLink(link_name_);
        if (link_) {
            link_->SetEnabled(false);
            // Output some confirmation
            ROS_INFO_STREAM("DisableLinkPlugin loaded! Link: \"" << link_name_);
        }
        else
            ROS_ERROR_STREAM("Link" << link_name_ << " not found! DisableLinkPlugin could not be loaded.");
    }

	CameraDriver( int camera_index = DEFAULT_CAMERA_INDEX )
	:
		nh( "~" ),
		it( nh ),
		camera( camera_index )
	{
		nh.param<int>( "camera_index", camera_index, DEFAULT_CAMERA_INDEX );
		if ( not camera.isOpened() )
		{
			ROS_ERROR_STREAM( "Failed to open camera device!" );
			ros::shutdown();
		}

		nh.param<int>( "fps", fps, DEFAULT_FPS );
		ros::Duration period = ros::Duration( 1. / fps );

		pub_image_raw = it.advertise( "image_raw", 1 );
	
		frame = boost::make_shared< cv_bridge::CvImage >();
		frame->encoding = sensor_msgs::image_encodings::BGR8;

		timer = nh.createTimer( period, &CameraDriver::capture, this );
	}

boost::shared_ptr<pcl17::PolygonMesh> SceneCloudView::convertToMesh(float* triangles, unsigned int maxverts)
{
    if (maxverts == 0)
        return boost::shared_ptr<pcl17::PolygonMesh>();

    pcl17::PointCloud<pcl17::PointXYZ> cloud;
    cloud.width = (int)(maxverts);
    cloud.height = 1;

    cloud.points.clear();
    for (uint i = 0; i < 3 * maxverts; i += 3)
        cloud.points.push_back(pcl17::PointXYZ(triangles[i], triangles[i + 1], triangles[i + 2]));

    boost::shared_ptr<pcl17::PolygonMesh> mesh_ptr(new pcl17::PolygonMesh());

    try
    {
        pcl17::toROSMsg(cloud, mesh_ptr->cloud);
    }
    catch (std::runtime_error e)
    {
        ROS_ERROR_STREAM("Error in converting cloud to image message: "
                         << e.what());
    }

    mesh_ptr->polygons.resize(maxverts / 3);

    for (size_t i = 0; i < mesh_ptr->polygons.size(); ++i)
    {
        pcl17::Vertices v;
        v.vertices.push_back(i * 3 + 0);
        v.vertices.push_back(i * 3 + 2);
        v.vertices.push_back(i * 3 + 1);
        mesh_ptr->polygons[i] = v;
    }
    return mesh_ptr;
}

void DataCollector::onMouse( int event, int x, int y, int, void* ptr) {
  if( event != cv::EVENT_LBUTTONDOWN )
    return;

  try {
    DataCollector* that  = (DataCollector*) ptr;
    std::string image_path1 = makePngName(std::to_string(that->label), that->sub_dir);
    std::string image_path2 = makeJp2Name(std::to_string(that->label), that->sub_dir);
    if( imwrite(image_path1, that->rgb_ptr->image) && imwrite(image_path2, that->depth_ptr->image)) {
      ROS_INFO_STREAM("Write to:" << image_path1<<"\n"<<image_path2);
      that->label++;
    } else {
      ROS_ERROR_STREAM("error writing!");
    }
    
    Mat depthRead = imread(image_path2, CV_LOAD_IMAGE_ANYDEPTH);
    FeatureExtractor FE(that->rgb_ptr->image ,depthRead);
    FE.extract_feats();
    FE.visualize_feats();
  }
  catch (cv::Exception& ex) {
    fprintf(stderr, "Exception converting image to PNG format: %s\n", ex.what());
  }
}

int main(int argc, char **argv) {
  // Initialize the ROS system and become a node.
  ros::init(argc, argv, "count_and_log");
  ros::NodeHandle nh;

  // Generate log messages of varying severity regularly
  ros::Rate rate(10);
  for (int i = 1; ros::ok(); i++) {
    ROS_DEBUG_STREAM("Counted to " << i);
    if((i % 3) == 0) {
      ROS_INFO_STREAM(i << " is divisible by 3.");
    }
    if((i % 5) == 0) {
      ROS_WARN_STREAM(i << " is divisivle by 5.");
    }
    if((i % 10) == 0) {
      ROS_ERROR_STREAM(i << " is divisible by 10.");
    }
    if((i % 20) == 0) {
    ROS_FATAL_STREAM(i << " is divisible by 20.");
    }
    rate.sleep();
   }
}

  void VrpnTrackerRos::init(std::string tracker_name, ros::NodeHandle nh, bool create_mainloop_timer)
  {
    ROS_INFO_STREAM("Creating new tracker " << tracker_name);

    tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_pose);
    tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_twist);
    tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_accel);
    tracker_remote_->shutup = true;

    std::string error;
    if (!ros::names::validate(tracker_name, error))
    {
      ROS_ERROR_STREAM("Invalid tracker name " << tracker_name << ", not creating topics : " << error);
      return;
    }

    this->tracker_name = tracker_name;

    output_nh_ = ros::NodeHandle(nh, tracker_name);

    std::string frame_id;
    nh.param<std::string>("frame_id", frame_id, "world");
    nh.param<bool>("use_server_time", use_server_time_, false);
    nh.param<bool>("broadcast_tf", broadcast_tf_, false);
    nh.param<bool>("process_sensor_id", process_sensor_id_, false);

    pose_msg_.header.frame_id = twist_msg_.header.frame_id = accel_msg_.header.frame_id = transform_stamped_.header.frame_id = frame_id;

    if (create_mainloop_timer)
    {
      double update_frequency;
      nh.param<double>("update_frequency", update_frequency, 100.0);
      mainloop_timer = nh.createTimer(ros::Duration(1 / update_frequency),
                                      boost::bind(&VrpnTrackerRos::mainloop, this));
    }
  }

 bool registerPlanningScene(std_srvs::Empty::Request &req,
                            std_srvs::Empty::Response &res)
 {
   register_lock_.lock();
   if(req.__connection_header->find("callerid") == req.__connection_header->end()) {
     ROS_ERROR_STREAM("Request has no callerid");
     return false;
   }
   std::string callerid = req.__connection_header->find("callerid")->second;
   if(sync_planning_scene_clients_.find(callerid) != sync_planning_scene_clients_.end()) {
     delete sync_planning_scene_clients_[callerid];
   }
   sync_planning_scene_clients_[callerid] = new actionlib::SimpleActionClient<arm_navigation_msgs::SyncPlanningSceneAction>(callerid+"/"+SYNC_PLANNING_SCENE_NAME, true);
   if(!sync_planning_scene_clients_[callerid]->waitForServer(ros::Duration(10.0))) {
     ROS_INFO_STREAM("Couldn't connect back to action server for " << callerid << ". Removing from list");
     delete sync_planning_scene_clients_[callerid];
     sync_planning_scene_clients_.erase(callerid);
     register_lock_.unlock();
     return false;
   } 
   ROS_INFO_STREAM("Successfully connected to planning scene action server for " << callerid);
   register_lock_.unlock();
   return true;
 }

void move_to_wait_position(move_group_interface::MoveGroup& move_group)
{
  //ROS_ERROR_STREAM("move_to_wait_position is not implemented yet.  Aborting."); exit(1);

  // task variables
  bool success; // saves the move result

  // set robot wait target
  /* Fill Code: [ use the 'setNamedTarget' method in the 'move_group' object] */
  move_group.setNamedTarget(cfg.WAIT_POSE_NAME);

  // move the robot
  /* Fill Code: [ use the 'move' method in the 'move_group' object and save the result in the 'success' variable] */
  success = move_group.move();
  if(success)
  {
    ROS_INFO_STREAM("Move " << cfg.WAIT_POSE_NAME<< " Succeeded");
  }
  else
  {
    ROS_ERROR_STREAM("Move " << cfg.WAIT_POSE_NAME<< " Failed");
    exit(1);
  }
}

void TinkerforgeSensors::publishTemperatureMessage(SensorDevice *sensor)
{
  if (sensor != NULL)
  {
    int16_t temperature;
    if(temperature_get_temperature((Temperature*)sensor->getDev(), &temperature) < 0) {
        ROS_ERROR_STREAM("Could not get temperature from " << sensor->getUID() << ", probably timeout");
        return;
    }
    // generate Temperature message from temperature sensor
    sensor_msgs::Temperature temp_msg;

    // message header
    temp_msg.header.seq =  sensor->getSeq();
    temp_msg.header.stamp = ros::Time::now();
    temp_msg.header.frame_id = sensor->getFrame();

    temp_msg.temperature = temperature / 100.0;
    temp_msg.variance = 0;

    // publish Temperature msg to ros
    sensor->getPub().publish(temp_msg);
  }
}