Skip to content

Commit d220774

Browse files
scchowscchow
committed
Update JointProperties access/tests
- Due to moveit/srdfdom#111, accessing Joint Properties has changed since properties now use a map instead of a vector. - Fixed tests/checking of joint properties to reflect this change
1 parent 30715a9 commit d220774

File tree

2 files changed

+31
-30
lines changed

2 files changed

+31
-30
lines changed

‎moveit_core/robot_model/src/robot_model.cpp

Lines changed: 30 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -968,35 +968,37 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const
968968
{
969969
new_joint_model->setDistanceFactor(new_joint_model->getStateSpaceDimension());
970970
const std::vector<srdf::Model::PassiveJoint>& pjoints = srdf_model.getPassiveJoints();
971+
std::string joint_name = new_joint_model->getName();
971972
for (const srdf::Model::PassiveJoint& pjoint : pjoints)
972973
{
973-
if (new_joint_model->getName() == pjoint.name_)
974+
if (joint_name == pjoint.name_)
974975
{
975976
new_joint_model->setPassive(true);
976977
break;
977978
}
978979
}
979-
for (const srdf::Model::JointProperty& property : srdf_model.getJointProperties(new_joint_model->getName()))
980+
for (const auto& property : srdf_model.getJointProperties(joint_name))
980981
{
981-
if (property.property_name_ == "angular_distance_weight")
982+
std::string property_name = property.first;
983+
std::string property_value = property.second;
984+
if (property_name == "angular_distance_weight")
982985
{
983986
double angular_distance_weight;
984987
try
985988
{
986989
std::string::size_type sz;
987-
angular_distance_weight = std::stod(property.value_, &sz);
988-
if (sz != property.value_.size())
990+
angular_distance_weight = std::stod(property_value, &sz);
991+
if (sz != property_value.size())
989992
{
990-
ROS_WARN_STREAM_NAMED(LOGNAME, "Extra characters after property "
991-
<< property.property_name_ << " for joint " << property.joint_name_
992-
<< " as double: '" << property.value_.substr(sz) << "'");
993+
ROS_WARN_STREAM_NAMED(LOGNAME, "Extra characters after property " << property_name << " for joint "
994+
<< joint_name << " as double: '"
995+
<< property_value.substr(sz) << "'");
993996
}
994997
}
995998
catch (const std::invalid_argument& e)
996999
{
997-
ROS_ERROR_STREAM_NAMED(LOGNAME, "Unable to parse property " << property.property_name_ << " for joint "
998-
<< property.joint_name_ << " as double: '"
999-
<< property.value_ << "'");
1000+
ROS_ERROR_STREAM_NAMED(LOGNAME, "Unable to parse property " << property_name << " for joint " << joint_name
1001+
<< " as double: '" << property_value << "'");
10001002
continue;
10011003
}
10021004

@@ -1010,71 +1012,70 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const
10101012
}
10111013
else
10121014
{
1013-
ROS_ERROR_NAMED(LOGNAME, "Cannot apply property %s to joint type: %s", property.property_name_.c_str(),
1015+
ROS_ERROR_NAMED(LOGNAME, "Cannot apply property %s to joint type: %s", property_name.c_str(),
10141016
new_joint_model->getTypeName().c_str());
10151017
}
10161018
}
1017-
else if (property.property_name_ == "motion_model")
1019+
else if (property_name == "motion_model")
10181020
{
10191021
if (new_joint_model->getType() != JointModel::JointType::PLANAR)
10201022
{
1021-
ROS_ERROR_NAMED(LOGNAME, "Cannot apply property %s to joint type: %s", property.property_name_.c_str(),
1023+
ROS_ERROR_NAMED(LOGNAME, "Cannot apply property %s to joint type: %s", property_name.c_str(),
10221024
new_joint_model->getTypeName().c_str());
10231025
continue;
10241026
}
10251027

10261028
PlanarJointModel::MotionModel motion_model;
1027-
if (property.value_ == "holonomic")
1029+
if (property_value == "holonomic")
10281030
{
10291031
motion_model = PlanarJointModel::MotionModel::HOLONOMIC;
10301032
}
1031-
else if (property.value_ == "diff_drive")
1033+
else if (property_value == "diff_drive")
10321034
{
10331035
motion_model = PlanarJointModel::MotionModel::DIFF_DRIVE;
10341036
}
10351037
else
10361038
{
1037-
ROS_ERROR_STREAM_NAMED(LOGNAME, "Unknown value for property " << property.property_name_ << " ("
1038-
<< property.joint_name_ << "): '"
1039-
<< property.value_ << "'");
1039+
ROS_ERROR_STREAM_NAMED(LOGNAME, "Unknown value for property " << property_name << " (" << joint_name << "): '"
1040+
<< property_value << "'");
10401041
ROS_ERROR_NAMED(LOGNAME, "Valid values are 'holonomic' and 'diff_drive'");
10411042
continue;
10421043
}
10431044

10441045
((PlanarJointModel*)new_joint_model)->setMotionModel(motion_model);
10451046
}
1046-
else if (property.property_name_ == "min_translational_distance")
1047+
else if (property_name == "min_translational_distance")
10471048
{
10481049
if (new_joint_model->getType() != JointModel::JointType::PLANAR)
10491050
{
1050-
ROS_ERROR_NAMED(LOGNAME, "Cannot apply property %s to joint type: %s", property.property_name_.c_str(),
1051+
ROS_ERROR_NAMED(LOGNAME, "Cannot apply property %s to joint type: %s", property_name.c_str(),
10511052
new_joint_model->getTypeName().c_str());
10521053
continue;
10531054
}
10541055
double min_translational_distance;
10551056
try
10561057
{
10571058
std::string::size_type sz;
1058-
min_translational_distance = std::stod(property.value_, &sz);
1059-
if (sz != property.value_.size())
1059+
min_translational_distance = std::stod(property_value, &sz);
1060+
if (sz != property_value.size())
10601061
{
1061-
ROS_WARN_STREAM_NAMED(LOGNAME, "Extra characters after property "
1062-
<< property.property_name_ << " for joint " << property.joint_name_
1063-
<< " as double: '" << property.value_.substr(sz) << "'");
1062+
ROS_WARN_STREAM_NAMED(LOGNAME, "Extra characters after property " << property_name << " for joint "
1063+
<< joint_name << " as double: '"
1064+
<< property_value.substr(sz) << "'");
10641065
}
10651066
}
10661067
catch (const std::invalid_argument& e)
10671068
{
1068-
ROS_ERROR_NAMED(LOGNAME, "Unable to parse property %s for joint %s as double: '%s'",
1069-
property.property_name_.c_str(), property.joint_name_.c_str(), property.value_.c_str());
1069+
ROS_ERROR_NAMED(LOGNAME, "Unable to parse property %s for joint %s as double: '%s'", property_name.c_str(),
1070+
joint_name.c_str(), property_value.c_str());
10701071
continue;
10711072
}
10721073

10731074
((PlanarJointModel*)new_joint_model)->setMinTranslationalDistance(min_translational_distance);
10741075
}
10751076
else
10761077
{
1077-
ROS_ERROR_NAMED(LOGNAME, "Unknown joint property: %s", property.property_name_.c_str());
1078+
ROS_ERROR_NAMED(LOGNAME, "Unknown joint property: %s", property_name.c_str());
10781079
}
10791080
}
10801081
}

‎moveit_core/utils/src/robot_model_test_utils.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -397,7 +397,7 @@ RobotModelBuilder& RobotModelBuilder::addEndEffector(const std::string& name, co
397397
void RobotModelBuilder::addJointProperty(const std::string& joint_name, const std::string& property_name,
398398
const std::string& value)
399399
{
400-
srdf_writer_->joint_properties_[joint_name].push_back({ joint_name, property_name, value });
400+
srdf_writer_->joint_properties_[joint_name][property_name] = value;
401401
}
402402

403403
bool RobotModelBuilder::isValid()

0 commit comments

Comments
 (0)