Skip to content

Commit

Permalink
Port kinematics_base submodule moveit_core (#8)
Browse files Browse the repository at this point in the history 
* Port kinematics_base submodule moveit_core

* kinematics_base, use spaces instead of tabs

* Remove unnecessary cmake lines

Addressed  in the top level CMakeLists.txt

* kinematics_base, remove redundancy in CMakeLists.txt

* Undo documentation removal

* Remove dead code

* remove dead code in kinematics_base upon request

* kinematics_base, apply clang-format

* Use underscores for node names to increase readability

Reviewed by @mlautman

* Adapt logging

Follows from #21

* Reverting change in CMakeLists.txt

Keep PROPERTIES VERSION

* Meet the LOGGER indications

Follows from #8 (comment)
taking into account that recommendations do currently fail
to compile given the structure of RCLCPP macros

* kinematics_base, clang format

* Remove leftover in kinematics_base

* Fix kinematics_base namespaces

* Do not use auto

Follows from review at #8 (comment)

* Comply partially with review request

#8 (review)

* Simplify lookupParam() in KinematicsBase

* Fix result param array access
  • Loading branch information
@henningkayser
Víctor Mayoral Vilches authored and henningkayser committed Apr 22, 2019
1 parent 7bbbb4b commit f5483c5
Show file tree
Hide file tree
Showing 3 changed files with 46 additions and 60 deletions.
15 changes: 8 additions & 7 deletions moveit_core/kinematics_base/CMakeLists.txt
View file
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}")

# Avoid warnings about deprecated members of KinematicsBase when building KinematicsBase itself.
# TODO: remove when deperecated variables (tip_frame_, search_discretization_) are finally removed.
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})
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)
76 changes: 31 additions & 45 deletions moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h
View file
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 <geometry_msgs/msg/pose_stamped.hpp>
#include <moveit_msgs/msg/move_it_error_codes.hpp>
#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 = rclcpp::get_logger("kinematics_base");

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

MOVEIT_CLASS_FORWARD(KinematicsBase);
MOVEIT_CLASS_FORWARD(KinematicsBase)

/**
* @class KinematicsBase
Expand All @@ -150,7 +152,8 @@ 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,
/** @brief The signature for a callback that can compute IK */
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,7 +170,7 @@ 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,
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,7 +206,7 @@ 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, moveit_msgs::msg::MoveItErrorCodes& error_code,
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;

Expand All @@ -222,7 +225,7 @@ 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::msg::MoveItErrorCodes& error_code,
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
Expand All @@ -241,7 +244,7 @@ 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::msg::MoveItErrorCodes& error_code,
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
Expand All @@ -262,7 +265,7 @@ 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::msg::MoveItErrorCodes& error_code,
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
Expand All @@ -289,7 +292,7 @@ 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::msg::MoveItErrorCodes& error_code,
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
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(LOGGER, "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,8 +438,8 @@ 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, "
"do not call getTipFrame()");
RCLCPP_ERROR(LOGGER, "This kinematic solver has more than one tip frame, "
"do not call getTipFrame()");

return tip_frames_[0];
}
Expand Down Expand Up @@ -612,34 +615,17 @@ 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;
}

if (pnh.hasParam(param))
// TODO(henningkayser): reuse node and fix private namespace lookup from kinematics.yaml
auto node = rclcpp::Node::make_shared("kinematics_base.lookup_param");
std::vector<rclcpp::Parameter> param_results = node->get_parameters(
{ group_name_ + "/" + param, param, "robot_description_kinematics/" + group_name_ + "/" + param,
"robot_description_kinematics/" + param });
if (!param_results.empty())
{
val = pnh.param(param, default_val);
val = param_results[0].get_value<T>();
return true;
}

ros::NodeHandle nh;
if (nh.hasParam("robot_description_kinematics/" + group_name_ + "/" + param))
{
val = nh.param("robot_description_kinematics/" + group_name_ + "/" + param, default_val);
return true;
}

if (nh.hasParam("robot_description_kinematics/" + param))
{
val = nh.param("robot_description_kinematics/" + param, default_val);
return true;
}

val = default_val;

return false;
}

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

#endif
15 changes: 7 additions & 8 deletions moveit_core/kinematics_base/src/kinematics_base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,8 +37,6 @@
#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;
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(LOGGER, "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(LOGGER,
"IK plugin for group '%s' relies on deprecated API. "
"Please implement initialize(RobotModel, ...).",
group_name.c_str());
return false;
}
Expand Down Expand Up @@ -188,7 +187,7 @@ 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 @@ -205,14 +204,14 @@ 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(LOGGER, "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(LOGGER, "Input ik_poses array is empty");
result.kinematic_error = KinematicErrors::EMPTY_TIP_POSES;
return false;
}
Expand Down

0 comments on commit f5483c5

Please sign in to comment.