Skip to content

Commit

Permalink
Merge pull request moveit#86 from AcutronicRobotics/port_srv_kinemati…
Browse files Browse the repository at this point in the history 
…cs_plugin_2

Port srv kinematics plugin
  • Loading branch information
@ahcorde
ahcorde authored May 23, 2019
2 parents ae294f9 + abf3d6c commit a21476d
Show file tree
Hide file tree
Showing 5 changed files with 95 additions and 93 deletions.
4 changes: 2 additions & 2 deletions moveit_kinematics/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ find_package(moveit_msgs REQUIRED)
set(THIS_PACKAGE_INCLUDE_DIRS
kdl_kinematics_plugin/include
lma_kinematics_plugin/include
# srv_kinematics_plugin/include
srv_kinematics_plugin/include
# cached_ik_kinematics_plugin/include
)

Expand All @@ -40,7 +40,7 @@ include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS}

add_subdirectory(kdl_kinematics_plugin)
add_subdirectory(lma_kinematics_plugin)
# add_subdirectory(srv_kinematics_plugin)
add_subdirectory(srv_kinematics_plugin)
# add_subdirectory(ikfast_kinematics_plugin)
# add_subdirectory(cached_ik_kinematics_plugin)
add_subdirectory(test)
Expand Down
2 changes: 1 addition & 1 deletion moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -144,7 +144,7 @@ bool LMAKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model

bool LMAKinematicsPlugin::timedOut(const std::chrono::system_clock::time_point& start_time, double duration) const
{
return (std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - start_time).count() *1e-9 >= duration);
return (std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - start_time).count() /1000.0 >= duration);
}

bool LMAKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state,
Expand Down
6 changes: 3 additions & 3 deletions moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@ set(MOVEIT_LIB_NAME moveit_srv_kinematics_plugin)
add_library(${MOVEIT_LIB_NAME} SHARED src/srv_kinematics_plugin.cpp)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")

target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES})
target_link_libraries(${MOVEIT_LIB_NAME} ${rclcpp_LIBRARIES})

install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION lib)
install(DIRECTORY include/ DESTINATION include)
37 changes: 20 additions & 17 deletions ...matics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,18 +42,18 @@
#ifndef MOVEIT_ROS_PLANNING_SRV_KINEMATICS_PLUGIN_
#define MOVEIT_ROS_PLANNING_SRV_KINEMATICS_PLUGIN_

// ROS
#include <ros/ros.h>
// ROS2
#include "rclcpp/rclcpp.hpp"

// System
#include <memory>

// ROS msgs
#include <geometry_msgs/PoseStamped.h>
#include <moveit_msgs/GetPositionFK.h>
#include <moveit_msgs/GetPositionIK.h>
#include <moveit_msgs/KinematicSolverInfo.h>
#include <moveit_msgs/MoveItErrorCodes.h>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <moveit_msgs/srv/get_position_fk.hpp>
#include <moveit_msgs/srv/get_position_ik.hpp>
#include <moveit_msgs/msg/kinematic_solver_info.hpp>
#include <moveit_msgs/msg/move_it_error_codes.hpp>

// MoveIt!
#include <moveit/kinematics_base/kinematics_base.h>
Expand All @@ -74,34 +74,34 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase
SrvKinematicsPlugin();

bool getPositionIK(
const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, std::vector<double>& solution,
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 override;

bool searchPositionIK(
const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
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 override;

bool searchPositionIK(
const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
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 override;

bool searchPositionIK(
const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
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 override;

bool searchPositionIK(
const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
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 override;

bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
std::vector<geometry_msgs::Pose>& poses) const override;
std::vector<geometry_msgs::msg::Pose>& poses) const override;

bool initialize(const moveit::core::RobotModel& robot_model, const std::string& group_name,
const std::string& base_name, const std::vector<std::string>& tip_frames,
Expand All @@ -124,21 +124,21 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase

protected:
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 std::vector<double>& consistency_limits,
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const;

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()) const;

bool setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices) override;

private:
bool timedOut(const ros::WallTime& start_time, double duration) const;
bool timedOut(const std::chrono::system_clock::time_point& start_time, double duration) const;

int getJointIndex(const std::string& name) const;

Expand All @@ -156,7 +156,10 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase

int num_possible_redundant_joints_;

std::shared_ptr<ros::ServiceClient> ik_service_client_;
rclcpp::Client<moveit_msgs::srv::GetPositionIK>::SharedPtr ik_service_client_;

rclcpp::Node::SharedPtr node_;

};
}

Expand Down
Loading

0 comments on commit a21476d

Please sign in to comment.