本文整理汇总了C++中ROS_ASSERT函数的典型用法代码示例。如果您正苦于以下问题:C++ ROS_ASSERT函数的具体用法?C++ ROS_ASSERT怎么用?C++ ROS_ASSERT使用的例子?那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了ROS_ASSERT函数的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: 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 );
}
}
开发者ID:F34140r,项目名称:visualization-userfriendly,代码行数:58,代码来源:line_strip_marker.cpp
示例2: ROS_ASSERT
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];
}
}
开发者ID:CaptD,项目名称:stomp,代码行数:9,代码来源:stomp_optimization_task.cpp
示例3: ServoStatePublisher
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);
}
开发者ID:2016UAVClass,项目名称:mavros-nsf-student-competition,代码行数:57,代码来源:servo_state_publisher.cpp
示例4: ROS_ASSERT
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());
}
}
开发者ID:haidai,项目名称:navigation,代码行数:9,代码来源:voxel_layer.cpp
示例5: ROS_ASSERT
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;
}
开发者ID:pragmaticTNT,项目名称:autonomy_hri,代码行数:57,代码来源:cleggrid.cpp
示例6: ROS_ASSERT
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);
}
开发者ID:HiroyukiMikita,项目名称:usc-clmc-ros-pkg,代码行数:18,代码来源:forward_kinematics.cpp
示例7: bind
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));
}
开发者ID:rosmod,项目名称:rosmod-comm,代码行数:18,代码来源:rosmod_xmlrpc_manager.cpp
示例8: ROS_ASSERT
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;
}
开发者ID:HiroyukiMikita,项目名称:usc-clmc-ros-pkg,代码行数:44,代码来源:nc2010_transformation_system.cpp
示例9: ROS_ASSERT
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;
}
开发者ID:robotambassador,项目名称:robot-ambassadors,代码行数:44,代码来源:transport_tcp.cpp
示例10: ROS_ASSERT
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;
}
开发者ID:GKIFreiburg,项目名称:pr2_tidyup,代码行数:10,代码来源:control_torso.cpp
示例11: ROS_ASSERT
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);
}
开发者ID:florianhauer,项目名称:GT_mini_autonomous_car,代码行数:10,代码来源:pwm_sysfs_driver_node.cpp
示例12: ROS_ASSERT
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());
}
开发者ID:rosmod,项目名称:rosmod-comm,代码行数:10,代码来源:rosmod_service_server_link.cpp
示例13: ROS_ASSERT
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;
}
}
开发者ID:limo001,项目名称:msg_translation,代码行数:55,代码来源:MsgTranslationWrapper.cpp
示例14: main
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;
}
开发者ID:GKIFreiburg,项目名称:pr2_tidyup,代码行数:55,代码来源:lift_torso.cpp
示例15: ROS_ERROR
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;
}
开发者ID:hgaiser,项目名称:shared_serial,代码行数:55,代码来源:server.cpp
示例16: ROS_ASSERT
bool CovariantMovementPrimitive::updateParameters(const std::vector<Eigen::MatrixXd>& updates, const std::vector<Eigen::VectorXd>& time_step_weights)
{
ROS_ASSERT(int(updates.size()) == num_dimensions_);
// this takes only the diagonal elements
/*for (int d=0; d<num_dimensions_; ++d)
{
parameters_all_[d].segment(free_vars_start_index_, num_vars_free_) += updates[d].diagonal();
}*/
// this averages all the updates
//double divisor = 1.0 / num_vars_free_;
double divisor = 1.0;
for (int d=0; d<num_dimensions_; ++d)
{
parameters_all_[d].segment(free_vars_start_index_, num_vars_free_).transpose() +=
divisor * updates[d].row(0);
}
// for (int d=0; d<num_dimensions_; ++d)
// {
// double weight = 0.0;
// double weight_sum = 0.0;
//
// Eigen::VectorXd update = Eigen::VectorXd::Zero(num_vars_free_);
// for (int t=0; t<num_time_steps_; ++t)
// {
// weight = time_step_weights[d][t];
// weight_sum += weight;
// update.transpose() += updates[d].row(t) * weight;
// //ROS_INFO_STREAM("Update at time " << t << " = " << updates[d].row(t));
// }
// if (weight_sum <1e-6)
// weight_sum = 1e-6;
// parameters_all_[d].segment(free_vars_start_index_, num_vars_free_) += (1.0/weight_sum)*update;
// }
// this weights updates by number of time-steps remaining:
// for (int d=0; d<num_dimensions_; ++d)
// {
// double weight=0.0;
// double weight_sum=0.0;
// Eigen::VectorXd update = Eigen::VectorXd::Zero(num_vars_free_);
// for (int t=0; t<num_time_steps_; ++t)
// {
// weight = double(num_time_steps_ - t);
// weight_sum += weight;
// update.transpose() += updates[d].row(t) * weight;
// //ROS_INFO_STREAM("Update at time " << t << " = " << updates[d].row(t));
// }
// parameters_all_[d].segment(free_vars_start_index_, num_vars_free_) += (1.0/weight_sum)*update;
// }
return true;
}
开发者ID:HiroyukiMikita,项目名称:usc-clmc-ros-pkg,代码行数:55,代码来源:covariant_movement_primitive.cpp
示例17: quit_at_end_
SegmentationVisualizer::SegmentationVisualizer(const std::vector<KinectCloud::Ptr>& original,
const std::vector<KinectCloud::Ptr>& segmented) :
quit_at_end_(false),
vis_("Segmentation"),
original_(original),
segmented_(segmented),
idx_(0),
stepping_(false),
recording_(false)
{
ROS_ASSERT(!original_.empty());
ROS_ASSERT(original_.size() == segmented_.size());
vis_.registerKeyboardCallback(&SegmentationVisualizer::keyboardEventOccurred, *this);
vis_.setBackgroundColor(255, 255, 255);
PointCloudColorHandlerCustom<PointXYZRGB> single_color(original_[0], 0, 0, 0);
vis_.addPointCloud(original_[0], single_color, "original");
vis_.addPointCloud(segmented_[0], "segmented");
vis_.setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 1, "original");
vis_.setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 4, "segmented");
// -- Hardcode the camera for the Kinect.
ROS_WARN("[SegmentationVisualizer] PCLVisualizer's camera param has been removed. Initial camera pose needs to be set somehow...");
// vis_.camera_.clip[0] = 0.00387244;
// vis_.camera_.clip[1] = 3.87244;
// vis_.camera_.focal[0] = -0.160878;
// vis_.camera_.focal[1] = -0.0444743;
// vis_.camera_.focal[2] = 1.281;
// vis_.camera_.pos[0] = 0.0402195;
// vis_.camera_.pos[1] = 0.0111186;
// vis_.camera_.pos[2] = -1.7;
// vis_.camera_.view[0] = 0;
// vis_.camera_.view[1] = -1;
// vis_.camera_.view[2] = 0;
// vis_.camera_.window_size[0] = 1678;
// vis_.camera_.window_size[1] = 525;
// vis_.camera_.window_pos[0] = 2;
// vis_.camera_.window_pos[1] = 82;
// vis_.updateCamera();
draw();
}
开发者ID:chen0510566,项目名称:asp,代码行数:42,代码来源:segmentation_visualizer.cpp
示例18: sprintf
// Constructor
Propulsion::Propulsion(ModelPlane * parent, int ID)
{
parentObj = parent;
id = ID;
XmlRpc::XmlRpcValue list;
int i;
char paramMsg[50];
sprintf(paramMsg, "motor%i/CGOffset", id);
if(!ros::param::getCached(paramMsg, list)) {ROS_FATAL("Invalid parameters for -%s- in param server!", paramMsg); ros::shutdown();}
for (i = 0; i < list.size(); ++i) {
ROS_ASSERT(list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
}
CGOffset.x = list[0];
CGOffset.y = list[1];
CGOffset.z = list[2];
sprintf(paramMsg, "motor%i/mountOrientation", id);
if(!ros::param::getCached(paramMsg, list)) {ROS_FATAL("Invalid parameters for -%s- in param server!", paramMsg); ros::shutdown();}
for (i = 0; i < list.size(); ++i) {
ROS_ASSERT(list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
}
// !!! Order mixed because tf::Quaternion::setEuler seems to work with PRY, instead of YPR
mountOrientation.y = list[0];
mountOrientation.z = list[1];
mountOrientation.x = list[2];
theta = 0; // Initialize propeller angle
sprintf(paramMsg, "motor%i/chanMotor", id);
if(!ros::param::getCached(paramMsg, chanMotor)) {ROS_INFO("No MOTOR%i channel selected", id); chanMotor=-1;}
sprintf(paramMsg, "motor%i/chanGimbal", id);
if(!ros::param::getCached(paramMsg, chanGimbal)) {ROS_INFO("No GIMBAL%i channel selected", id); chanGimbal=-1;}
sprintf(paramMsg, "motor%i/gimbalAngle_max", id);
if(!ros::param::getCached(paramMsg, gimbalAngle_max)) {ROS_INFO("No GIMBALANGLE_MAX%i value selected", id); gimbalAngle_max=0.0;}
inputMotor = 0.0;
inputGimbal = 0.0;
sprintf(paramMsg, "motor%i/rotationDir", id);
if(!ros::param::getCached(paramMsg, rotationDir)) {ROS_INFO("No ROTATION_DIR%i value selected", id); rotationDir=1.0;}
}
开发者ID:jdrusso,项目名称:last_letter,代码行数:43,代码来源:propulsionLib.cpp
示例19: ROS_ASSERT
FkLookup::ChainFK::ChainFK(const KDL::Tree& tree, const std::string& root, const std::string& tip):root_name(root),tip_name(tip){
KDL::Chain chain;
tree.getChain(root_name, tip_name, chain);
solver = new KDL::ChainFkSolverPos_recursive(chain);
unsigned int num = chain.getNrOfSegments();
for(unsigned int i = 0; i < num; ++i){
const KDL::Joint &joint = chain.getSegment(i).getJoint();
if (joint.getType() != KDL::Joint::None) joints.insert(std::make_pair(joint.getName(),joints.size()));
}
ROS_ASSERT(joints.size() == chain.getNrOfJoints());
}
开发者ID:ipa-fmw,项目名称:cob_manipulation,代码行数:11,代码来源:ik_wrapper.cpp
示例20: ROS_ASSERT
void RobotBase2DPoseDisplay::targetFrameChanged()
{
ROS_ASSERT( messages_.size() == arrows_.size() );
V_RobotBase2DOdom::iterator msg_it = messages_.begin();
V_Arrow::iterator arrow_it = arrows_.begin();
V_RobotBase2DOdom::iterator msg_end = messages_.end();
for ( ; msg_it != msg_end; ++msg_it, ++arrow_it )
{
transformArrow( *msg_it, *arrow_it );
}
}
开发者ID:janfrs,项目名称:kwc-ros-pkg,代码行数:11,代码来源:robot_base2d_pose_display.cpp
注:本文中的ROS_ASSERT函数示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论