Skip to content

Commit

Permalink
Merge pull request moveit#38 from AcutronicRobotics/robot_model_loader
Browse files Browse the repository at this point in the history 
Port Robot model loader
  • Loading branch information
Víctor Mayoral Vilches committed Apr 14, 2019
2 parents 64ea5af + bd6ae32 commit 09571cb
Show file tree
Hide file tree
Showing 3 changed files with 48 additions and 26 deletions.
2 changes: 1 addition & 1 deletion moveit_ros/planning/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,7 @@ include_directories(SYSTEM
add_subdirectory(rdf_loader)
add_subdirectory(collision_plugin_loader)
add_subdirectory(kinematics_plugin_loader)
# add_subdirectory(robot_model_loader)
add_subdirectory(robot_model_loader)
# add_subdirectory(constraint_sampler_manager_loader)
# add_subdirectory(planning_pipeline)
# add_subdirectory(planning_request_adapter_plugins)
Expand Down
14 changes: 11 additions & 3 deletions moveit_ros/planning/robot_model_loader/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,15 @@ endif()

add_library(${MOVEIT_LIB_NAME} src/robot_model_loader.cpp)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
target_link_libraries(${MOVEIT_LIB_NAME} moveit_rdf_loader moveit_kinematics_plugin_loader ${catkin_LIBRARIES})
ament_target_dependencies(${MOVEIT_LIB_NAME}
moveit_rdf_loader
moveit_kinematics_plugin_loader
rclcpp
urdf)

install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION})
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)

install(DIRECTORY include/ DESTINATION include)
58 changes: 36 additions & 22 deletions moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,9 +36,11 @@

#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/profiler/profiler.h>
#include <ros/ros.h>
#include "rclcpp/rclcpp.hpp"
#include <typeinfo>

rclcpp::Logger LOGGER = rclcpp::get_logger("planning").get_child("robot_model_loader");

namespace robot_model_loader
{
RobotModelLoader::RobotModelLoader(const std::string& robot_description, bool load_kinematics_solvers)
Expand Down Expand Up @@ -70,15 +72,19 @@ namespace
bool canSpecifyPosition(const robot_model::JointModel* jmodel, const unsigned int index)
{
bool ok = false;
if (jmodel->getType() == robot_model::JointModel::PLANAR && index == 2)
ROS_ERROR("Cannot specify position limits for orientation of planar joint '%s'", jmodel->getName().c_str());
else if (jmodel->getType() == robot_model::JointModel::FLOATING && index > 2)
ROS_ERROR("Cannot specify position limits for orientation of floating joint '%s'", jmodel->getName().c_str());
if (jmodel->getType() == robot_model::JointModel::PLANAR && index == 2){
RCLCPP_ERROR(LOGGER,"Cannot specify position limits for orientation of planar joint '%s'", jmodel->getName().c_str());
}
else if (jmodel->getType() == robot_model::JointModel::FLOATING && index > 2){
RCLCPP_ERROR(LOGGER,"Cannot specify position limits for orientation of floating joint '%s'", jmodel->getName().c_str());
}
else if (jmodel->getType() == robot_model::JointModel::REVOLUTE &&
static_cast<const robot_model::RevoluteJointModel*>(jmodel)->isContinuous())
ROS_ERROR("Cannot specify position limits for continuous joint '%s'", jmodel->getName().c_str());
else
static_cast<const robot_model::RevoluteJointModel*>(jmodel)->isContinuous()){
RCLCPP_ERROR(LOGGER,"Cannot specify position limits for continuous joint '%s'", jmodel->getName().c_str());
}
else{
ok = true;
}
return ok;
}
} // namespace
Expand All @@ -88,7 +94,9 @@ void RobotModelLoader::configure(const Options& opt)
moveit::tools::Profiler::ScopedStart prof_start;
moveit::tools::Profiler::ScopedBlock prof_block("RobotModelLoader::configure");

ros::WallTime start = ros::WallTime::now();
auto node = rclcpp::Node::make_shared("additional_joints");
rclcpp::Clock clock;
rclcpp::Time start = clock.now();
if (!opt.urdf_string_.empty() && !opt.srdf_string_.empty())
rdf_loader_.reset(new rdf_loader::RDFLoader(opt.urdf_string_, opt.srdf_string_));
else
Expand All @@ -105,7 +113,8 @@ void RobotModelLoader::configure(const Options& opt)
moveit::tools::Profiler::ScopedBlock prof_block2("RobotModelLoader::configure joint limits");

// if there are additional joint limits specified in some .yaml file, read those in
ros::NodeHandle nh("~");
// TODO (anasarrak): Add the correct node name to the .yaml
auto additional_joints_parameters = std::make_shared<rclcpp::SyncParametersClient>(node);

for (std::size_t i = 0; i < model_->getJointModels().size(); ++i)
{
Expand All @@ -116,41 +125,47 @@ void RobotModelLoader::configure(const Options& opt)
std::string prefix = rdf_loader_->getRobotDescription() + "_planning/joint_limits/" + jlim[j].joint_name + "/";

double max_position;
if (nh.getParam(prefix + "max_position", max_position))
if (additional_joints_parameters->has_parameter(prefix + "max_position"))
{
max_position = node->get_parameter(prefix + "max_position").get_value<double>();
if (canSpecifyPosition(jmodel, j))
{
jlim[j].has_position_limits = true;
jlim[j].max_position = max_position;
}
}
double min_position;
if (nh.getParam(prefix + "min_position", min_position))
if (additional_joints_parameters->has_parameter(prefix + "min_position"))
{
min_position = node->get_parameter(prefix + "min_position").get_value<double>();
if (canSpecifyPosition(jmodel, j))
{
jlim[j].has_position_limits = true;
jlim[j].min_position = min_position;
}
}
double max_velocity;
if (nh.getParam(prefix + "max_velocity", max_velocity))
if (additional_joints_parameters->has_parameter(prefix + "max_velocity"))
{
max_velocity = node->get_parameter(prefix + "max_velocity").get_value<double>();
jlim[j].has_velocity_limits = true;
jlim[j].max_velocity = max_velocity;
}
bool has_vel_limits;
if (nh.getParam(prefix + "has_velocity_limits", has_vel_limits))
if (additional_joints_parameters->has_parameter(prefix + "has_velocity_limits"))
has_vel_limits = node->get_parameter(prefix + "has_velocity_limits").get_value<bool>();
jlim[j].has_velocity_limits = has_vel_limits;

double max_acc;
if (nh.getParam(prefix + "max_acceleration", max_acc))
if (additional_joints_parameters->has_parameter(prefix + "max_acceleration"))
{
max_acc = node->get_parameter(prefix + "max_acceleration").get_value<double>();
jlim[j].has_acceleration_limits = true;
jlim[j].max_acceleration = max_acc;
}
bool has_acc_limits;
if (nh.getParam(prefix + "has_acceleration_limits", has_acc_limits))
if (additional_joints_parameters->has_parameter(prefix + "has_acceleration_limits"))
has_acc_limits = node->get_parameter(prefix + "has_acceleration_limits").get_value<bool>();
jlim[j].has_acceleration_limits = has_acc_limits;
}
jmodel->setVariableBounds(jlim);
Expand All @@ -160,8 +175,7 @@ void RobotModelLoader::configure(const Options& opt)
if (model_ && opt.load_kinematics_solvers_)
loadKinematicsSolvers();

ROS_DEBUG_STREAM_NAMED("robot_model_loader", "Loaded kinematic model in " << (ros::WallTime::now() - start).toSec()
<< " seconds");
RCLCPP_DEBUG(node->get_logger(), "Loaded kinematic model in %d seconds", (clock.now() - start).seconds());
}

void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::KinematicsPluginLoaderPtr& kloader)
Expand All @@ -181,9 +195,9 @@ void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::Kin
const std::vector<std::string>& groups = kinematics_loader_->getKnownGroups();
std::stringstream ss;
std::copy(groups.begin(), groups.end(), std::ostream_iterator<std::string>(ss, " "));
ROS_DEBUG_STREAM("Loaded information about the following groups: '" << ss.str() << "'");
RCLCPP_DEBUG(LOGGER,"Loaded information about the following groups: '%S' ", ss.str().c_str());
if (groups.empty() && !model_->getJointModelGroups().empty())
ROS_WARN("No kinematics plugins defined. Fill and load kinematics.yaml!");
RCLCPP_WARN(LOGGER,"No kinematics plugins defined. Fill and load kinematics.yaml!");

std::map<std::string, robot_model::SolverAllocatorFn> imap;
for (std::size_t i = 0; i < groups.size(); ++i)
Expand All @@ -204,13 +218,13 @@ void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::Kin
}
else
{
ROS_ERROR("Kinematics solver %s does not support joint group %s. Error: %s", typeid(*solver).name(),
RCLCPP_ERROR(LOGGER,"Kinematics solver %s does not support joint group %s. Error: %s", typeid(*solver).name(),
groups[i].c_str(), error_msg.c_str());
}
}
else
{
ROS_ERROR("Kinematics solver could not be instantiated for joint group %s.", groups[i].c_str());
RCLCPP_ERROR(LOGGER,"Kinematics solver could not be instantiated for joint group %s.", groups[i].c_str());
}
}
model_->setKinematicsAllocators(imap);
Expand Down

0 comments on commit 09571cb

Please sign in to comment.