C++ ROS_ASSERT函数代码示例

void LineStripMarker::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
{
    ROS_ASSERT(new_message->type == visualization_msgs::Marker::LINE_STRIP);

    if (!lines_)
    {
        lines_ = new BillboardLine(context_->getSceneManager(), scene_node_);
    }

    Ogre::Vector3 pos, scale;
    Ogre::Quaternion orient;
    transform(new_message, pos, orient, scale);

    setPosition(pos);
    setOrientation(orient);
    lines_->setScale(scale);
    lines_->setColor(new_message->color.r, new_message->color.g, new_message->color.b, new_message->color.a);

    lines_->clear();
    if (new_message->points.empty())
    {
        return;
    }

    lines_->setLineWidth(new_message->scale.x);
    lines_->setMaxPointsPerLine(new_message->points.size());

    bool has_per_point_color = new_message->colors.size() == new_message->points.size();

    size_t i = 0;
    std::vector<geometry_msgs::Point>::const_iterator it = new_message->points.begin();
    std::vector<geometry_msgs::Point>::const_iterator end = new_message->points.end();
    for ( ; it != end; ++it, ++i )
    {
        const geometry_msgs::Point& p = *it;

        Ogre::Vector3 v( p.x, p.y, p.z );

        Ogre::ColourValue c;
        if (has_per_point_color)
        {
            const std_msgs::ColorRGBA& color = new_message->colors[i];
            c.r = color.r;
            c.g = color.g;
            c.b = color.b;
            c.a = color.a * new_message->color.a;
        }
        else
        {
            c.r = new_message->color.r;
            c.g = new_message->color.g;
            c.b = new_message->color.b;
            c.a = new_message->color.a;
        }

        lines_->addPoint( v, c );
    }
}

void StompOptimizationTask::setFeatureWeights(const std::vector<double>& weights)
{
  ROS_ASSERT((int)weights.size() == num_split_features_);
  feature_weights_ = Eigen::VectorXd::Zero(weights.size());
  for (unsigned int i=0; i<weights.size(); ++i)
  {
    feature_weights_(i) = weights[i];
  }
}

	ServoStatePublisher() :
		nh()
	{
		ros::NodeHandle priv_nh("~");

		XmlRpc::XmlRpcValue param_dict;
		priv_nh.getParam("", param_dict);

		ROS_ASSERT(param_dict.getType() == XmlRpc::XmlRpcValue::TypeStruct);

		urdf::Model model;
		model.initParam("robot_description");
		ROS_INFO("SSP: URDF robot: %s", model.getName().c_str());

		for (auto &pair : param_dict) {
			ROS_DEBUG("SSP: Loading joint: %s", pair.first.c_str());

			// inefficient, but easier to program
			ros::NodeHandle pnh(priv_nh, pair.first);

			bool rc_rev;
			int rc_channel, rc_min, rc_max, rc_trim, rc_dz;

			if (!pnh.getParam("rc_channel", rc_channel)) {
				ROS_ERROR("SSP: '%s' should provice rc_channel", pair.first.c_str());
				continue;
			}

			pnh.param("rc_min", rc_min, 1000);
			pnh.param("rc_max", rc_max, 2000);
			if (!pnh.getParam("rc_trim", rc_trim)) {
				rc_trim = rc_min + (rc_max - rc_min) / 2;
			}

			pnh.param("rc_dz", rc_dz, 0);
			pnh.param("rc_rev", rc_rev, false);

			auto joint = model.getJoint(pair.first);
			if (!joint) {
				ROS_ERROR("SSP: URDF: there no joint '%s'", pair.first.c_str());
				continue;
			}
			if (!joint->limits) {
				ROS_ERROR("SSP: URDF: joint '%s' should provide <limit>", pair.first.c_str());
				continue;
			}

			double lower = joint->limits->lower;
			double upper = joint->limits->upper;

			servos.emplace_back(pair.first, lower, upper, rc_channel, rc_min, rc_max, rc_trim, rc_dz, rc_rev);
			ROS_INFO("SSP: joint '%s' (RC%d) loaded", pair.first.c_str(), rc_channel);
		}

		rc_out_sub = nh.subscribe("rc_out", 10, &ServoStatePublisher::rc_out_cb, this);
		joint_states_pub = nh.advertise<sensor_msgs::JointState>("joint_states", 10);
	}

void VoxelLayer::matchSize()
{
  ObstacleLayer::matchSize();
  voxel_grid_.resize(size_x_, size_y_, size_z_);
  ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);
  if(clear_old_){
    locations_utime.resize(voxel_grid_.sizeX() * voxel_grid_.sizeY());
  }
}

void CLegGrid::filterStates()
{

    ROS_ASSERT(cmeas_.size() == cstate_.size());

    std::vector<PolarPose> fstate, fmeas;
    PolarPose nstate, s1, s2, nmeas, m1, m2;

    /* check for close states */
    if(cstate_.size() < 2)
    {
        fstate = cstate_;
        fmeas = cmeas_;
    }
    else
    {
        if(!fstate.empty()) fstate.clear();
        fstate.push_back(cstate_.at(0));

        if(!fmeas.empty()) fmeas.clear();
        fmeas.push_back(cmeas_.at(0));

        for(uint8_t i = 1; i < cstate_.size(); i++)
        {
            s1 = cstate_.at(i);
            s2 = fstate.back();

            m1 = cmeas_.at(i);
            m2 = fmeas.back();

            if(s1.distance(s2) < 1.0)
            {
                fstate.pop_back();
                nstate.range = (s1.range + s2.range) / 2;
                nstate.angle = (s1.angle + s2.angle) / 2;
                nstate.setZeroVar();
                fstate.push_back(nstate);

                fmeas.pop_back();
                nmeas.range = (m1.range + m2.range) / 2;
                nmeas.angle = (m1.angle + m2.angle) / 2;
                fmeas.push_back(nstate);

            } else
            {
                fstate.push_back(s1);
                fmeas.push_back(m1);
            }
        }
    }

    if(!cstate_.empty()) cstate_.clear();
    cstate_ = fstate;

    if(!cmeas_.empty()) cmeas_.clear();
    cmeas_ = fmeas;
}

void ForwardKinematics::forwardKinematics(const std::vector<double>& joint_positions, geometry_msgs::Pose& pose)
{
  ROS_ASSERT(initialized_);
  ROS_ASSERT(int(joint_positions.size()) == chain_.getNumJoints());
  KDL::Frame frame;
  for (int i=0; i<chain_.getNumJoints(); ++i)
  {
    kdl_joint_array_(i) = joint_positions[i];
  }
  chain_.forwardKinematics(kdl_joint_array_, frame);
  pose.position.x = frame.p.x();
  pose.position.y = frame.p.y();
  pose.position.z = frame.p.z();
  frame.M.GetQuaternion(pose.orientation.x,
                        pose.orientation.y,
                        pose.orientation.z,
                        pose.orientation.w);
}

void XMLRPCManager::start()
{
  shutting_down_ = false;
  port_ = 0;
  bind("getPid", getPid);

  bool bound = server_.bindAndListen(0);
  (void) bound;
  ROS_ASSERT(bound);
  port_ = server_.get_port();
  ROS_ASSERT(port_ != 0);

  std::stringstream ss;
  ss << "http://" << network::getHost() << ":" << port_ << "/";
  uri_ = ss.str();

  server_thread_ = boost::thread(boost::bind(&XMLRPCManager::serverThreadFunc, this));
}

bool NC2010TransformationSystem::initOneDimensionalTransformationSystemHelper(std::vector<dmp_lib::NC2010TSPtr>& transformation_systems,
                                                                                XmlRpc::XmlRpcValue transformation_systems_parameters_xml,
                                                                                ros::NodeHandle& node_handle,
                                                                                dmp_lib::TransformationSystem::IntegrationMethod integration_method)
{
  ROS_ASSERT(transformation_systems_parameters_xml.getType() == XmlRpc::XmlRpcValue::TypeArray);
  for (int j = 0; j < transformation_systems_parameters_xml.size(); ++j)
  {
    ROS_DEBUG("Initializing NC2010 transformation system number >%i< from node handle.",j);

    // create transformation system
    dmp_lib::NC2010TSPtr transformation_system(new dmp_lib::NC2010TransformationSystem());

    XmlRpc::XmlRpcValue ts_xml = transformation_systems_parameters_xml[j];
    ROS_ASSERT(ts_xml.hasMember(transformation_system->getVersionString()));
    XmlRpc::XmlRpcValue nc2010_ts_xml = ts_xml[transformation_system->getVersionString()];

    double k_gain = 0;
    if (!usc_utilities::getParam(nc2010_ts_xml, "k_gain", k_gain))
    {
      return false;
    }
    double d_gain = dmp_lib::NC2010TransformationSystem::getDGain(k_gain);

    // create parameters
    dmp_lib::NC2010TSParamPtr parameters(new dmp_lib::NC2010TransformationSystemParameters());

    // initialize parameters
    ROS_VERIFY(parameters->initialize(k_gain, d_gain));

    // initialize base class
    ROS_VERIFY(TransformationSystem::initFromNodeHandle(parameters, ts_xml, node_handle));

    // create state
    dmp_lib::NC2010TSStatePtr state(new dmp_lib::NC2010TransformationSystemState());

    // initialize transformation system with parameters and state
    ROS_VERIFY(transformation_system->initialize(parameters, state, integration_method));
    ROS_VERIFY(transformation_system->setIntegrationMethod(integration_method));

    transformation_systems.push_back(transformation_system);
  }
  return true;
}

bool TransportTCP::initializeSocket()
{
    ROS_ASSERT(sock_ != ROS_INVALID_SOCKET);

    if (!setNonBlocking())
    {
        return false;
    }

    setKeepAlive(s_use_keepalive_, 60, 10, 9);

    // connect() will set cached_remote_host_ because it already has the host/port available
    if (cached_remote_host_.empty())
    {
        if (is_server_)
        {
            cached_remote_host_ = "TCPServer Socket";
        }
        else
        {
            std::stringstream ss;
            ss << getClientURI() << " on socket " << sock_;
            cached_remote_host_ = ss.str();
        }
    }

#ifdef ROSCPP_USE_TCP_NODELAY
    setNoDelay(true);
#endif

    ROS_ASSERT(poll_set_ || (flags_ & SYNCHRONOUS));
    if (poll_set_)
    {
        ROS_DEBUG("Adding tcp socket [%d] to pollset", sock_);
        poll_set_->addSocket(sock_, boost::bind(&TransportTCP::socketUpdate, this, _1), shared_from_this());
    }

    if (!(flags_ & SYNCHRONOUS))
    {
        //enableRead();
    }

    return true;
}

	bool ControlTorso::torsoLiftMin(control_robot_msgs::MoveIt::Request &req,
			control_robot_msgs::MoveIt::Response &res)
	{
		std::vector<std::string> torso_joints = torso_group_->getJoints();
		ROS_ASSERT(torso_joints.size() > 0);
		double min_position = torso_group_->getCurrentState().get()->getJointModel(torso_joints[0])->getVariableBounds()[0].min_position_;
		moveit::planning_interface::MoveItErrorCode error_code;
		error_code = moveTorsoJointPosition(min_position);
		return error_code == moveit::planning_interface::MoveItErrorCode::SUCCESS;
	}

void PwmSysfsDriverNode::setDuty(double duty)
{
  ROS_ASSERT(driver_);
  if (duty >= 0 && duty <= 1) {
    ROS_DEBUG("Setting PWM duty cycle to: %f", duty);
    driver_->duty_fraction(duty);
  }
  else
    ROS_WARN("Commanded PWM duty cycle value (%f) invalid, ignoring.", duty);
}

void ServiceServerLink::onConnectionDropped(const ConnectionPtr& conn)
{
  ROS_ASSERT(conn == connection_);
  ROSCPP_LOG_DEBUG("Service client from [%s] for [%s] dropped", conn->getRemoteString().c_str(), service_name_.c_str());

  dropped_ = true;
  clearCalls();

  ServiceManager::instance()->removeServiceServerLink(shared_from_this());
}

void MsgTranslationWrapper::updateGripperCommand()
{
/*
             * NOTE: gripper slide rails are always symmetric, but the fingers can be screwed in different
             * positions! The published values account for the distance between the gripper slide rails, not the fingers
             * themselves. Of course if the finger are screwed to the most inner position (i.e. the can close completely),
             * than it is correct.
             */
for (int armIndex = 0; armIndex < static_cast<int> (nodeConfiguration.nodeArmConfigurations.size()); armIndex++)
       {

           ROS_ASSERT(nodeConfiguration.nodeArmConfigurations.size() == gripperTrajectoryMessages.size());  // gripperTrajectoryMessages
           
           gripperTrajectoryMessages[armIndex].joint_names.resize(NumberOfFingers);

           gripperTrajectoryMessages[armIndex].joint_names[0] = "gripper_finger_joint_l";
           gripperTrajectoryMessages[armIndex].joint_names[1] = "gripper_finger_joint_r";
          

             trajectory_msgs::JointTrajectoryPoint  pointGripper ;

             pointGripper.positions.resize(NumberOfFingers);
             pointGripper.velocities.resize(NumberOfFingers);
             pointGripper.effort.resize(NumberOfFingers);
       
            if(static_cast<int>(currentGripperPos.size()) == NumberOfFingers)
              {           
                     for (int i = 0; i < NumberOfFingers; ++i)
                     {
                        pointGripper.positions[i] = currentGripperPos[i] ;
                       
                       }                   
                }else{                       
                        pointGripper.positions[0] =  0.05004093943948895;
                        pointGripper.positions[1] =  0.01294137360159624;                   
               }
             
                        pointGripper.velocities[0] = -0.03175738364753859;
                        pointGripper.velocities[1] = 1.019040908525729466;
                        
              
                        pointGripper.effort[0] = -0.034732168550673986;
                        pointGripper.effort[1] = 0.43880198765294803;
                        

                pointGripper.time_from_start = ros::Duration(0.1);

                gripperTrajectoryMessages[armIndex].points.resize(1);   
                gripperTrajectoryMessages[armIndex].points[0] = pointGripper;//.push_back(point);
                gripperTrajectoryMessages[armIndex].header.stamp = currentTime;  


  }
    
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "lift_torso");

	ros::NodeHandle nh;
	ros::AsyncSpinner spinner(1);
	spinner.start();

	double position;
	if (argc != 2)
	{
		ROS_ERROR("Usage: rosrun control_robot lift_torso <torso_position>");
		return 1;
	}
	position = atof(argv[1]);
	ROS_INFO("Set torso position to %lf", position);

	moveit::planning_interface::MoveGroup torso_group("torso");
	std::vector<std::string> torso_joints = torso_group.getJoints();
	ROS_ASSERT(torso_joints.size() > 0);
	if (!torso_group.setJointValueTarget(torso_joints[0], position))
	{
		ROS_ERROR("Position out of bounds - not moving torso");
		return 1;
	}
	moveit::planning_interface::MoveItErrorCode error_code;
	ROS_INFO("Lifting torso...");
	error_code = torso_group.move();
	ros::shutdown();
	return error_code == moveit::planning_interface::MoveItErrorCode::SUCCESS;

//	//ros::ServiceClient torso_client = nh.serviceClient<control_robot_msgs::MoveIt>("/control_robot/torso_lift_max");
//	ros::ServiceClient torso_client = nh.serviceClient<control_robot_msgs::MoveItPosition>("/control_robot/torso_lift");
//	control_robot_msgs::MoveItPosition srv;
//	srv.request.position.data = position;
//
//	ROS_INFO("Wait for existence of service: %s", torso_client.getService().c_str());
//	torso_client.waitForExistence();
//
//	if (!torso_client.exists())
//		ROS_ERROR("Client %s does not exist.", torso_client.getService().c_str());
//
//	ROS_INFO("Lifting torso...");
//
//	if (!torso_client.call(srv))
//	{
//		ROS_ERROR("Could not send service message to client: %s", torso_client.getService().c_str());
//		return 1;
//	}
//	ROS_INFO("Service call for torso was successful.");

//	ros::shutdown();
//	return 0;

}

bool SerialServer::callbackSendRecv(shared_serial::SendRecv::Request& req, shared_serial::SendRecv::Response& res)
{
  int socket = lock_.lock(req.socket, req.sock_timeout);
  if (socket < 0) return false;

  // *** Send ***
  unsigned char *buf = new unsigned char[req.send_data.size()];
  for (unsigned int ii=0; ii != req.send_data.size(); ++ii)
    buf[ii] = req.send_data[ii];
    
  int n = serial_port_.port_write(buf, req.send_data.size());
  delete buf;
  
  if (n != (int)req.send_data.size())
  {
    ROS_ERROR("Truncated send, flushing port");
    serial_port_.flush_buffer();
    
    lock_.unlock();
    return false;
  }

  ROS_DEBUG_STREAM("Sent " << n << " bytes");

  // *** Receive ***
  ROS_ASSERT(req.length <= 65536);

  buf = new unsigned char[req.length];
  n = serial_port_.port_read(buf, req.length, (int)req.recv_timeout, (int)((req.recv_timeout-(int)req.recv_timeout)*1000000));
  
  if (n < 0)
  {
    ROS_ERROR("Error while reading serial port");
    delete buf;

    serial_port_.flush_buffer();
    usleep(10);
    serial_port_.flush_buffer();
    
    lock_.unlock();
    return false;
  }
  
  ROS_DEBUG_STREAM("Read " << n << " bytes");
  
  res.recv_data.resize(n);
  for (int ii=0; ii != n; ++ii)
    res.recv_data[ii] = buf[ii];
  delete buf;
  
  res.socket = socket;
    
  lock_.unlock();    
  return true;
}