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 to noetic #8

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Port to noetic #8

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1a3ed61
CMakeLists: fix catkin lint error
machinekoder Oct 21, 2020
d0324c6
port to noetic
machinekoder Oct 21, 2020
394bf6c
fix linter warnings
machinekoder Oct 21, 2020
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
14 changes: 6 additions & 8 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,12 +14,12 @@ add_definitions(-W -Wall -Wextra
find_package(catkin REQUIRED COMPONENTS
descartes_core
descartes_moveit
descartes_trajectory
descartes_planner
descartes_trajectory
moveit_core
moveit_ros_move_group
moveit_ros_planning
moveit_visual_tools
moveit_ros_move_group
roscpp
roslint
rospy
Expand All @@ -34,12 +34,12 @@ find_package(Eigen3 REQUIRED)
###################################
catkin_package(
CATKIN_DEPENDS
roscpp
rospy
moveit_core
moveit_ros_planning
moveit_ros_move_group
moveit_ros_planning
moveit_visual_tools
roscpp
rospy
INCLUDE_DIRS
include
LIBRARIES
Expand Down Expand Up @@ -123,7 +123,7 @@ install(PROGRAMS
)

install(FILES
descartes_capability_plugin_description.xml
${PROJECT_NAME}_plugin_description.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

#############
Expand All @@ -133,8 +133,6 @@ install(FILES
if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
find_package(pluginlib REQUIRED)
find_package(moveit_resources REQUIRED)
include_directories(${moveit_resources_INCLUDE_DIRS})

add_rostest_gtest(descartes_path_service_capability_test test/descartes_path_service_capability_test.test test/descartes_path_service_capability_test.cpp)
target_link_libraries(descartes_path_service_capability_test
Expand Down
6 changes: 3 additions & 3 deletions include/descartes_capability/descartes_path_service_capability.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -80,8 +80,8 @@ class MoveGroupDescartesPathService : public move_group::MoveGroupCapability
double computeMaxJointDelta(const std::vector<double>& joints1, const std::vector<double>& joints2);

/** \brief Interpolates between start and end poses and appends them to deense_waypoints **/
void createDensePath(const Eigen::Isometry3d& start, const Eigen::Isometry3d& end, double max_step,
EigenSTL::vector_Isometry3d& dense_waypoints);
static void createDensePath(const Eigen::Isometry3d& start, const Eigen::Isometry3d& end, double max_step,
EigenSTL::vector_Isometry3d& dense_waypoints);

/** \brief Transforms each point in a vector of affine**/
void createDescartesTrajectory(const EigenSTL::vector_Isometry3d& dense_waypoints,
Expand Down Expand Up @@ -128,6 +128,6 @@ class MoveGroupDescartesPathService : public move_group::MoveGroupCapability
// For Rviz visualizations
moveit_visual_tools::MoveItVisualToolsPtr visual_tools_;
};
}
} // namespace descartes_capability

#endif // MOVEIT_MOVE_GROUP_DESCARTES_PATH_SERVICE_CAPABILITY_
37 changes: 20 additions & 17 deletions src/descartes_path_service_capability.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,10 +54,15 @@ namespace descartes_capability
MoveGroupDescartesPathService::MoveGroupDescartesPathService()
: MoveGroupCapability("DescartesPathService")
, nh_("~")
, positional_tolerance_(0.0)
, positional_tolerance_increment_(0.0)
, roll_orientation_tolerance_(0.0)
, pitch_orientation_tolerance_(0.0)
, yaw_orientation_tolerance_(0.0)
, orientation_tolerance_increment_(0.0)
, verbose_debug_(false)
, visual_debug_(false)
, display_computed_paths_(true)
, current_group_name_("")
, current_world_frame_("")
, current_tcp_frame_("")
{
}

Expand Down Expand Up @@ -122,9 +127,8 @@ void MoveGroupDescartesPathService::createDescartesTrajectory(
const EigenSTL::vector_Isometry3d& dense_waypoints,
std::vector<descartes_core::TrajectoryPtPtr>& input_descartes_trajectory)
{
for (std::size_t i = 0; i < dense_waypoints.size(); ++i)
for (auto eigen_pose : dense_waypoints)
{
const Eigen::Isometry3d eigen_pose = dense_waypoints[i];
const Eigen::Quaterniond rotation(eigen_pose.rotation());

if (verbose_debug_)
Expand Down Expand Up @@ -258,10 +262,9 @@ bool MoveGroupDescartesPathService::initializeDescartesModel(const std::string&
{
// Setup Descartes model
descartes_model_.reset(new descartes_moveit::MoveitStateAdapter);
descartes_moveit::MoveitStateAdapter* moveit_state_adapter =
dynamic_cast<descartes_moveit::MoveitStateAdapter*>(descartes_model_.get());
bool model_init =
moveit_state_adapter->initialize(context_->planning_scene_monitor_, group_name, world_frame, tcp_frame);
auto* moveit_state_adapter = dynamic_cast<descartes_moveit::MoveitStateAdapter*>(descartes_model_.get());
bool model_init = moveit_state_adapter->initialize(context_->planning_scene_monitor_->getRobotModel(), group_name,
world_frame, tcp_frame);

if (!model_init)
{
Expand Down Expand Up @@ -332,7 +335,7 @@ bool MoveGroupDescartesPathService::computeService(moveit_msgs::GetCartesianPath
Eigen::Isometry3d current_pose;
descartes_model_->getFK(current_joints, current_pose);

if (req.waypoints.size() < 1)
if (req.waypoints.empty())
{
ROS_ERROR_NAMED(name_, "Must provide at least 1 input trajectory point %zu provided", req.waypoints.size());
res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
Expand Down Expand Up @@ -428,7 +431,7 @@ bool MoveGroupDescartesPathService::computeService(moveit_msgs::GetCartesianPath
if (!descartes_planner.planPath(descartes_trajectory))
{
valid_path = false;
ROS_ERROR_STREAM_NAMED(name_, "Could not solve for a valid path.");
ROS_INFO_STREAM_NAMED(name_, "Could not solve for a valid path.");
}
else
{
Expand All @@ -439,15 +442,15 @@ bool MoveGroupDescartesPathService::computeService(moveit_msgs::GetCartesianPath
if (!descartes_planner.getPath(descartes_result))
{
valid_path = false;
ROS_ERROR_STREAM_NAMED(name_, "Could not retrieve path.");
ROS_INFO_STREAM_NAMED(name_, "Could not retrieve path.");
}

if (valid_path && verbose_debug_)
ROS_INFO_STREAM_NAMED(name_, "Full path length = " << descartes_result.size());

if (!valid_path)
{
ROS_ERROR_STREAM_NAMED(name_, "Unable to generate a plan using Descartes.");
ROS_INFO_STREAM_NAMED(name_, "Unable to generate a plan using Descartes.");
res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
res.fraction = 0.0;
return true;
Expand Down Expand Up @@ -499,9 +502,9 @@ void MoveGroupDescartesPathService::printJoints(const std::vector<double>& joint
{
std::stringstream o;

for (std::size_t i = 0; i < joints.size(); ++i)
for (double joint : joints)
{
o << "\t" << std::fixed << std::setw(6) << std::setprecision(3) << joints[i];
o << "\t" << std::fixed << std::setw(6) << std::setprecision(3) << joint;
}
ROS_DEBUG_STREAM_NAMED(name_, o.str());
}
Expand All @@ -510,9 +513,9 @@ void MoveGroupDescartesPathService::printJointsNamed(const std::string& name, co
{
std::stringstream o;
o << name;
for (std::size_t i = 0; i < joints1.size(); ++i)
for (double i : joints1)
{
o << "\t" << std::fixed << std::setw(6) << std::setprecision(3) << joints1[i];
o << "\t" << std::fixed << std::setw(6) << std::setprecision(3) << i;
}
ROS_DEBUG_STREAM_NAMED(name_, o.str());
}
Expand Down