diff --git a/ras_moveit/CMakeLists.txt b/ras_moveit/CMakeLists.txt index f3ce378..19fae78 100644 --- a/ras_moveit/CMakeLists.txt +++ b/ras_moveit/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(std_srvs REQUIRED) install( DIRECTORY + config include launch DESTINATION @@ -26,7 +27,11 @@ install( include_directories(include) -add_executable(moveit_server src/moveit_server.cpp) +## build executable +add_executable(moveit_server + src/moveit_server.cpp + src/moveit_services.cpp + ) ament_target_dependencies(moveit_server rclcpp @@ -43,12 +48,7 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/ras_moveit/config/tune_params.yaml b/ras_moveit/config/tune_params.yaml new file mode 100644 index 0000000..17bac1c --- /dev/null +++ b/ras_moveit/config/tune_params.yaml @@ -0,0 +1,56 @@ +moveit_server: + ros__parameters: + collision_object_frame: "world" + base_frame_id: "link_base" + end_effector_frame_id: "link_eef" + orientation_tolerance: + x: 0.75 + y: 0.75 + z: 0.75 + trajectory: + velocity_scaling_factor: 0.2 + acceleration_scaling_factor: 0.4 + planning_attempts: 5 + planning_time: 2.0 + goal_tolerance: 0.005 + goal_orientation_tolerance: 0.005 + sync: + velocity_scaling_factor: 0.8 + acceleration_scaling_factor: 1.0 + planning_attempts: 5 + planning_time: 2.0 + goal_tolerance: 0.005 + goal_orientation_tolerance: 0.005 + workspace: + minx: -1.0 + miny: -1.0 + minz: -1.0 + maxx: 1.0 + maxy: 1.0 + maxz: 1.0 + planner_id: RRTConnect + + # Available planner id + # - SBL + # - EST + # - LBKPIECE + # - BKPIECE + # - KPIECE + # - RRT + # - RRTConnect + # - RRTstar + # - TRRT + # - PRM + # - PRMstar + # - FMT + # - BFMT + # - PDST + # - STRIDE + # - BiTRRT + # - LBTRRT + # - BiEST + # - ProjEST + # - LazyPRM + # - LazyPRMstar + # - SPARS + # - SPARStwo diff --git a/ras_moveit/include/moveit_server.hpp b/ras_moveit/include/moveit_server.hpp deleted file mode 100644 index 6cce2f1..0000000 --- a/ras_moveit/include/moveit_server.hpp +++ /dev/null @@ -1,94 +0,0 @@ -/* - * - * Copyright (C) 2024 Harsh Davda - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU Affero General Public License as published - * by the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU Affero General Public License for more details. - * - * You should have received a copy of the GNU Affero General Public License - * along with this program. If not, see . - * - * For inquiries or further information, you may contact: - * Harsh Davda - * Email: info@opensciencestack.org -*/ - -#ifndef MOVEIT_SERVER_HPP -#define MOVEIT_SERVER_HPP - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using moveit::planning_interface::MoveGroupInterface; - -class MoveitServer : public rclcpp::Node, public std::enable_shared_from_this -{ -public: - MoveitServer(std::shared_ptr move_group_node); - void move_to_pose_callback(const std::shared_ptr request, std::shared_ptr response); - void move_to_joint_callback(const std::shared_ptr request, std::shared_ptr response); - void rotate_effector_callback(const std::shared_ptr request, std::shared_ptr response); - void joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr msg); - void sync_callback(const std::shared_ptr request,std::shared_ptr response); - void set_constraints(const geometry_msgs::msg::Pose::_orientation_type& quat); - bool Execute(geometry_msgs::msg::Pose target_pose); - bool Execute(sensor_msgs::msg::JointState target_joints); - void trajectory_callback(const std::shared_ptr request, - std::shared_ptr response); - // void trajectory_callback(const std::shared_ptr request, - // std::shared_ptr response); - void AddScenePlane(); - -private: - std::shared_ptr move_group_arm; - moveit::planning_interface::PlanningSceneInterface planning_scene_interface; - // moveit::core::RobotStatePtr current_state_arm; - // const moveit::core::JointModelGroup *joint_model_group_arm; - // moveit::planning_interface::MoveGroupInterface::Plan my_plan_arm; - - rclcpp::Service::SharedPtr move_to_pose_srv_; - rclcpp::Service::SharedPtr move_to_joint_srv_; - rclcpp::Publisher::SharedPtr trajectory_pub; - rclcpp::Service::SharedPtr rotate_effector_srv_; - rclcpp::Subscription::SharedPtr joint_state_sub_; - rclcpp::Service::SharedPtr sync_srv; - rclcpp::Service::SharedPtr execute_traj_srv; - - std::vector joint_angle; - std::string collision_object_frame; - std::string base_frame_id; - std::string end_effector_frame_id; - std::string move_group_name; -}; - -#endif \ No newline at end of file diff --git a/ras_moveit/include/ras_moveit/moveit_server.hpp b/ras_moveit/include/ras_moveit/moveit_server.hpp new file mode 100644 index 0000000..c736409 --- /dev/null +++ b/ras_moveit/include/ras_moveit/moveit_server.hpp @@ -0,0 +1,137 @@ +/* + * + * Copyright (C) 2024 Harsh Davda + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published + * by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * For inquiries or further information, you may contact: + * Harsh Davda + * Email: info@opensciencestack.org + */ + +#ifndef MOVEIT_SERVER_HPP +#define MOVEIT_SERVER_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using moveit::planning_interface::MoveGroupInterface; + + +struct Tolerance +{ + double x; + double y; + double z; +}; + +struct Workspace +{ + double minx; + double miny; + double minz; + double maxx; + double maxy; + double maxz; +}; + +struct MotionFactor +{ + double velocity_scaling_factor; + double acceleration_scaling_factor; + int planning_attempts; + double planning_time; + double goal_tolerance; + double goal_orientation_tolerance; +}; + +struct PlanningParameters +{ + Tolerance orientation_tolerance; + Workspace workspace; + MotionFactor sync; + MotionFactor trajectory; + std::string planner_id; +}; + +class MoveitServer : public rclcpp::Node, public std::enable_shared_from_this +{ +public: + MoveitServer(std::shared_ptr move_group_node); + ~MoveitServer(); + void move_to_pose_callback(const std::shared_ptr request, std::shared_ptr response); + void move_to_joint_callback(const std::shared_ptr request, std::shared_ptr response); + void rotate_effector_callback(const std::shared_ptr request, std::shared_ptr response); + void joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr msg); + void sync_callback(const std::shared_ptr request, std::shared_ptr response); + void set_constraints(const geometry_msgs::msg::Pose::_orientation_type &quat); + bool Execute(const geometry_msgs::msg::Pose target_pose, const MotionFactor motion); + bool Execute(const sensor_msgs::msg::JointState target_joints, const MotionFactor motion); + void trajectory_callback(const std::shared_ptr request, + std::shared_ptr response); + // void trajectory_callback(const std::shared_ptr request, + // std::shared_ptr response); + void AddScenePlane(); + +private: + void configure_move_group(const MotionFactor motion_factor); + bool plan_and_execute_with_retries(bool publish_trajectory = true); + + std::shared_ptr move_group_arm; + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + // moveit::core::RobotStatePtr current_state_arm; + // const moveit::core::JointModelGroup *joint_model_group_arm; + // moveit::planning_interface::MoveGroupInterface::Plan my_plan_arm; + + rclcpp::Service::SharedPtr move_to_pose_srv_; + rclcpp::Service::SharedPtr move_to_joint_srv_; + rclcpp::Publisher::SharedPtr trajectory_pub; + rclcpp::Service::SharedPtr rotate_effector_srv_; + rclcpp::Subscription::SharedPtr joint_state_sub_; + rclcpp::Service::SharedPtr sync_srv; + rclcpp::Service::SharedPtr execute_traj_srv; + + std::vector joint_angle; + std::string collision_object_frame; + std::string base_frame_id; + std::string end_effector_frame_id; + std::string move_group_name; + + PlanningParameters planning_parameters_; +}; + +#endif \ No newline at end of file diff --git a/ras_moveit/launch/moveit_real_server.launch.py b/ras_moveit/launch/moveit_real_server.launch.py index 1622871..e9fe012 100644 --- a/ras_moveit/launch/moveit_real_server.launch.py +++ b/ras_moveit/launch/moveit_real_server.launch.py @@ -3,14 +3,18 @@ from ras_common.config.loaders.lab_setup import LabSetup as LabLoader from ras_resource_lib.managers.asset_manager import AssetManager,AssetType from ras_resource_lib.types.manipulator.component import ManipulatorComponent - +from ament_index_python.packages import get_package_share_directory +import os def generate_launch_description(): - AssetManager.init() - LabLoader.init() - robot_component : ManipulatorComponent = AssetManager.get_asset_component(LabLoader.robot_name,AssetType.MANIPULATOR) - return LaunchDescription([ - Node( + AssetManager.init() + LabLoader.init() + robot_component : ManipulatorComponent = AssetManager.get_asset_component(LabLoader.robot_name,AssetType.MANIPULATOR) + config_directory = os.path.join(get_package_share_directory('ras_moveit'), 'config') + params_file = os.path.join(config_directory, 'tune_params.yaml') + + return LaunchDescription([ + Node( package='ras_moveit', # namespace='moveit_server', executable='moveit_server', @@ -18,7 +22,7 @@ def generate_launch_description(): output='screen', parameters=[ {'move_group_name': robot_component.movegroup_name}, - ] - ), + params_file + ]), - ]) \ No newline at end of file + ]) \ No newline at end of file diff --git a/ras_moveit/launch/moveit_server.launch.py b/ras_moveit/launch/moveit_server.launch.py index 1622871..451c377 100644 --- a/ras_moveit/launch/moveit_server.launch.py +++ b/ras_moveit/launch/moveit_server.launch.py @@ -3,14 +3,19 @@ from ras_common.config.loaders.lab_setup import LabSetup as LabLoader from ras_resource_lib.managers.asset_manager import AssetManager,AssetType from ras_resource_lib.types.manipulator.component import ManipulatorComponent +from ament_index_python.packages import get_package_share_directory +import os def generate_launch_description(): - AssetManager.init() - LabLoader.init() - robot_component : ManipulatorComponent = AssetManager.get_asset_component(LabLoader.robot_name,AssetType.MANIPULATOR) - return LaunchDescription([ - Node( + AssetManager.init() + LabLoader.init() + robot_component : ManipulatorComponent = AssetManager.get_asset_component(LabLoader.robot_name,AssetType.MANIPULATOR) + config_directory = os.path.join(get_package_share_directory('ras_moveit'), 'config') + params_file = os.path.join(config_directory, 'tune_params.yaml') + + return LaunchDescription([ + Node( package='ras_moveit', # namespace='moveit_server', executable='moveit_server', @@ -18,7 +23,6 @@ def generate_launch_description(): output='screen', parameters=[ {'move_group_name': robot_component.movegroup_name}, - ] - ), - - ]) \ No newline at end of file + params_file + ]), + ]) \ No newline at end of file diff --git a/ras_moveit/src/moveit_server.cpp b/ras_moveit/src/moveit_server.cpp index f6c07fd..59aee02 100644 --- a/ras_moveit/src/moveit_server.cpp +++ b/ras_moveit/src/moveit_server.cpp @@ -1,434 +1,252 @@ /* - * + * * Copyright (C) 2024 Harsh Davda - * + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU Affero General Public License as published * by the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. - * + * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU Affero General Public License for more details. - * + * * You should have received a copy of the GNU Affero General Public License * along with this program. If not, see . - * + * * For inquiries or further information, you may contact: * Harsh Davda * Email: info@opensciencestack.org -*/ - -#include "../include/moveit_server.hpp" - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("Moveit Server Init"); + */ +#include "ras_moveit/moveit_server.hpp" +// static const rclcpp::this->get_logger() this->get_logger() = rclcpp::get_logger("Moveit Server Init"); MoveitServer::MoveitServer(std::shared_ptr move_group_node) : Node("moveit_server") - { +{ + + + this->declare_parameter("move_group_name", "lite6"); + this->declare_parameter("collision_object_frame", "world"); + this->declare_parameter("base_frame_id", "link_base"); + this->declare_parameter("end_effector_frame_id", "link_eef"); + this->declare_parameter("planner_id", "RRTConnect"); + this->declare_parameter("orientation_tolerance.x", 0.75); + this->declare_parameter("orientation_tolerance.y", 0.75); + this->declare_parameter("orientation_tolerance.z", 0.75); + this->declare_parameter("trajectory.planning_attempts", 5); + this->declare_parameter("trajectory.planning_time", 2.0); + this->declare_parameter("trajectory.goal_tolerance", 0.005); + this->declare_parameter("trajectory.goal_orientation_tolerance", 0.005); + this->declare_parameter("trajectory.velocity_scaling_factor", 0.2); + this->declare_parameter("trajectory.acceleration_scaling_factor", 0.4); + this->declare_parameter("sync.planning_attempts", 5); + this->declare_parameter("sync.planning_time", 2.0); + this->declare_parameter("sync.goal_tolerance", 0.005); + this->declare_parameter("sync.goal_orientation_tolerance", 0.005); + this->declare_parameter("sync.velocity_scaling_factor", 0.2); + this->declare_parameter("sync.acceleration_scaling_factor", 0.4); + + this->declare_parameter("workspace.minx", -1.0); + this->declare_parameter("workspace.miny", -1.0); + this->declare_parameter("workspace.minz", -1.0); + this->declare_parameter("workspace.maxx", 1.0); + this->declare_parameter("workspace.maxy", 1.0); + this->declare_parameter("workspace.maxz", 1.0); + + planning_parameters_.orientation_tolerance.x = this->get_parameter("orientation_tolerance.x").as_double(); + planning_parameters_.orientation_tolerance.y = this->get_parameter("orientation_tolerance.y").as_double(); + planning_parameters_.orientation_tolerance.z = this->get_parameter("orientation_tolerance.z").as_double(); + planning_parameters_.trajectory.planning_attempts = this->get_parameter("trajectory.planning_attempts").as_int(); + planning_parameters_.trajectory.planning_time = this->get_parameter("trajectory.planning_time").as_double(); + planning_parameters_.trajectory.goal_tolerance = this->get_parameter("trajectory.goal_tolerance").as_double(); + planning_parameters_.trajectory.goal_orientation_tolerance = this->get_parameter("trajectory.goal_orientation_tolerance").as_double(); + planning_parameters_.trajectory.velocity_scaling_factor = this->get_parameter("trajectory.velocity_scaling_factor").as_double(); + planning_parameters_.trajectory.acceleration_scaling_factor = this->get_parameter("trajectory.acceleration_scaling_factor").as_double(); + planning_parameters_.sync.planning_attempts = this->get_parameter("sync.planning_attempts").as_int(); + planning_parameters_.sync.planning_time = this->get_parameter("sync.planning_time").as_double(); + planning_parameters_.sync.goal_tolerance = this->get_parameter("sync.goal_tolerance").as_double(); + planning_parameters_.sync.goal_orientation_tolerance = this->get_parameter("sync.goal_orientation_tolerance").as_double(); + planning_parameters_.sync.velocity_scaling_factor = this->get_parameter("sync.velocity_scaling_factor").as_double(); + planning_parameters_.sync.acceleration_scaling_factor = this->get_parameter("sync.acceleration_scaling_factor").as_double(); + planning_parameters_.planner_id = this->get_parameter("planner_id").as_string(); - this->declare_parameter("move_group_name", "lite6"); - this->declare_parameter("collision_object_frame", "world"); - this->declare_parameter("base_frame_id", "link_base"); - this->declare_parameter("end_effector_frame_id", "link_eef"); + + move_group_name = this->get_parameter("move_group_name").as_string(); + collision_object_frame = this->get_parameter("collision_object_frame").as_string(); + base_frame_id = this->get_parameter("base_frame_id").as_string(); + end_effector_frame_id = this->get_parameter("end_effector_frame_id").as_string(); - move_group_name = this->get_parameter("move_group_name").as_string(); - collision_object_frame = this->get_parameter("collision_object_frame").as_string(); - base_frame_id = this->get_parameter("base_frame_id").as_string(); - end_effector_frame_id = this->get_parameter("end_effector_frame_id").as_string(); + move_group_arm = std::make_shared( + move_group_node, + move_group_name); - move_group_arm = std::make_shared( - move_group_node, - move_group_name - ); + RCLCPP_INFO(this->get_logger(), "Node Started"); + move_to_pose_srv_ = this->create_service( + "/create_traj", + std::bind(&MoveitServer::move_to_pose_callback, this, std::placeholders::_1, std::placeholders::_2)); - RCLCPP_INFO(LOGGER, "Node Started"); + move_to_joint_srv_ = this->create_service( + "/move_to_joint_states", + std::bind(&MoveitServer::move_to_joint_callback, this, std::placeholders::_1, std::placeholders::_2)); - move_to_pose_srv_ = this->create_service( - "/create_traj", - std::bind(&MoveitServer::move_to_pose_callback, this, std::placeholders::_1, std::placeholders::_2)); + rotate_effector_srv_ = this->create_service( + "/rotate_effector", + std::bind(&MoveitServer::rotate_effector_callback, this, std::placeholders::_1, std::placeholders::_2)); - move_to_joint_srv_ = this->create_service( - "/move_to_joint_states", - std::bind(&MoveitServer::move_to_joint_callback, this, std::placeholders::_1, std::placeholders::_2)); + joint_state_sub_ = this->create_subscription( + "/joint_states", 10, std::bind(&MoveitServer::joint_state_callback, this, std::placeholders::_1)); - rotate_effector_srv_ = this->create_service( - "/rotate_effector", - std::bind(&MoveitServer::rotate_effector_callback, this, std::placeholders::_1, std::placeholders::_2)); + trajectory_pub = this->create_publisher("trajectory_topic", 10); - joint_state_sub_ = this->create_subscription( - "/joint_states", 10, std::bind(&MoveitServer::joint_state_callback, this, std::placeholders::_1)); + sync_srv = this->create_service( + "/sync_arm", + std::bind(&MoveitServer::sync_callback, this, std::placeholders::_1, std::placeholders::_2)); - trajectory_pub = this->create_publisher("trajectory_topic", 10); - - sync_srv = this->create_service( - "/sync_arm", - std::bind(&MoveitServer::sync_callback, this, std::placeholders::_1, std::placeholders::_2) - ); + execute_traj_srv = this->create_service( + "trajectory_topic", + std::bind(&MoveitServer::trajectory_callback, this, std::placeholders::_1, std::placeholders::_2)); - execute_traj_srv = this->create_service( - "trajectory_topic", - std::bind(&MoveitServer::trajectory_callback, this, std::placeholders::_1, std::placeholders::_2)); - - AddScenePlane(); - } + AddScenePlane(); +} - void MoveitServer::trajectory_callback(const std::shared_ptr request, - std::shared_ptr response) - { - RCLCPP_INFO(this->get_logger(), "Received trajectory message"); +MoveitServer::~MoveitServer() +{ +} - moveit_msgs::msg::RobotTrajectory robot_trajectory; +void MoveitServer::AddScenePlane() +{ + moveit_msgs::msg::CollisionObject collision_object; + collision_object.header.frame_id = collision_object_frame; + collision_object.id = "ground_plane"; + shape_msgs::msg::SolidPrimitive primitive; + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + primitive.dimensions[primitive.BOX_X] = 10; + primitive.dimensions[primitive.BOX_Y] = 10; + primitive.dimensions[primitive.BOX_Z] = 0.01; + geometry_msgs::msg::Pose box_pose; + box_pose.orientation.w = 1.0; + box_pose.position.x = 0.0; + box_pose.position.y = 0.0; + box_pose.position.z = -0.05; + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + collision_object.operation = collision_object.ADD; + planning_scene_interface.applyCollisionObject(collision_object); + RCLCPP_INFO(this->get_logger(), "Added plane to planning scene"); +} - // Convert JointTrajectory to RobotTrajectory - robot_trajectory.joint_trajectory = request->traj; +void MoveitServer::set_constraints(const geometry_msgs::msg::Pose::_orientation_type &quat) +{ + RCLCPP_INFO(this->get_logger(), "Orientation Constrains Set"); + + // Goal constraints - position + moveit_msgs::msg::Constraints goal_constraints; + + // Goal constraints - orientation + moveit_msgs::msg::OrientationConstraint ori_constraint; + ori_constraint.header.stamp = this->get_clock()->now(); + ori_constraint.header.frame_id = base_frame_id; + ori_constraint.orientation.x = quat.x; + ori_constraint.orientation.y = quat.y; + ori_constraint.orientation.z = quat.z; + ori_constraint.orientation.w = quat.w; + ori_constraint.link_name = end_effector_frame_id; + ori_constraint.absolute_x_axis_tolerance = planning_parameters_.orientation_tolerance.x; + ori_constraint.absolute_y_axis_tolerance = planning_parameters_.orientation_tolerance.y; + ori_constraint.absolute_z_axis_tolerance = planning_parameters_.orientation_tolerance.z; + ori_constraint.weight = 1.0; + ori_constraint.parameterization = 1.0; + goal_constraints.orientation_constraints.push_back(ori_constraint); + + move_group_arm->setPathConstraints(goal_constraints); +} - bool success = (move_group_arm->execute(robot_trajectory) == moveit::planning_interface::MoveItErrorCode::SUCCESS); +bool MoveitServer::Execute(const sensor_msgs::msg::JointState target_joints, const MotionFactor motion) +{ + // move_group_arm->clearPathConstraints(); + // RCLCPP_INFO(this->get_logger(), "clear constraints"); + configure_move_group(motion); + move_group_arm->setJointValueTarget(target_joints); + return plan_and_execute_with_retries(); +} - response->success = 1; - } - - void MoveitServer::AddScenePlane() - { - moveit_msgs::msg::CollisionObject collision_object; - collision_object.header.frame_id = collision_object_frame; - collision_object.id = "ground_plane"; - shape_msgs::msg::SolidPrimitive primitive; - primitive.type = primitive.BOX; - primitive.dimensions.resize(3); - primitive.dimensions[primitive.BOX_X] = 10; - primitive.dimensions[primitive.BOX_Y] = 10; - primitive.dimensions[primitive.BOX_Z] = 0.01; - geometry_msgs::msg::Pose box_pose; - box_pose.orientation.w = 1.0; - box_pose.position.x = 0.0; - box_pose.position.y = 0.0; - box_pose.position.z = -0.05; - collision_object.primitives.push_back(primitive); - collision_object.primitive_poses.push_back(box_pose); - collision_object.operation = collision_object.ADD; - planning_scene_interface.applyCollisionObject(collision_object); - RCLCPP_INFO(this->get_logger(), "Added plane to planning scene"); - } +bool MoveitServer::Execute(const geometry_msgs::msg::Pose target_pose, const MotionFactor motion) +{ + trajectory_msgs::msg::JointTrajectory trajectory_msg; + // move_group_arm->clearPathConstraints(); + RCLCPP_INFO(this->get_logger(), "clear constraints"); - void MoveitServer::set_constraints(const geometry_msgs::msg::Pose::_orientation_type& quat) - { - RCLCPP_INFO(LOGGER, "Orientation Constrains Set"); - - // Goal constraints - position - - - moveit_msgs::msg::Constraints goal_constraints; - - // Goal constraints - orientation - moveit_msgs::msg::OrientationConstraint ori_constraint; - ori_constraint.header.stamp = this->get_clock()->now(); // Set the current time - ori_constraint.header.frame_id = base_frame_id; - ori_constraint.orientation.x = quat.x; - ori_constraint.orientation.y = quat.y; - ori_constraint.orientation.z = quat.z; - ori_constraint.orientation.w = quat.w; - ori_constraint.link_name = end_effector_frame_id; - ori_constraint.absolute_x_axis_tolerance = 0.75; - ori_constraint.absolute_y_axis_tolerance = 0.75; - ori_constraint.absolute_z_axis_tolerance = 0.75; - ori_constraint.weight = 1.0; - ori_constraint.parameterization = 1.0; - goal_constraints.orientation_constraints.push_back(ori_constraint); - - move_group_arm->setPathConstraints(goal_constraints); - } + configure_move_group(motion); + set_constraints(target_pose.orientation); + move_group_arm->setPoseTarget(target_pose); + return plan_and_execute_with_retries(); +} +void MoveitServer::configure_move_group(const MotionFactor motion_factor) +{ + move_group_arm->setWorkspace(planning_parameters_.workspace.minx, planning_parameters_.workspace.miny, planning_parameters_.workspace.minz, planning_parameters_.workspace.maxx, planning_parameters_.workspace.maxy, planning_parameters_.workspace.maxz); + move_group_arm->setPlannerId(planning_parameters_.planner_id); + move_group_arm->setNumPlanningAttempts(motion_factor.planning_attempts); + move_group_arm->setPlanningTime(motion_factor.planning_time); + move_group_arm->setGoalTolerance(motion_factor.goal_tolerance); + move_group_arm->setGoalOrientationTolerance(motion_factor.goal_orientation_tolerance); + move_group_arm->setMaxVelocityScalingFactor(motion_factor.velocity_scaling_factor); + move_group_arm->setMaxAccelerationScalingFactor(motion_factor.acceleration_scaling_factor); +} - bool MoveitServer::Execute(sensor_msgs::msg::JointState target_joints) +bool MoveitServer::plan_and_execute_with_retries(bool publish_trajectory) +{ + trajectory_msgs::msg::JointTrajectory trajectory_msg; + int count = 15; + for (int i = 0; i < count; i++) { - RCLCPP_INFO(this->get_logger(),"function call"); - - trajectory_msgs::msg::JointTrajectory trajectory_msg; - - // move_group_arm->clearPathConstraints(); - RCLCPP_INFO(this->get_logger(),"clear constraints"); - - move_group_arm->setWorkspace(-1.0, -1.0, -1.0, 1.0, 1.0, 1.0); - - move_group_arm->setPlannerId("RRTConnectkConfigDefault"); - - move_group_arm->setNumPlanningAttempts(5); - move_group_arm->setPlanningTime(1); - move_group_arm->setGoalTolerance(0.005); - move_group_arm->setGoalOrientationTolerance(0.005); - move_group_arm->setMaxVelocityScalingFactor(0.2); - move_group_arm->setMaxAccelerationScalingFactor(0.4); - RCLCPP_INFO(this->get_logger(),"beforeconstraints"); - - - RCLCPP_INFO(this->get_logger(),"after constraints"); - move_group_arm->setJointValueTarget(target_joints); - - RCLCPP_INFO(this->get_logger(),"after target pose"); - - int count = 15; - for (int i = 0; i < count; i++) + if (i < count - 2) { - if (i < count-2) - { - moveit::planning_interface::MoveGroupInterface::Plan my_plan; - bool success = (move_group_arm->plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - RCLCPP_INFO(this->get_logger(),"after target plan"); - trajectory_msg = my_plan.trajectory_.joint_trajectory; - if (success) - { + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + bool success = (move_group_arm->plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + trajectory_msg = my_plan.trajectory_.joint_trajectory; + if (success) + { move_group_arm->execute(my_plan); - trajectory_pub->publish(trajectory_msg); - return 1; - } - } - else - { - move_group_arm->clearPathConstraints(); - RCLCPP_INFO(this->get_logger(), "Clearning Constraints"); - moveit::planning_interface::MoveGroupInterface::Plan my_plan2; - bool success2 = (move_group_arm->plan(my_plan2) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - trajectory_msg = my_plan2.trajectory_.joint_trajectory; - if (success2) + if (publish_trajectory) { - move_group_arm->execute(my_plan2); - trajectory_pub->publish(trajectory_msg); - return 1; + trajectory_pub->publish(trajectory_msg); } - else - { - return 0; - } - } + break; + } } - } - - bool MoveitServer::Execute(geometry_msgs::msg::Pose target_pose) { - RCLCPP_INFO(this->get_logger(),"function call"); - - trajectory_msgs::msg::JointTrajectory trajectory_msg; - - // move_group_arm->clearPathConstraints(); - RCLCPP_INFO(this->get_logger(),"clear constraints"); - - move_group_arm->setWorkspace(-1.0, -1.0, -1.0, 1.0, 1.0, 1.0); - - move_group_arm->setPlannerId("RRTConnectkConfigDefault"); - - move_group_arm->setNumPlanningAttempts(5); - move_group_arm->setPlanningTime(1); - move_group_arm->setGoalTolerance(0.005); - move_group_arm->setGoalOrientationTolerance(0.005); - move_group_arm->setMaxVelocityScalingFactor(0.2); - move_group_arm->setMaxAccelerationScalingFactor(0.4); - RCLCPP_INFO(this->get_logger(),"beforeconstraints"); - - set_constraints(target_pose.orientation); - - RCLCPP_INFO(this->get_logger(),"after constraints"); - move_group_arm->setPoseTarget(target_pose); - - RCLCPP_INFO(this->get_logger(),"after target pose"); - - int count = 15; - for (int i = 0; i < count; i++) + else { - if (i < count-2) - { - moveit::planning_interface::MoveGroupInterface::Plan my_plan; - bool success = (move_group_arm->plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - RCLCPP_INFO(this->get_logger(),"after target plan"); - trajectory_msg = my_plan.trajectory_.joint_trajectory; - if (success) - { - move_group_arm->execute(my_plan); - trajectory_pub->publish(trajectory_msg); - return 1; - } - } - else - { - move_group_arm->clearPathConstraints(); - RCLCPP_INFO(this->get_logger(), "Clearning Constraints"); - moveit::planning_interface::MoveGroupInterface::Plan my_plan2; - bool success2 = (move_group_arm->plan(my_plan2) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - trajectory_msg = my_plan2.trajectory_.joint_trajectory; - if (success2) - { + move_group_arm->clearPathConstraints(); + RCLCPP_INFO(this->get_logger(), "Clearning Constraints"); + moveit::planning_interface::MoveGroupInterface::Plan my_plan2; + bool success2 = (move_group_arm->plan(my_plan2) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + trajectory_msg = my_plan2.trajectory_.joint_trajectory; + if (success2) + { move_group_arm->execute(my_plan2); - trajectory_pub->publish(trajectory_msg); - return 1; - } - else + if (publish_trajectory) { - return 0; + trajectory_pub->publish(trajectory_msg); } - } - } - } - - void MoveitServer::move_to_pose_callback( - const std::shared_ptr request, - std::shared_ptr response) - { - RCLCPP_WARN(this->get_logger(), "Received Pose request..."); - - geometry_msgs::msg::Pose config_pose; - config_pose = request->object_pose; - - RCLCPP_INFO(this->get_logger(), - "Received pose:\n" - "Position: x=%f, y=%f, z=%f\n" - "Orientation: x=%f, y=%f, z=%f, w=%f", - config_pose.position.x, config_pose.position.y, config_pose.position.z, - config_pose.orientation.x, config_pose.orientation.y, config_pose.orientation.z, config_pose.orientation.w - ); - - bool status = Execute(config_pose); - - response->success = status; - } - - void MoveitServer::move_to_joint_callback( - const std::shared_ptr request, - std::shared_ptr response) - { - RCLCPP_WARN(this->get_logger(), "Received Joint request..."); - - sensor_msgs::msg::JointState config_joints; - config_joints = request->joints; - - - std::ostringstream joint_info; - joint_info << "Received joints:\n"; - for (size_t i = 0; i < config_joints.position.size(); ++i) { - joint_info << "Joint" << (i + 1) << ": " << config_joints.position[i] << "\n"; - } - RCLCPP_INFO(this->get_logger(), joint_info.str().c_str()); - - bool status = Execute(config_joints); - - response->success = status; - } - - void MoveitServer::rotate_effector_callback( - const std::shared_ptr request, - std::shared_ptr response) - { - trajectory_msgs::msg::JointTrajectory trajectory_msg; - RCLCPP_INFO(LOGGER, "Received RotateEffector request to rotate end effector by angle: %f", request->rotation_angle); - - - move_group_arm->setWorkspace(-1.0, -1.0, -1.0, 1.0, 1.0, 1.0); - - move_group_arm->setPlannerId("RRTConnectkConfigDefault"); - - move_group_arm->setNumPlanningAttempts(5); - move_group_arm->setPlanningTime(2); - move_group_arm->setGoalTolerance(0.005); - move_group_arm->setGoalOrientationTolerance(0.005); - move_group_arm->setMaxVelocityScalingFactor(0.4); - move_group_arm->setMaxAccelerationScalingFactor(0.4); - move_group_arm->clearPathConstraints(); - - // Ensure joint states are available - if (joint_angle.empty()) - { - RCLCPP_ERROR(LOGGER, "No joint states available. Aborting rotation."); - response->success = false; - return; - } - - int count = 5; - - // set_constraints(); - // Convert joint_angle (std::vector) to std::vector - std::vector target_joint_values = {joint_angle[2], joint_angle[0], joint_angle[1], joint_angle[3], joint_angle[4], joint_angle[5]+(request->rotation_angle)}; - - // Update the last joint (assumed to be joint6) by adding the rotation - - // target_joint_values[5] += request->rotation_angle; // Add rotation in radians to joint6 - RCLCPP_INFO(LOGGER, "Updated joint6 angle: %f", target_joint_values[5]); - - - for (int i = 0; i < count; i++) - { - move_group_arm->setJointValueTarget(target_joint_values); - moveit::planning_interface::MoveGroupInterface::Plan my_plan3; - bool success = (move_group_arm->plan(my_plan3) == moveit::core::MoveItErrorCode::SUCCESS); - trajectory_msg = my_plan3.trajectory_.joint_trajectory; - - if (success) - { - move_group_arm->execute(my_plan3); - trajectory_pub->publish(trajectory_msg); - response->success = true; - RCLCPP_INFO(LOGGER, "End effector rotation executed successfully."); break; - } - } - } - - void MoveitServer::sync_callback(const std::shared_ptr request, - std::shared_ptr response) - { - RCLCPP_INFO(this->get_logger(), "MoveitServer::sync_callback called"); - std::vector joint_values; - for (const auto& x : request->joint_state.position) - { - joint_values.push_back(x); - } - - move_group_arm->setJointValueTarget(joint_values); - move_group_arm->setMaxVelocityScalingFactor(0.7); - move_group_arm->setMaxAccelerationScalingFactor(0.7); - // testing purpose - move_group_arm->setWorkspace(-1.0, -1.0, -1.0, 1.0, 1.0, 1.0); - - // move_group_arm->setPlannerId("RRTConnectkConfigDefault"); - - move_group_arm->setNumPlanningAttempts(5); - move_group_arm->setPlanningTime(2); - move_group_arm->setGoalTolerance(0.01); - move_group_arm->setGoalOrientationTolerance(0.035); - move_group_arm->clearPathConstraints(); - - moveit::planning_interface::MoveGroupInterface::Plan my_plan; - bool result = (move_group_arm->plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); - if (result) - { - result = (move_group_arm->execute(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - if (result) - { - RCLCPP_INFO(this->get_logger(), "Trajectory executed successfully"); - response->successq = true; } else { - RCLCPP_ERROR(this->get_logger(), "Trajectory not executed"); - response->successq = false; + return 0; } } - else - { - RCLCPP_WARN(this->get_logger(), "Planning for trajectory failed"); - response->successq = false; - } - } - - void MoveitServer::joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr msg) // <-- Updated this line - { - joint_angle.clear(); - for (auto i : msg->position) - { - joint_angle.push_back(i); - } } + // TODO(Sachin): check if the goal executed successfully + return 1; +} -int main(int argc, char **argv) { +int main(int argc, char **argv) +{ rclcpp::init(argc, argv); auto move_group_node = rclcpp::Node::make_shared("moveit_server"); diff --git a/ras_moveit/src/moveit_services.cpp b/ras_moveit/src/moveit_services.cpp new file mode 100644 index 0000000..231dd63 --- /dev/null +++ b/ras_moveit/src/moveit_services.cpp @@ -0,0 +1,122 @@ +/* + * + * Copyright (C) 2025 Sachin Kumar + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published + * by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * For inquiries or further information, you may contact: + * Sachin Kumar + * Email: info@opensciencestack.org + */ + +#include "ras_moveit/moveit_server.hpp" + +void MoveitServer::trajectory_callback(const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_INFO(this->get_logger(), "Received trajectory message"); + + moveit_msgs::msg::RobotTrajectory robot_trajectory; + + // Convert JointTrajectory to RobotTrajectory + robot_trajectory.joint_trajectory = request->traj; + + bool success = (move_group_arm->execute(robot_trajectory) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + + response->success = 1; +} + +void MoveitServer::move_to_pose_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_WARN(this->get_logger(), "Received Pose request..."); + + geometry_msgs::msg::Pose config_pose; + config_pose = request->object_pose; + + RCLCPP_INFO(this->get_logger(), + "Received pose:\n" + "Position: x=%f, y=%f, z=%f\n" + "Orientation: x=%f, y=%f, z=%f, w=%f", + config_pose.position.x, config_pose.position.y, config_pose.position.z, + config_pose.orientation.x, config_pose.orientation.y, config_pose.orientation.z, config_pose.orientation.w); + + response->success = Execute(config_pose, planning_parameters_.trajectory); +} + +void MoveitServer::move_to_joint_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_WARN(this->get_logger(), "Received Joint request..."); + + sensor_msgs::msg::JointState config_joints; + config_joints = request->joints; + + std::ostringstream joint_info; + joint_info << "Received joints:\n"; + for (size_t i = 0; i < config_joints.position.size(); ++i) + { + joint_info << "Joint" << (i + 1) << ": " << config_joints.position[i] << "\n"; + } + RCLCPP_INFO(this->get_logger(), joint_info.str().c_str()); + + response->success = Execute(config_joints, planning_parameters_.trajectory); +} + +void MoveitServer::rotate_effector_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + // Ensure joint states are available + if (joint_angle.empty()) + { + RCLCPP_ERROR(this->get_logger(), "No joint states available. Aborting rotation."); + response->success = false; + return; + } + RCLCPP_INFO(this->get_logger(), "Received RotateEffector request to rotate end effector by angle: %f", request->rotation_angle); + // Convert joint_angle (std::vector) to std::vector + std::vector target_joint_values = {joint_angle[2], joint_angle[0], joint_angle[1], joint_angle[3], joint_angle[4], joint_angle[5] + (request->rotation_angle)}; + + move_group_arm->setJointValueTarget(target_joint_values); + configure_move_group(planning_parameters_.trajectory); + move_group_arm->clearPathConstraints(); + response->success = plan_and_execute_with_retries(); +} + +void MoveitServer::sync_callback(const std::shared_ptr request, + std::shared_ptr response) +{ + std::vector joint_values; + for (const auto &x : request->joint_state.position) + { + joint_values.push_back(x); + } + + move_group_arm->setJointValueTarget(joint_values); + configure_move_group(planning_parameters_.sync); + move_group_arm->clearPathConstraints(); + response->successq = plan_and_execute_with_retries(false); +} + +void MoveitServer::joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr msg) // <-- Updated this line +{ + joint_angle.clear(); + for (auto i : msg->position) + { + joint_angle.push_back(i); + } +} \ No newline at end of file