Skip to content

Commit

Permalink
separate examples
Browse files Browse the repository at this point in the history
  • Loading branch information
Svastits committed Oct 25, 2023
1 parent 42ea574 commit 16cf499
Show file tree
Hide file tree
Showing 5 changed files with 181 additions and 31 deletions.
20 changes: 19 additions & 1 deletion kuka_driver_examples/eci_demo/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,24 @@ ament_target_dependencies(moveit_basic_planners_example
kuka_driver_interfaces
)

add_executable(moveit_collision_avoidance_example src/MoveitCollisionAvoidanceExample.cpp)
ament_target_dependencies(moveit_collision_avoidance_example
moveit_ros_planning_interface
rclcpp
rviz_visual_tools
moveit_visual_tools
kuka_driver_interfaces
)

add_executable(moveit_constrained_planning_example src/MoveitConstrainedPlanningExample.cpp)
ament_target_dependencies(moveit_constrained_planning_example
moveit_ros_planning_interface
rclcpp
rviz_visual_tools
moveit_visual_tools
kuka_driver_interfaces
)

add_executable(moveit_depalletizing_example src/MoveitDepalletizingExample.cpp)
ament_target_dependencies(moveit_depalletizing_example
moveit_ros_planning_interface
Expand All @@ -40,7 +58,7 @@ ament_target_dependencies(moveit_depalletizing_example
kuka_driver_interfaces
)

install(TARGETS moveit_basic_planners_example moveit_depalletizing_example
install(TARGETS moveit_basic_planners_example moveit_collision_avoidance_example moveit_constrained_planning_example moveit_depalletizing_example
EXPORT export_${PROJECT_NAME}
DESTINATION lib/${PROJECT_NAME}
)
Expand Down
15 changes: 9 additions & 6 deletions kuka_driver_examples/eci_demo/include/moveit_example.h
Original file line number Diff line number Diff line change
Expand Up @@ -59,13 +59,15 @@ class MoveitExample : public rclcpp::Node
planning_scene_diff_publisher_ = this->create_publisher<moveit_msgs::msg::PlanningScene>(
"planning_scene", 10);

move_group_interface_->setMaxVelocityScalingFactor(1.0);
move_group_interface_->setMaxAccelerationScalingFactor(1.0);
move_group_interface_->setMaxVelocityScalingFactor(0.1);
move_group_interface_->setMaxAccelerationScalingFactor(0.1);

// Create collision box subscriber
collision_box_subscriber_ =
this->create_subscription<kuka_driver_interfaces::msg::CollisionBox>("collision_box", 10, std::bind(&MoveitExample::addCollisionBox, this, std::placeholders::_1));

collision_box_subscriber_ =
this->create_subscription<kuka_driver_interfaces::msg::CollisionBox>(
"collision_box", 10, std::bind(
&MoveitExample::addCollisionBox, this,
std::placeholders::_1));
}

moveit_msgs::msg::RobotTrajectory::SharedPtr drawCircle()
Expand Down Expand Up @@ -349,7 +351,8 @@ class MoveitExample : public rclcpp::Node
protected:
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_interface_;
rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_diff_publisher_;
rclcpp::Subscription<kuka_driver_interfaces::msg::CollisionBox>::SharedPtr collision_box_subscriber_;
rclcpp::Subscription<kuka_driver_interfaces::msg::CollisionBox>::SharedPtr
collision_box_subscriber_;
std::shared_ptr<moveit_visual_tools::MoveItVisualTools> moveit_visual_tools_;
const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_basic_plan");
const std::string PLANNING_GROUP = "lbr_iisy_arm";
Expand Down
25 changes: 1 addition & 24 deletions kuka_driver_examples/eci_demo/src/MoveitBasicPlannersExample.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ int main(int argc, char * argv[])
box_msg.size.x = 0.1;
box_msg.size.y = 0.4;
box_msg.size.z = 0.1;

example_node->addCollisionBox(std::make_shared<kuka_driver_interfaces::msg::CollisionBox>(box_msg));
example_node->addBreakPoint();

Expand All @@ -106,29 +106,6 @@ int main(int argc, char * argv[])
}
example_node->addBreakPoint();

// Plan with collision avoidance
planned_trajectory = example_node->planToPoint(standing_pose, "ompl", "RRTConnectkConfigDefault");
if (planned_trajectory != nullptr) {
example_node->drawTrajectory(*planned_trajectory);
example_node->addBreakPoint();
example_node->moveGroupInterface()->execute(*planned_trajectory);
}
example_node->addBreakPoint();

geometry_msgs::msg::Quaternion q;
q.x = 0;
q.y = 0;
q.z = 0;
q.w = 1;
example_node->setOrientationConstraint(q);
// Plan with collision avoidance
planned_trajectory = example_node->planToPoint(cart_goal, "ompl", "RRTConnectkConfigDefault");
if (planned_trajectory != nullptr) {
example_node->drawTrajectory(*planned_trajectory);
example_node->addBreakPoint();
example_node->moveGroupInterface()->execute(*planned_trajectory);
}

// Shutdown ROS
rclcpp::shutdown();
return 0;
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,73 @@
// Copyright 2022 Áron Svastits
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <math.h>

#include <kuka_driver_interfaces/msg/collision_box.hpp>
#include <memory>

#include "moveit_example.h"

int main(int argc, char * argv[])
{
// Setup
// Initialize ROS and create the Node
rclcpp::init(argc, argv);
auto const example_node = std::make_shared<MoveitExample>();
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(example_node);
std::thread(
[&executor]()
{executor.spin();})
.detach();

example_node->initialize();
example_node->addBreakPoint();

// Add robot platform
example_node->addRobotPlatform();

// Add collision object
kuka_driver_interfaces::msg::CollisionBox box_msg;
box_msg.position.x = 0.25;
box_msg.position.y = -0.075;
box_msg.position.z = 0.675;

box_msg.size.x = 0.1;
box_msg.size.y = 0.4;
box_msg.size.z = 0.1;

example_node->addCollisionBox(std::make_shared<kuka_driver_interfaces::msg::CollisionBox>(box_msg));
example_node->addBreakPoint();

auto standing_pose = Eigen::Isometry3d(
Eigen::Translation3d(
0.1, 0,
0.8) *
Eigen::Quaterniond::Identity());

// Plan with collision avoidance
auto planned_trajectory = example_node->planToPoint(
standing_pose, "ompl",
"RRTConnectkConfigDefault");
if (planned_trajectory != nullptr) {
example_node->drawTrajectory(*planned_trajectory);
example_node->addBreakPoint();
example_node->moveGroupInterface()->execute(*planned_trajectory);
}

// Shutdown ROS
rclcpp::shutdown();
return 0;
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,79 @@
// Copyright 2022 Áron Svastits
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <math.h>

#include <kuka_driver_interfaces/msg/collision_box.hpp>
#include <memory>

#include "moveit_example.h"

int main(int argc, char * argv[])
{
// Setup
// Initialize ROS and create the Node
rclcpp::init(argc, argv);
auto const example_node = std::make_shared<MoveitExample>();
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(example_node);
std::thread(
[&executor]()
{executor.spin();})
.detach();

example_node->initialize();
example_node->addBreakPoint();

// Add robot platform
example_node->addRobotPlatform();

// Add collision object
kuka_driver_interfaces::msg::CollisionBox box_msg;
box_msg.position.x = 0.25;
box_msg.position.y = -0.075;
box_msg.position.z = 0.675;

box_msg.size.x = 0.1;
box_msg.size.y = 0.4;
box_msg.size.z = 0.1;

example_node->addCollisionBox(std::make_shared<kuka_driver_interfaces::msg::CollisionBox>(box_msg));
example_node->addBreakPoint();

auto cart_goal = Eigen::Isometry3d(
Eigen::Translation3d(
0.4, -0.15,
0.55) *
Eigen::Quaterniond::Identity());

geometry_msgs::msg::Quaternion q;
q.x = 0;
q.y = 0;
q.z = 0;
q.w = 1;

example_node->setOrientationConstraint(q);
// Plan with collision avoidance
auto planned_trajectory =
example_node->planToPoint(cart_goal, "ompl", "RRTConnectkConfigDefault");
if (planned_trajectory != nullptr) {
example_node->drawTrajectory(*planned_trajectory);
example_node->addBreakPoint();
example_node->moveGroupInterface()->execute(*planned_trajectory);
}

// Shutdown ROS
rclcpp::shutdown();
return 0;
}

0 comments on commit 16cf499

Please sign in to comment.