本文整理汇总了C++中robot_state::RobotState::getLinkModel方法的典型用法代码示例。如果您正苦于以下问题：C++ RobotState::getLinkModel方法的具体用法？C++ RobotState::getLinkModel怎么用？C++ RobotState::getLinkModel使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类robot_state::RobotState的用法示例。
在下文中一共展示了RobotState::getLinkModel方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: visualizeDistribution
void constraint_samplers::visualizeDistribution(const ConstraintSamplerPtr& sampler,
const robot_state::RobotState& reference_state,
const std::string& link_name, unsigned int sample_count,
visualization_msgs::MarkerArray& markers)
{
if (!sampler)
{
ROS_ERROR_NAMED("constraint_samplers", "No sampler specified for visualizing distribution of samples");
return;
}
const robot_state::LinkModel* lm = reference_state.getLinkModel(link_name);
if (!lm)
return;
robot_state::RobotState ks(reference_state);
std_msgs::ColorRGBA color;
color.r = 1.0f;
color.g = 0.0f;
color.b = 0.0f;
color.a = 1.0f;
for (unsigned int i = 0; i < sample_count; ++i)
{
if (!sampler->sample(ks))
continue;
const Eigen::Vector3d& pos = ks.getGlobalLinkTransform(lm).translation();
visualization_msgs::Marker mk;
mk.header.stamp = ros::Time::now();
mk.header.frame_id = sampler->getJointModelGroup()->getParentModel().getModelFrame();
mk.ns = "constraint_samples";
mk.id = i;
mk.type = visualization_msgs::Marker::SPHERE;
mk.action = visualization_msgs::Marker::ADD;
mk.pose.position.x = pos.x();
mk.pose.position.y = pos.y();
mk.pose.position.z = pos.z();
mk.pose.orientation.w = 1.0;
mk.scale.x = mk.scale.y = mk.scale.z = 0.035;
mk.color = color;
mk.lifetime = ros::Duration(30.0);
markers.markers.push_back(mk);
}
}