diff --git a/rasmt_moveit_config/launch/rasmt_moveit.launch.py b/rasmt_moveit_config/launch/rasmt_moveit.launch.py index 25cfcda..ae52ea4 100644 --- a/rasmt_moveit_config/launch/rasmt_moveit.launch.py +++ b/rasmt_moveit_config/launch/rasmt_moveit.launch.py @@ -174,28 +174,29 @@ def generate_launch_description(): ] ) - # move_to_joint_state_action_server = Node( - # package="robossembler_servers", - # executable="move_to_joint_states_action_server", - # name="joint_states_moveit", - # parameters=[ - # robot_description, - # robot_description_semantic, - # kinematics_yaml, - # robot_description_planning, - # ompl_yaml, - # planning, - # trajectory_execution, - # moveit_controllers, - # planning_scene_monitor_parameters, - # use_sim_time - # ] - # ) + move_cartesian_path_action_server = Node( + package="robossembler_servers", + executable="move_cartesian_path_action_server", + name="move_cartesian_path_action_server", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + robot_description_planning, + ompl_yaml, + planning, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + use_sim_time + ] + ) launch_nodes = [] launch_nodes.append(rviz) launch_nodes.append(move_group_node) launch_nodes.append(move_topose_action_server) + launch_nodes.append(move_cartesian_path_action_server) # launch_nodes.append(move_to_joint_state_action_server) diff --git a/rasmt_support/launch/rasmt_gazebo.launch.py b/rasmt_support/launch/rasmt_gazebo.launch.py index 086eb3d..1f3daca 100644 --- a/rasmt_support/launch/rasmt_gazebo.launch.py +++ b/rasmt_support/launch/rasmt_gazebo.launch.py @@ -19,6 +19,7 @@ def generate_launch_description(): )) world_file = os.path.join(get_package_share_directory("rasmt_support"), "world", "robossembler.world") + #test_world_file = "/home/bill-finger/gazebo_pkgs_ws/gazebo-pkgs/gazebo_grasp_plugin/test_world/test_world_full.world" gzserver = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -27,7 +28,7 @@ def generate_launch_description(): "launch", "gzserver.launch.py" ]) - ), launch_arguments={'world':world_file}.items() + ), launch_arguments={'world':world_file, 'extra_gazebo_args':'--verbose'}.items() ) gzclient = IncludeLaunchDescription( PythonLaunchDescriptionSource( diff --git a/robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml b/robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml index aa511c7..097abbc 100644 --- a/robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml +++ b/robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml @@ -7,9 +7,10 @@ - + - + + diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp index 4c72cb4..ada3ee9 100644 --- a/robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp +++ b/robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp @@ -15,7 +15,7 @@ class SendGripperCmd : public BtAction : BtAction(name, config) { //gripper_gap = 0.02; gripper_point_a_ = 0.06; - gripper_point_b_ = 0.025; + gripper_point_b_ = 0.05; gripper_cmd.insert(std::make_pair("open", gripper_point_a_)); gripper_cmd.insert(std::make_pair("close", gripper_point_b_)); diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp index 75e66e9..8591c45 100644 --- a/robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp +++ b/robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp @@ -8,7 +8,7 @@ using namespace BT; using MoveToPoseAction = robossembler_interfaces::action::MoveitSendPose; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToPointActionClient"); +static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToPoseActionClient"); class MoveToPose : public BtAction { @@ -23,16 +23,25 @@ class MoveToPose : public BtAction target_pose_a_.orientation.z = 0.0; target_pose_a_.orientation.w = 0.009; - target_pose_b_.position.x = 0.40644; + target_pose_b_.position.x = 0.0; target_pose_b_.position.y = 0.0; - target_pose_b_.position.z = 0.20; + target_pose_b_.position.z = -0.12; target_pose_b_.orientation.x = 0; - target_pose_b_.orientation.y = 1.0; + target_pose_b_.orientation.y = 0.0; target_pose_b_.orientation.z = 0.0; - target_pose_b_.orientation.w = 0.009; + target_pose_b_.orientation.w = 0.0; + + target_pose_c_.position.x = 0.0; + target_pose_c_.position.y = 0.0; + target_pose_c_.position.z = 0.12; + target_pose_c_.orientation.x = 0; + target_pose_c_.orientation.y = 0.0; + target_pose_c_.orientation.z = 0.0; + target_pose_c_.orientation.w = 0.0; pick_and_place_command.insert(std::make_pair("go_to", target_pose_a_)); - pick_and_place_command.insert(std::make_pair("pick", target_pose_b_)); + pick_and_place_command.insert(std::make_pair("down", target_pose_b_)); + pick_and_place_command.insert(std::make_pair("up", target_pose_c_)); } @@ -62,7 +71,7 @@ class MoveToPose : public BtAction private: geometry_msgs::msg::Pose target_pose_a_; - geometry_msgs::msg::Pose target_pose_b_; + geometry_msgs::msg::Pose target_pose_b_, target_pose_c_; std::string robot_name_; std::map pick_and_place_command; std::string command; diff --git a/robossembler_servers/src/move_cartesian_path_action_server.cpp b/robossembler_servers/src/move_cartesian_path_action_server.cpp new file mode 100644 index 0000000..18dcaf1 --- /dev/null +++ b/robossembler_servers/src/move_cartesian_path_action_server.cpp @@ -0,0 +1,181 @@ +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +// action libs +#include "rclcpp_action/rclcpp_action.hpp" +#include "robossembler_interfaces/msg/action_feedback_status_constants.hpp" +#include "robossembler_interfaces/action/moveit_send_pose.hpp" + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/transform.hpp" + +// moveit libs +#include "moveit/move_group_interface/move_group_interface.h" +#include "moveit/planning_interface/planning_interface.h" +#include "moveit/robot_model_loader/robot_model_loader.h" + +/* +#include +#include +#include +*/ +// For Visualization +//#include +#include "moveit_msgs/msg/display_robot_state.hpp" +#include "moveit_msgs/msg/display_trajectory.hpp" +#include "moveit_msgs/msg/robot_trajectory.hpp" +#include "moveit_msgs/action/move_group.hpp" + +namespace robossembler_actions +{ + +class MoveCartesianActionServer : public rclcpp::Node +{ +public: + using MoveitSendPose = robossembler_interfaces::action::MoveitSendPose; + + //explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node) + explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_cartesian_action_server"), node_(node) + { + // using namespace std::placeholders; + // this->action_server_ = rclcpp_action::create_server( + // this->get_node_base_interface(), + // this->get_node_clock_interface(), + // this->get_node_logging_interface(), + // this->get_node_waitables_interface(), + // "move_topose", + // std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2), + // std::bind(&MoveCartesianActionServer::cancel_callback, this, _1), + // std::bind(&MoveCartesianActionServer::accepted_callback, this, _1)); + } + + void init() + { + + action_server_ = rclcpp_action::create_server( + node_->get_node_base_interface(), + node_->get_node_clock_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), + "move_cartesian", + std::bind(&MoveCartesianActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&MoveCartesianActionServer::cancel_callback, this, std::placeholders::_1), + std::bind(&MoveCartesianActionServer::accepted_callback, this, std::placeholders::_1) + ); + + } + +private: + rclcpp::Node::SharedPtr node_; + rclcpp_action::Server::SharedPtr action_server_; + + using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + + rclcpp_action::GoalResponse goal_callback( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) + { + RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, %f, %f]", + goal->robot_name.c_str(), goal->target_pose.position.x, goal->target_pose.position.y, goal->target_pose.position.z, + goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z, + goal->target_pose.orientation.w); + (void)uuid; + if (false) { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle) + { + RCLCPP_INFO(this->get_logger(), "Received cancel request"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void accepted_callback(const std::shared_ptr goal_handle) + { + using namespace std::placeholders; + std::thread(std::bind(&MoveCartesianActionServer::execute, this, _1), goal_handle).detach(); + } + + void execute(const std::shared_ptr goal_handle) + { + RCLCPP_INFO(this->get_logger(), "Executing action goal"); + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + + moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name); + + + moveit::core::RobotState start_state(*move_group_interface.getCurrentState()); + auto start_pose = move_group_interface.getCurrentPose(); + + std::vector waypoints; + RCLCPP_INFO(this->get_logger(), "Current with Pose [%f, %f, %f, %f, %f, %f, %f]", + start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z, + start_pose.pose.orientation.x, start_pose.pose.orientation.y, start_pose.pose.orientation.z, + start_pose.pose.orientation.w); + //waypoints.push_back(start_pose.pose); + geometry_msgs::msg::Pose target_pose = start_pose.pose; + //goal->target_pose + target_pose.position.z = target_pose.position.z + goal->target_pose.position.z; + target_pose.position.y = target_pose.position.y + goal->target_pose.position.y; + target_pose.position.x = target_pose.position.x + goal->target_pose.position.x; + waypoints.push_back(target_pose); + + RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]", + target_pose.position.x, target_pose.position.y, target_pose.position.z); + //waypoints.push_back(start_pose.pose); + moveit_msgs::msg::RobotTrajectory trajectory; + const double jump_threshold = 0.0; + const double eef_step = 0.01; + double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); + waypoints.clear(); + if(fraction>0) + { + RCLCPP_INFO(this->get_logger(), "Planning success"); + move_group_interface.execute(trajectory); + //move_group_interface.move(); + }else{ + RCLCPP_WARN(this->get_logger(), "Failed to generate plan"); + } + + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); + return; + } + + + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); + } +}; // class MoveCartesianActionServer + +}// namespace robossembler_actions + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options); + + robossembler_actions::MoveCartesianActionServer server(node); + std::thread run_server([&server]() { + rclcpp::sleep_for(std::chrono::seconds(3)); + server.init(); + }); + + rclcpp::spin(node); + run_server.join(); + + return 0; +} \ No newline at end of file diff --git a/robossembler_servers/src/move_topose_action_server.cpp b/robossembler_servers/src/move_topose_action_server.cpp index 34cfae6..0e5eff2 100644 --- a/robossembler_servers/src/move_topose_action_server.cpp +++ b/robossembler_servers/src/move_topose_action_server.cpp @@ -112,9 +112,7 @@ private: auto result = std::make_shared(); moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name); - - std::copy(move_group_interface.getJointModelGroupNames().begin(), move_group_interface.getJointModelGroupNames().end(), - std::ostream_iterator(std::cout, ", ")); + move_group_interface.setStartState(*move_group_interface.getCurrentState()); move_group_interface.setPoseTarget(goal->target_pose); move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity); diff --git a/sdf_models/models/cube/model.sdf b/sdf_models/models/cube/model.sdf index b37a8d9..ed877c1 100644 --- a/sdf_models/models/cube/model.sdf +++ b/sdf_models/models/cube/model.sdf @@ -1,14 +1,36 @@ + 0 + 0 0 0 0 0 0 + + 0.000000 0.000000 0.000000 0.000000 -0.000000 -0.000000 + 0.005709 + + 0.000001 + -0.000000 + -0.000000 + 0.000002 + -0.000000 + 0.000001 + + 0.1 0.1 0.1 + + + + 10000 + 10000 + + +