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.

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

Port kinematics_base submodule moveit_core #8

Merged
merged 21 commits into from
Apr 22, 2019
Merged
Show file tree
Hide file tree
Changes from 4 commits
Commits
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
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)
Original file line number Diff line number Diff line change
Expand Up @@ -37,14 +37,16 @@
#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>
#include <moveit/logging/logging.h>

namespace moveit
{
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 @@ -149,9 +151,9 @@ class KinematicsBase
static const double DEFAULT_SEARCH_DISCRETIZATION; /* = 0.1 */
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 +169,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,7 +188,7 @@ 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,
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;

Expand All @@ -203,8 +205,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 +224,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 +243,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 +264,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 +291,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 Down Expand Up @@ -323,7 +325,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 @@ -612,33 +614,74 @@ 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;
// ros::NodeHandle pnh("~");
vmayoral marked this conversation as resolved.
Show resolved Hide resolved
vmayoral marked this conversation as resolved.
Show resolved Hide resolved

auto node = rclcpp::Node::make_shared("lookupparam");
vmayoral marked this conversation as resolved.
Show resolved Hide resolved

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});
vmayoral marked this conversation as resolved.
Show resolved Hide resolved

for (auto & parameter : groupname_param) {
if (!parameter.get_name().compare(group_name_ + "/" + param)) {
vmayoral marked this conversation as resolved.
Show resolved Hide resolved
val = parameter.value_to_string();
return true;
}
}

if (pnh.hasParam(param))
{
val = pnh.param(param, default_val);
return true;
auto only_param = parameters_lookup->get_parameters({param});

for (auto & parameter : only_param) {
if (!parameter.get_name().compare(param)) {
val = parameter.value_to_string();
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;
auto robot_description_groupname_kinematics_param = parameters_lookup->get_parameters({"robot_description_kinematics/" + group_name_ + "/" + param});

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

if (nh.hasParam("robot_description_kinematics/" + param))
{
val = nh.param("robot_description_kinematics/" + param, default_val);
return true;
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;
}
}

val = default_val;
// if (pnh.hasParam(group_name_ + "/" + param))
vmayoral marked this conversation as resolved.
Show resolved Hide resolved
// {
// val = pnh.param(group_name_ + "/" + param, default_val);
// return true;
// }
//
// if (pnh.hasParam(param))
// {
// val = pnh.param(param, default_val);
// 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;
vmayoral marked this conversation as resolved.
Show resolved Hide resolved

return false;
}
Expand All @@ -658,6 +701,6 @@ class KinematicsBase
private:
std::string removeSlash(const std::string& str) const;
};
};
}
vmayoral marked this conversation as resolved.
Show resolved Hide resolved

#endif
12 changes: 6 additions & 6 deletions moveit_core/kinematics_base/src/kinematics_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -112,15 +112,15 @@ 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");
ROS_ERROR_NAMED(LOGNAME.c_str(), "This solver does not support multiple tip frames");
vmayoral marked this conversation as resolved.
Show resolved Hide resolved
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. "
ROS_WARN_NAMED(LOGNAME.c_str(), "IK plugin for group '%s' relies on deprecated API. "
"Please implement initialize(RobotModel, ...).",
group_name.c_str());
return false;
Expand Down Expand Up @@ -190,7 +190,7 @@ KinematicsBase::~KinematicsBase()
{
}

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 +207,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");
ROS_ERROR_NAMED(LOGNAME.c_str(), "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");
ROS_ERROR_NAMED(LOGNAME.c_str(), "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