Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Port kinematics_base submodule moveit_core #8

Merged
merged 21 commits into from
Apr 22, 2019
Merged

Port kinematics_base submodule moveit_core #8

Show file tree
Hide file tree
Changes from 10 commits
Commits
Show all changes
21 commits
Select commit Hold shift + click to select a range
54e0542
Port kinematics_base submodule moveit_core
vmayoral Feb 19, 2019
791fee9
kinematics_base, use spaces instead of tabs
vmayoral Feb 24, 2019
066b300
Remove unnecessary cmake lines
vmayoral Feb 24, 2019
d5a108c
kinematics_base, remove redundancy in CMakeLists.txt
vmayoral Feb 24, 2019
50ea822
Undo documentation removal
vmayoral Feb 24, 2019
7469781
Remove dead code
vmayoral Feb 24, 2019
504df6d
remove dead code in kinematics_base upon request
vmayoral Feb 24, 2019
09512ba
kinematics_base, apply clang-format
vmayoral Feb 24, 2019
a415ab5
Use underscores for node names to increase readability
vmayoral Mar 14, 2019
43a77cf
Adapt logging
vmayoral Mar 14, 2019
ff1966e
Merge remote-tracking branch 'moveit2/master' into core-kinematics-base
vmayoral Mar 16, 2019
5984c03
Reverting change in CMakeLists.txt
vmayoral Mar 16, 2019
8b4f35c
Meet the LOGGER indications
vmayoral Mar 16, 2019
8257334
kinematics_base, clang format
vmayoral Mar 16, 2019
10fe1c1
Remove leftover in kinematics_base
vmayoral Mar 24, 2019
a8a03c3
Fix kinematics_base namespaces
vmayoral Mar 24, 2019
7f706b5
Do not use auto
vmayoral Mar 30, 2019
b2d436f
Comply partially with review request
vmayoral Mar 30, 2019
82b1bf4
Simplify lookupParam() in KinematicsBase
henningkayser Apr 16, 2019
887f05e
Merge branch 'master' into core-kinematics-base
henningkayser Apr 16, 2019
339c4f9
Fix result param array access
henningkayser Apr 16, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 9 additions & 8 deletions moveit_core/kinematics_base/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,18 +1,19 @@
cmake_minimum_required(VERSION 3.5)
set(MOVEIT_LIB_NAME moveit_kinematics_base)

add_library(${MOVEIT_LIB_NAME} src/kinematics_base.cpp)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
vmayoral marked this conversation as resolved.
Show resolved Hide resolved

# Avoid warnings about deprecated members of KinematicsBase when building KinematicsBase itself.
# TODO: remove when deperecated variables (tip_frame_, search_discretization_) are finally removed.
vmayoral marked this conversation as resolved.
Show resolved Hide resolved
target_compile_options(${MOVEIT_LIB_NAME} PRIVATE -Wno-deprecated-declarations)
# target_compile_options(${MOVEIT_LIB_NAME} PRIVATE -Wno-deprecated-declarations)

# This line is needed to ensure that messages are done being built before this is built
add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})

target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
# add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
vmayoral marked this conversation as resolved.
Show resolved Hide resolved
ament_target_dependencies(${MOVEIT_LIB_NAME} urdf urdfdom_headers)

install(TARGETS ${MOVEIT_LIB_NAME}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib)

install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
install(DIRECTORY include/
DESTINATION include)
114 changes: 69 additions & 45 deletions moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,12 +37,11 @@
#ifndef MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_
#define MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_

#include <geometry_msgs/PoseStamped.h>
#include <moveit_msgs/MoveItErrorCodes.h>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <moveit_msgs/msg/move_it_error_codes.hpp>
vmayoral marked this conversation as resolved.
Show resolved Hide resolved
#include <moveit/macros/class_forward.h>
#include <moveit/macros/deprecation.h>
#include <ros/node_handle.h>

#include "rclcpp/rclcpp.hpp"
#include <boost/function.hpp>
#include <string>

Expand All @@ -53,12 +52,15 @@ namespace core
MOVEIT_CLASS_FORWARD(JointModelGroup)
MOVEIT_CLASS_FORWARD(RobotState)
MOVEIT_CLASS_FORWARD(RobotModel)
}
}
} // namespace core
} // namespace moveit

/** @brief API for forward and inverse kinematics */
namespace kinematics
{
// Logger
rclcpp::Logger logger_kinematics_base = rclcpp::get_logger("kinematics_base");
vmayoral marked this conversation as resolved.
Show resolved Hide resolved

/*
* @enum DiscretizationMethods
*
Expand Down Expand Up @@ -137,7 +139,7 @@ struct KinematicsResult
of solutions explored. */
};

MOVEIT_CLASS_FORWARD(KinematicsBase);
MOVEIT_CLASS_FORWARD(KinematicsBase)
mlautman marked this conversation as resolved.
Show resolved Hide resolved

/**
* @class KinematicsBase
Expand All @@ -150,8 +152,9 @@ class KinematicsBase
static const double DEFAULT_TIMEOUT; /* = 1.0 */

/** @brief Signature for a callback to validate an IK solution. Typically used for collision checking. */
typedef boost::function<void(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_solution,
moveit_msgs::MoveItErrorCodes& error_code)>
/** @brief The signature for a callback that can compute IK */
vmayoral marked this conversation as resolved.
Show resolved Hide resolved
typedef boost::function<void(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_solution,
moveit_msgs::msg::MoveItErrorCodes& error_code)>
IKCallbackFn;

/**
Expand All @@ -167,8 +170,8 @@ class KinematicsBase
* @return True if a valid solution was found, false otherwise
*/
virtual bool
getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state,
std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;

/**
Expand All @@ -186,9 +189,9 @@ class KinematicsBase
* other will result in failure.
* @return True if a valid set of solutions was found, false otherwise.
*/
virtual bool getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
std::vector<std::vector<double> >& solutions, KinematicsResult& result,
const kinematics::KinematicsQueryOptions& options) const;
virtual bool getPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses,
const std::vector<double>& ik_seed_state, std::vector<std::vector<double> >& solutions,
KinematicsResult& result, const kinematics::KinematicsQueryOptions& options) const;

/**
* @brief Given a desired pose of the end-effector, search for the joint angles required to reach it.
Expand All @@ -203,8 +206,8 @@ class KinematicsBase
* @return True if a valid solution was found, false otherwise
*/
virtual bool
searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;

/**
Expand All @@ -222,9 +225,9 @@ class KinematicsBase
* @return True if a valid solution was found, false otherwise
*/
virtual bool
searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
const std::vector<double>& consistency_limits, std::vector<double>& solution,
moveit_msgs::MoveItErrorCodes& error_code,
moveit_msgs::msg::MoveItErrorCodes& error_code,
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;

/**
Expand All @@ -241,9 +244,9 @@ class KinematicsBase
* @return True if a valid solution was found, false otherwise
*/
virtual bool
searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
std::vector<double>& solution, const IKCallbackFn& solution_callback,
moveit_msgs::MoveItErrorCodes& error_code,
moveit_msgs::msg::MoveItErrorCodes& error_code,
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;

/**
Expand All @@ -262,9 +265,9 @@ class KinematicsBase
* @return True if a valid solution was found, false otherwise
*/
virtual bool
searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
const std::vector<double>& consistency_limits, std::vector<double>& solution,
const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code,
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;

/**
Expand All @@ -289,9 +292,9 @@ class KinematicsBase
* @return True if a valid solution was found, false otherwise
*/
virtual bool
searchPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
searchPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
double timeout, const std::vector<double>& consistency_limits, std::vector<double>& solution,
const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code,
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
const moveit::core::RobotState* context_state = NULL) const
{
Expand All @@ -311,7 +314,7 @@ class KinematicsBase
}

// Otherwise throw error because this function should have been implemented
ROS_ERROR_NAMED("kinematics_base", "This kinematic solver does not support searchPositionIK with multiple poses");
RCLCPP_ERROR(kinematics::logger_kinematics_base, "This kinematic solver does not support searchPositionIK with multiple poses");
return false;
}

Expand All @@ -323,7 +326,7 @@ class KinematicsBase
* @return True if a valid solution was found, false otherwise
*/
virtual bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
std::vector<geometry_msgs::Pose>& poses) const = 0;
std::vector<geometry_msgs::msg::Pose>& poses) const = 0;

/**
* @brief Set the parameters for the solver, for use with kinematic chain IK solvers
Expand Down Expand Up @@ -435,7 +438,7 @@ class KinematicsBase
virtual const std::string& getTipFrame() const
{
if (tip_frames_.size() > 1)
ROS_ERROR_NAMED("kinematics_base", "This kinematic solver has more than one tip frame, "
RCLCPP_ERROR(kinematics::logger_kinematics_base, "This kinematic solver has more than one tip frame, "
"do not call getTipFrame()");

return tip_frames_[0];
Expand Down Expand Up @@ -612,34 +615,55 @@ class KinematicsBase
template <typename T>
inline bool lookupParam(const std::string& param, T& val, const T& default_val) const
{
ros::NodeHandle pnh("~");
if (pnh.hasParam(group_name_ + "/" + param))
{
val = pnh.param(group_name_ + "/" + param, default_val);
return true;
}
auto node = rclcpp::Node::make_shared("lookup_param");

if (pnh.hasParam(param))
auto parameters_lookup = std::make_shared<rclcpp::SyncParametersClient>(node);
vmayoral marked this conversation as resolved.
Show resolved Hide resolved

auto groupname_param = parameters_lookup->get_parameters({ group_name_ + "/" + param });
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

auto is nice where the type is obvious but in cases like this it's more confusing then helpful.
Especially since the following for loops are using auto as well this type should be made explicit.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also, the SyncParametersClient class contains the functions has_param() and get_parameter() which can be used to replace hasParam() and param() directly. This way we don't have to loop and compare the results as well.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Partially complied with your request @henningkayser at b2d436f.

Please make a suggestion if you require further changes.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

get_parameters() is meant to be used for multiple parameters but here it's always called with only a single entry. In this case it's much more straightforward to query every parameter like in the sample code below:

    bool param_success = node.get_parameter<T>(group_name + "/" + param, val);

No need to create a parameter vector or loop over the results and also we can use the node directly and don't need to cast it to rclcpp::SyncParametersClient.

Since lookupParam() does in fact query multiple parameters in different locations we can absolutely use Node::get_parameters() but in that case including all parameter names:

val = default_val;
std::vector<std::string> param_names = {
  group_name_ + "/" + param, param,
  "robot_description_kinematics/" + group_name_ + "/" + param,
  robot_description_kinematics/" + param
};
std::vector<rclcpp::parameter::ParameterVariant> param_results =
  node.getParameters(param_names);
bool param_success = !param_results.empty();
if (param_success)
    val = param_results[0].get_value<T>();
return param_success;

I actually prefer the second implementation.
Please also note that your current implementation calls value_to_string() on all parameters but lookupParam() is a template function that supports all parameter types. The get_value() function takes care of using the correct type.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I've already made some changes (on planning_scene_monitor) related to this:

https://github.com/AcutronicRobotics/moveit2/blob/5a8ad535e33548434b02e5e9e735ab614465f5a5/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp#L1472-L1516

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@anasarrak this looks much better. However, each if/else block can be implemented in a single line using node->get_parameter_or(param, val, default_val).


for (auto& parameter : groupname_param)
{
val = pnh.param(param, default_val);
return true;
if (!parameter.get_name().compare(group_name_ + "/" + param))
{
val = parameter.value_to_string();
return true;
}
}

ros::NodeHandle nh;
if (nh.hasParam("robot_description_kinematics/" + group_name_ + "/" + param))
auto only_param = parameters_lookup->get_parameters({ param });
vmayoral marked this conversation as resolved.
Show resolved Hide resolved

for (auto& parameter : only_param)
{
val = nh.param("robot_description_kinematics/" + group_name_ + "/" + param, default_val);
return true;
if (!parameter.get_name().compare(param))
{
val = parameter.value_to_string();
return true;
}
}

if (nh.hasParam("robot_description_kinematics/" + param))
auto robot_description_groupname_kinematics_param =
parameters_lookup->get_parameters({ "robot_description_kinematics/" + group_name_ + "/" + param });

for (auto& parameter : robot_description_groupname_kinematics_param)
{
val = nh.param("robot_description_kinematics/" + param, default_val);
return true;
if (!parameter.get_name().compare("robot_description_kinematics/" + group_name_ + "/" + param))
{
val = parameter.value_to_string();
return true;
}
}

val = default_val;
auto robot_description_kinematics_param =
parameters_lookup->get_parameters({ "robot_description_kinematics/" + param });

for (auto& parameter : robot_description_kinematics_param)
{
if (!parameter.get_name().compare("robot_description_kinematics/" + param))
{
val = parameter.value_to_string();
return true;
}
}
return false;
}

Expand All @@ -658,6 +682,6 @@ class KinematicsBase
private:
std::string removeSlash(const std::string& str) const;
};
};
} // namespace kinematics

#endif
23 changes: 10 additions & 13 deletions moveit_core/kinematics_base/src/kinematics_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,14 +37,12 @@
#include <moveit/kinematics_base/kinematics_base.h>
#include <moveit/robot_model/joint_model_group.h>

static const std::string LOGNAME = "kinematics_base";

namespace kinematics
{
const double KinematicsBase::DEFAULT_SEARCH_DISCRETIZATION = 0.1;
const double KinematicsBase::DEFAULT_TIMEOUT = 1.0;

static void noDeleter(const moveit::core::RobotModel*)
static void noDeleter(const moveit::core::RobotModel* /*unused*/)
{
}

Expand Down Expand Up @@ -112,16 +110,17 @@ bool KinematicsBase::initialize(const std::string& robot_description, const std:
return initialize(robot_description, group_name, base_frame, tip_frames[0], search_discretization);
}

ROS_ERROR_NAMED(LOGNAME, "This solver does not support multiple tip frames");
RCLCPP_ERROR(kinematics::logger_kinematics_base, "This solver does not support multiple tip frames");
return false;
}

bool KinematicsBase::initialize(const moveit::core::RobotModel& robot_model, const std::string& group_name,
const std::string& base_frame, const std::vector<std::string>& tip_frames,
double search_discretization)
{
ROS_WARN_NAMED(LOGNAME, "IK plugin for group '%s' relies on deprecated API. "
"Please implement initialize(RobotModel, ...).",
RCLCPP_WARN(kinematics::logger_kinematics_base,
"IK plugin for group '%s' relies on deprecated API. "
"Please implement initialize(RobotModel, ...).",
group_name.c_str());
return false;
}
Expand Down Expand Up @@ -186,11 +185,9 @@ KinematicsBase::KinematicsBase()
supported_methods_.push_back(DiscretizationMethods::NO_DISCRETIZATION);
}

KinematicsBase::~KinematicsBase()
{
}
KinematicsBase::~KinematicsBase() = default;

bool KinematicsBase::getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
bool KinematicsBase::getPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses,
const std::vector<double>& ik_seed_state,
std::vector<std::vector<double> >& solutions, KinematicsResult& result,
const KinematicsQueryOptions& options) const
Expand All @@ -207,19 +204,19 @@ bool KinematicsBase::getPositionIK(const std::vector<geometry_msgs::Pose>& ik_po

if (ik_poses.size() != 1)
{
ROS_ERROR_NAMED(LOGNAME, "This kinematic solver does not support getPositionIK for multiple tips");
RCLCPP_ERROR(kinematics::logger_kinematics_base, "This kinematic solver does not support getPositionIK for multiple tips");
result.kinematic_error = KinematicErrors::MULTIPLE_TIPS_NOT_SUPPORTED;
return false;
}

if (ik_poses.empty())
{
ROS_ERROR_NAMED(LOGNAME, "Input ik_poses array is empty");
RCLCPP_ERROR(kinematics::logger_kinematics_base, "Input ik_poses array is empty");
result.kinematic_error = KinematicErrors::EMPTY_TIP_POSES;
return false;
}

moveit_msgs::MoveItErrorCodes error_code;
moveit_msgs::msg::MoveItErrorCodes error_code;
if (getPositionIK(ik_poses[0], ik_seed_state, solution, error_code, options))
{
solutions.resize(1);
Expand Down