From 407e9f878435b4641b9587ea1f1d61f7b0f6b6b8 Mon Sep 17 00:00:00 2001 From: wangWking <842749351@qq.com> Date: Thu, 7 Apr 2022 17:03:41 +0800 Subject: [PATCH] code integrate --- .../{ => mypalletizer_260}/CMakeLists.txt | 0 .../{ => mypalletizer_260}/LICENSE | 0 .../config/mycobot_with_marker.rviz | 0 .../config/mypal_260.rviz | 0 .../launch/detect_marker.launch | 0 .../launch/detect_marker_with_topic.launch | 0 .../launch/follow.launch | 0 .../launch/simple_gui.launch | 0 .../launch/slider_control.launch | 0 .../launch/teleop_keyboard.launch | 0 .../{ => mypalletizer_260}/launch/test.launch | 0 .../{ => mypalletizer_260}/package.xml | 0 .../scripts/detect_marker.py | 0 .../scripts/follow_and_pump.py | 0 .../scripts/follow_display.py | 0 .../scripts/following_marker.py | 0 .../scripts/listen_real.py | 0 .../scripts/listen_real_of_topic.py | 0 .../scripts/simple_gui.py | 0 .../scripts/slider_control.py | 0 .../scripts/teleop_keyboard.py | 0 .../mypalletizer_260_moveit/CMakeLists.txt | 22 ++ .../mypalletizer_260_moveit/LICENSE | 25 ++ .../config/chomp_planning.yaml | 18 ++ .../config/fake_controllers.yaml | 10 + .../config/firefighter.srdf | 47 ++++ .../config/joint_limits.yaml | 39 +++ .../config/kinematics.yaml | 5 + .../config/ompl_planning.yaml | 150 +++++++++++ .../config/ros_controllers.yaml | 27 ++ .../config/sensors_3d.yaml | 10 + .../launch/chomp_planning_pipeline.launch.xml | 10 + .../launch/default_warehouse_db.launch | 15 ++ .../launch/demo.launch | 57 ++++ .../launch/demo_gazebo.launch | 70 +++++ .../fake_moveit_controller_manager.launch.xml | 9 + ...ghter_moveit_controller_manager.launch.xml | 10 + ...refighter_moveit_sensor_manager.launch.xml | 3 + .../launch/gazebo.launch | 23 ++ .../launch/joystick_control.launch | 17 ++ .../launch/move_group.launch | 77 ++++++ .../launch/moveit.rviz | 244 ++++++++++++++++++ .../launch/moveit_rviz.launch | 16 ++ .../launch/mypal_moveit.launch | 57 ++++ .../launch/ompl_planning_pipeline.launch.xml | 22 ++ .../launch/planning_context.launch | 24 ++ .../launch/planning_pipeline.launch.xml | 10 + .../launch/ros_controllers.launch | 11 + .../launch/run_benchmark_ompl.launch | 22 ++ .../launch/sensor_manager.launch.xml | 17 ++ .../launch/setup_assistant.launch | 15 ++ .../launch/trajectory_execution.launch.xml | 20 ++ .../launch/warehouse.launch | 15 ++ .../launch/warehouse_settings.launch.xml | 16 ++ .../mypalletizer_260_moveit/package.xml | 40 +++ ...th_planning_and_obstacle_avoidance_demo.py | 132 ++++++++++ .../scripts/sync_plan.py | 41 +++ .../mypalletizer_communication/CMakeLists.txt | 216 ++++++++++++++++ .../mypalletizer_communication/LICENSE | 25 ++ .../launch/communication_jsnn.launch | 10 + .../launch/communication_seeed.launch | 10 + .../launch/communication_service.launch | 10 + .../launch/communication_topic.launch | 12 + .../launch/communication_topic_pi.launch | 10 + .../msg/MypalAngles.msg | 7 + .../msg/MypalCoords.msg | 6 + .../msg/MypalGripperStatus.msg | 1 + .../msg/MypalPumpStatus.msg | 3 + .../msg/MypalSetAngles.msg | 9 + .../msg/MypalSetCoords.msg | 9 + .../mypalletizer_communication/package.xml | 86 ++++++ .../scripts/mypal_services.py | 149 +++++++++++ .../scripts/mypal_topics.py | 216 ++++++++++++++++ .../scripts/mypal_topics_seeed.py | 212 +++++++++++++++ .../scripts/test.py | 7 + .../srv/GetAngles.srv | 11 + .../srv/GetCoords.srv | 9 + .../srv/GripperStatus.srv | 5 + .../srv/PumpStatus.srv | 7 + .../srv/SetAngles.srv | 15 ++ .../srv/SetCoords.srv | 13 + 81 files changed, 2404 insertions(+) rename mypalletizer_260/{ => mypalletizer_260}/CMakeLists.txt (100%) mode change 100755 => 100644 rename mypalletizer_260/{ => mypalletizer_260}/LICENSE (100%) rename mypalletizer_260/{ => mypalletizer_260}/config/mycobot_with_marker.rviz (100%) rename mypalletizer_260/{ => mypalletizer_260}/config/mypal_260.rviz (100%) rename mypalletizer_260/{ => mypalletizer_260}/launch/detect_marker.launch (100%) rename mypalletizer_260/{ => mypalletizer_260}/launch/detect_marker_with_topic.launch (100%) rename mypalletizer_260/{ => mypalletizer_260}/launch/follow.launch (100%) rename mypalletizer_260/{ => mypalletizer_260}/launch/simple_gui.launch (100%) rename mypalletizer_260/{ => mypalletizer_260}/launch/slider_control.launch (100%) rename mypalletizer_260/{ => mypalletizer_260}/launch/teleop_keyboard.launch (100%) rename mypalletizer_260/{ => mypalletizer_260}/launch/test.launch (100%) rename mypalletizer_260/{ => mypalletizer_260}/package.xml (100%) rename mypalletizer_260/{ => mypalletizer_260}/scripts/detect_marker.py (100%) mode change 100755 => 100644 rename mypalletizer_260/{ => mypalletizer_260}/scripts/follow_and_pump.py (100%) mode change 100755 => 100644 rename mypalletizer_260/{ => mypalletizer_260}/scripts/follow_display.py (100%) mode change 100755 => 100644 rename mypalletizer_260/{ => mypalletizer_260}/scripts/following_marker.py (100%) mode change 100755 => 100644 rename mypalletizer_260/{ => mypalletizer_260}/scripts/listen_real.py (100%) mode change 100755 => 100644 rename mypalletizer_260/{ => mypalletizer_260}/scripts/listen_real_of_topic.py (100%) mode change 100755 => 100644 rename mypalletizer_260/{ => mypalletizer_260}/scripts/simple_gui.py (100%) mode change 100755 => 100644 rename mypalletizer_260/{ => mypalletizer_260}/scripts/slider_control.py (100%) mode change 100755 => 100644 rename mypalletizer_260/{ => mypalletizer_260}/scripts/teleop_keyboard.py (100%) mode change 100755 => 100644 create mode 100644 mypalletizer_260/mypalletizer_260_moveit/CMakeLists.txt create mode 100644 mypalletizer_260/mypalletizer_260_moveit/LICENSE create mode 100644 mypalletizer_260/mypalletizer_260_moveit/config/chomp_planning.yaml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/config/fake_controllers.yaml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/config/firefighter.srdf create mode 100644 mypalletizer_260/mypalletizer_260_moveit/config/joint_limits.yaml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/config/kinematics.yaml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/config/ompl_planning.yaml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/config/ros_controllers.yaml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/config/sensors_3d.yaml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/chomp_planning_pipeline.launch.xml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/default_warehouse_db.launch create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/demo.launch create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/demo_gazebo.launch create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/fake_moveit_controller_manager.launch.xml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/firefighter_moveit_controller_manager.launch.xml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/firefighter_moveit_sensor_manager.launch.xml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/gazebo.launch create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/joystick_control.launch create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/move_group.launch create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/moveit.rviz create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/moveit_rviz.launch create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/mypal_moveit.launch create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/ompl_planning_pipeline.launch.xml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/planning_context.launch create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/planning_pipeline.launch.xml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/ros_controllers.launch create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/run_benchmark_ompl.launch create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/sensor_manager.launch.xml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/setup_assistant.launch create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/trajectory_execution.launch.xml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/warehouse.launch create mode 100644 mypalletizer_260/mypalletizer_260_moveit/launch/warehouse_settings.launch.xml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/package.xml create mode 100644 mypalletizer_260/mypalletizer_260_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py create mode 100644 mypalletizer_260/mypalletizer_260_moveit/scripts/sync_plan.py create mode 100644 mypalletizer_260/mypalletizer_communication/CMakeLists.txt create mode 100644 mypalletizer_260/mypalletizer_communication/LICENSE create mode 100644 mypalletizer_260/mypalletizer_communication/launch/communication_jsnn.launch create mode 100644 mypalletizer_260/mypalletizer_communication/launch/communication_seeed.launch create mode 100644 mypalletizer_260/mypalletizer_communication/launch/communication_service.launch create mode 100644 mypalletizer_260/mypalletizer_communication/launch/communication_topic.launch create mode 100644 mypalletizer_260/mypalletizer_communication/launch/communication_topic_pi.launch create mode 100644 mypalletizer_260/mypalletizer_communication/msg/MypalAngles.msg create mode 100644 mypalletizer_260/mypalletizer_communication/msg/MypalCoords.msg create mode 100644 mypalletizer_260/mypalletizer_communication/msg/MypalGripperStatus.msg create mode 100644 mypalletizer_260/mypalletizer_communication/msg/MypalPumpStatus.msg create mode 100644 mypalletizer_260/mypalletizer_communication/msg/MypalSetAngles.msg create mode 100644 mypalletizer_260/mypalletizer_communication/msg/MypalSetCoords.msg create mode 100644 mypalletizer_260/mypalletizer_communication/package.xml create mode 100644 mypalletizer_260/mypalletizer_communication/scripts/mypal_services.py create mode 100644 mypalletizer_260/mypalletizer_communication/scripts/mypal_topics.py create mode 100644 mypalletizer_260/mypalletizer_communication/scripts/mypal_topics_seeed.py create mode 100644 mypalletizer_260/mypalletizer_communication/scripts/test.py create mode 100644 mypalletizer_260/mypalletizer_communication/srv/GetAngles.srv create mode 100644 mypalletizer_260/mypalletizer_communication/srv/GetCoords.srv create mode 100644 mypalletizer_260/mypalletizer_communication/srv/GripperStatus.srv create mode 100644 mypalletizer_260/mypalletizer_communication/srv/PumpStatus.srv create mode 100644 mypalletizer_260/mypalletizer_communication/srv/SetAngles.srv create mode 100644 mypalletizer_260/mypalletizer_communication/srv/SetCoords.srv diff --git a/mypalletizer_260/CMakeLists.txt b/mypalletizer_260/mypalletizer_260/CMakeLists.txt old mode 100755 new mode 100644 similarity index 100% rename from mypalletizer_260/CMakeLists.txt rename to mypalletizer_260/mypalletizer_260/CMakeLists.txt diff --git a/mypalletizer_260/LICENSE b/mypalletizer_260/mypalletizer_260/LICENSE similarity index 100% rename from mypalletizer_260/LICENSE rename to mypalletizer_260/mypalletizer_260/LICENSE diff --git a/mypalletizer_260/config/mycobot_with_marker.rviz b/mypalletizer_260/mypalletizer_260/config/mycobot_with_marker.rviz similarity index 100% rename from mypalletizer_260/config/mycobot_with_marker.rviz rename to mypalletizer_260/mypalletizer_260/config/mycobot_with_marker.rviz diff --git a/mypalletizer_260/config/mypal_260.rviz b/mypalletizer_260/mypalletizer_260/config/mypal_260.rviz similarity index 100% rename from mypalletizer_260/config/mypal_260.rviz rename to mypalletizer_260/mypalletizer_260/config/mypal_260.rviz diff --git a/mypalletizer_260/launch/detect_marker.launch b/mypalletizer_260/mypalletizer_260/launch/detect_marker.launch similarity index 100% rename from mypalletizer_260/launch/detect_marker.launch rename to mypalletizer_260/mypalletizer_260/launch/detect_marker.launch diff --git a/mypalletizer_260/launch/detect_marker_with_topic.launch b/mypalletizer_260/mypalletizer_260/launch/detect_marker_with_topic.launch similarity index 100% rename from mypalletizer_260/launch/detect_marker_with_topic.launch rename to mypalletizer_260/mypalletizer_260/launch/detect_marker_with_topic.launch diff --git a/mypalletizer_260/launch/follow.launch b/mypalletizer_260/mypalletizer_260/launch/follow.launch similarity index 100% rename from mypalletizer_260/launch/follow.launch rename to mypalletizer_260/mypalletizer_260/launch/follow.launch diff --git a/mypalletizer_260/launch/simple_gui.launch b/mypalletizer_260/mypalletizer_260/launch/simple_gui.launch similarity index 100% rename from mypalletizer_260/launch/simple_gui.launch rename to mypalletizer_260/mypalletizer_260/launch/simple_gui.launch diff --git a/mypalletizer_260/launch/slider_control.launch b/mypalletizer_260/mypalletizer_260/launch/slider_control.launch similarity index 100% rename from mypalletizer_260/launch/slider_control.launch rename to mypalletizer_260/mypalletizer_260/launch/slider_control.launch diff --git a/mypalletizer_260/launch/teleop_keyboard.launch b/mypalletizer_260/mypalletizer_260/launch/teleop_keyboard.launch similarity index 100% rename from mypalletizer_260/launch/teleop_keyboard.launch rename to mypalletizer_260/mypalletizer_260/launch/teleop_keyboard.launch diff --git a/mypalletizer_260/launch/test.launch b/mypalletizer_260/mypalletizer_260/launch/test.launch similarity index 100% rename from mypalletizer_260/launch/test.launch rename to mypalletizer_260/mypalletizer_260/launch/test.launch diff --git a/mypalletizer_260/package.xml b/mypalletizer_260/mypalletizer_260/package.xml similarity index 100% rename from mypalletizer_260/package.xml rename to mypalletizer_260/mypalletizer_260/package.xml diff --git a/mypalletizer_260/scripts/detect_marker.py b/mypalletizer_260/mypalletizer_260/scripts/detect_marker.py old mode 100755 new mode 100644 similarity index 100% rename from mypalletizer_260/scripts/detect_marker.py rename to mypalletizer_260/mypalletizer_260/scripts/detect_marker.py diff --git a/mypalletizer_260/scripts/follow_and_pump.py b/mypalletizer_260/mypalletizer_260/scripts/follow_and_pump.py old mode 100755 new mode 100644 similarity index 100% rename from mypalletizer_260/scripts/follow_and_pump.py rename to mypalletizer_260/mypalletizer_260/scripts/follow_and_pump.py diff --git a/mypalletizer_260/scripts/follow_display.py b/mypalletizer_260/mypalletizer_260/scripts/follow_display.py old mode 100755 new mode 100644 similarity index 100% rename from mypalletizer_260/scripts/follow_display.py rename to mypalletizer_260/mypalletizer_260/scripts/follow_display.py diff --git a/mypalletizer_260/scripts/following_marker.py b/mypalletizer_260/mypalletizer_260/scripts/following_marker.py old mode 100755 new mode 100644 similarity index 100% rename from mypalletizer_260/scripts/following_marker.py rename to mypalletizer_260/mypalletizer_260/scripts/following_marker.py diff --git a/mypalletizer_260/scripts/listen_real.py b/mypalletizer_260/mypalletizer_260/scripts/listen_real.py old mode 100755 new mode 100644 similarity index 100% rename from mypalletizer_260/scripts/listen_real.py rename to mypalletizer_260/mypalletizer_260/scripts/listen_real.py diff --git a/mypalletizer_260/scripts/listen_real_of_topic.py b/mypalletizer_260/mypalletizer_260/scripts/listen_real_of_topic.py old mode 100755 new mode 100644 similarity index 100% rename from mypalletizer_260/scripts/listen_real_of_topic.py rename to mypalletizer_260/mypalletizer_260/scripts/listen_real_of_topic.py diff --git a/mypalletizer_260/scripts/simple_gui.py b/mypalletizer_260/mypalletizer_260/scripts/simple_gui.py old mode 100755 new mode 100644 similarity index 100% rename from mypalletizer_260/scripts/simple_gui.py rename to mypalletizer_260/mypalletizer_260/scripts/simple_gui.py diff --git a/mypalletizer_260/scripts/slider_control.py b/mypalletizer_260/mypalletizer_260/scripts/slider_control.py old mode 100755 new mode 100644 similarity index 100% rename from mypalletizer_260/scripts/slider_control.py rename to mypalletizer_260/mypalletizer_260/scripts/slider_control.py diff --git a/mypalletizer_260/scripts/teleop_keyboard.py b/mypalletizer_260/mypalletizer_260/scripts/teleop_keyboard.py old mode 100755 new mode 100644 similarity index 100% rename from mypalletizer_260/scripts/teleop_keyboard.py rename to mypalletizer_260/mypalletizer_260/scripts/teleop_keyboard.py diff --git a/mypalletizer_260/mypalletizer_260_moveit/CMakeLists.txt b/mypalletizer_260/mypalletizer_260_moveit/CMakeLists.txt new file mode 100644 index 0000000..67ac15b --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mypalletizer_260_moveit) + +find_package(catkin REQUIRED + rospy + std_msgs + moveit_msgs +) + +catkin_package() + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +catkin_install_python(PROGRAMS + scripts/sync_plan.py + scripts/path_planning_and_obstacle_avoidance_demo.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/mypalletizer_260/mypalletizer_260_moveit/LICENSE b/mypalletizer_260/mypalletizer_260_moveit/LICENSE new file mode 100644 index 0000000..b8468e6 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/LICENSE @@ -0,0 +1,25 @@ +BSD 2-Clause License + +Copyright (c) 2020, Elephant Robotics +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/mypalletizer_260/mypalletizer_260_moveit/config/chomp_planning.yaml b/mypalletizer_260/mypalletizer_260_moveit/config/chomp_planning.yaml new file mode 100644 index 0000000..75258e5 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/mypalletizer_260/mypalletizer_260_moveit/config/fake_controllers.yaml b/mypalletizer_260/mypalletizer_260_moveit/config/fake_controllers.yaml new file mode 100644 index 0000000..bb2abb5 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/config/fake_controllers.yaml @@ -0,0 +1,10 @@ +controller_list: + - name: fake_arm_group_controller + joints: + - joint1_to_base + - joint2_to_joint1 + - joint3_to_joint2 + - joint4_to_joint3 + - joint5_to_joint4 + + # - joint6output_to_joint6 \ No newline at end of file diff --git a/mypalletizer_260/mypalletizer_260_moveit/config/firefighter.srdf b/mypalletizer_260/mypalletizer_260_moveit/config/firefighter.srdf new file mode 100644 index 0000000..9781c25 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/config/firefighter.srdf @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/config/joint_limits.yaml b/mypalletizer_260/mypalletizer_260_moveit/config/joint_limits.yaml new file mode 100644 index 0000000..f51037d --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/config/joint_limits.yaml @@ -0,0 +1,39 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + joint1_to_base: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint2_to_joint1: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint3_to_joint2: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint4_to_joint3: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint5_to_joint4: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + # joint6_to_joint5: + # has_velocity_limits: false + # max_velocity: 0 + # has_acceleration_limits: false + # max_acceleration: 0 + # joint6output_to_joint6: + # has_velocity_limits: false + # max_velocity: 0 + # has_acceleration_limits: false + # max_acceleration: 0 \ No newline at end of file diff --git a/mypalletizer_260/mypalletizer_260_moveit/config/kinematics.yaml b/mypalletizer_260/mypalletizer_260_moveit/config/kinematics.yaml new file mode 100644 index 0000000..031b273 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/config/kinematics.yaml @@ -0,0 +1,5 @@ +arm_group: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 \ No newline at end of file diff --git a/mypalletizer_260/mypalletizer_260_moveit/config/ompl_planning.yaml b/mypalletizer_260/mypalletizer_260_moveit/config/ompl_planning.yaml new file mode 100644 index 0000000..3f4d0c2 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/config/ompl_planning.yaml @@ -0,0 +1,150 @@ +planner_configs: + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 +arm_group: + default_planner_config: None + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(joint2_to_joint1,joint3_to_joint2) + longest_valid_segment_fraction: 0.005 \ No newline at end of file diff --git a/mypalletizer_260/mypalletizer_260_moveit/config/ros_controllers.yaml b/mypalletizer_260/mypalletizer_260_moveit/config/ros_controllers.yaml new file mode 100644 index 0000000..86c9156 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/config/ros_controllers.yaml @@ -0,0 +1,27 @@ +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: arm_group + joint_model_group_pose: init_pose +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: + joints: + - vitual_joint + - joint1_to_base + - joint2_to_joint1 + - joint3_to_joint2 + - joint4_to_joint3 + - joint5_to_joint4 + # - joint6_to_joint5 + # - joint6output_to_joint6 + sim_control_mode: 1 # 0: position, 1: velocity +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: + [] \ No newline at end of file diff --git a/mypalletizer_260/mypalletizer_260_moveit/config/sensors_3d.yaml b/mypalletizer_260/mypalletizer_260_moveit/config/sensors_3d.yaml new file mode 100644 index 0000000..a4bb13e --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/config/sensors_3d.yaml @@ -0,0 +1,10 @@ +# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it +sensors: + - filtered_cloud_topic: filtered_cloud + max_range: 5.0 + max_update_rate: 1.0 + padding_offset: 0.1 + padding_scale: 1.0 + point_cloud_topic: /head_mount_kinect/depth_registered/points + point_subsample: 1 + sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater \ No newline at end of file diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/chomp_planning_pipeline.launch.xml b/mypalletizer_260/mypalletizer_260_moveit/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..29cc7ab --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/default_warehouse_db.launch b/mypalletizer_260/mypalletizer_260_moveit/launch/default_warehouse_db.launch new file mode 100644 index 0000000..83d8e9d --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/demo.launch b/mypalletizer_260/mypalletizer_260_moveit/launch/demo.launch new file mode 100644 index 0000000..ef13292 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/demo.launch @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/demo_gazebo.launch b/mypalletizer_260/mypalletizer_260_moveit/launch/demo_gazebo.launch new file mode 100644 index 0000000..925bfa1 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/demo_gazebo.launch @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [/joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/fake_moveit_controller_manager.launch.xml b/mypalletizer_260/mypalletizer_260_moveit/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..9b5c686 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/firefighter_moveit_controller_manager.launch.xml b/mypalletizer_260/mypalletizer_260_moveit/launch/firefighter_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..7aad413 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/firefighter_moveit_controller_manager.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/firefighter_moveit_sensor_manager.launch.xml b/mypalletizer_260/mypalletizer_260_moveit/launch/firefighter_moveit_sensor_manager.launch.xml new file mode 100644 index 0000000..5d02698 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/firefighter_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/gazebo.launch b/mypalletizer_260/mypalletizer_260_moveit/launch/gazebo.launch new file mode 100644 index 0000000..a34b0ad --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/gazebo.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/joystick_control.launch b/mypalletizer_260/mypalletizer_260_moveit/launch/joystick_control.launch new file mode 100644 index 0000000..9411f6e --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/move_group.launch b/mypalletizer_260/mypalletizer_260_moveit/launch/move_group.launch new file mode 100644 index 0000000..1638533 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/move_group.launch @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/moveit.rviz b/mypalletizer_260/mypalletizer_260_moveit/launch/moveit.rviz new file mode 100644 index 0000000..fea68f1 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/moveit.rviz @@ -0,0 +1,244 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - /MotionPlanning1/Scene Geometry1 + - /MotionPlanning1/Scene Robot1 + - /MotionPlanning1/Planning Request1 + - /MotionPlanning1/Planned Path1 + Splitter Ratio: 0.742560029 + Tree Height: 325 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + + + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.0799999982 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: mycobot_arm + Query Goal State: true + Query Start State: true + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.200000003 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + + + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: false + Value: true + Velocity_Scaling_Factor: 1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: base + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 1.39158678 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.113567002 + Y: 0.105920002 + Z: 2.23518001e-07 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.439796686 + Target Frame: link1 + Value: XYOrbit (rviz) + Yaw: 2.5317657 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1028 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002ad000003befc0200000007fb000000100044006900730070006c0061007900730100000028000001da000000d700fffffffb0000000800480065006c00700000000342000000bb0000007300fffffffb0000000a0056006900650077007300000003b0000000b0000000ad00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007201000002080000004a0000004a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000002580000018e0000018300ffffff00000471000003be00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1828 + X: 92 + Y: 24 diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/moveit_rviz.launch b/mypalletizer_260/mypalletizer_260_moveit/launch/moveit_rviz.launch new file mode 100644 index 0000000..effca9d --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/moveit_rviz.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/mypal_moveit.launch b/mypalletizer_260/mypalletizer_260_moveit/launch/mypal_moveit.launch new file mode 100644 index 0000000..ef13292 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/mypal_moveit.launch @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/ompl_planning_pipeline.launch.xml b/mypalletizer_260/mypalletizer_260_moveit/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 0000000..635cb70 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/planning_context.launch b/mypalletizer_260/mypalletizer_260_moveit/launch/planning_context.launch new file mode 100644 index 0000000..2598e0c --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/planning_context.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/planning_pipeline.launch.xml b/mypalletizer_260/mypalletizer_260_moveit/launch/planning_pipeline.launch.xml new file mode 100644 index 0000000..3d1875c --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/ros_controllers.launch b/mypalletizer_260/mypalletizer_260_moveit/launch/ros_controllers.launch new file mode 100644 index 0000000..3bb4041 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/run_benchmark_ompl.launch b/mypalletizer_260/mypalletizer_260_moveit/launch/run_benchmark_ompl.launch new file mode 100644 index 0000000..799e778 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/run_benchmark_ompl.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/sensor_manager.launch.xml b/mypalletizer_260/mypalletizer_260_moveit/launch/sensor_manager.launch.xml new file mode 100644 index 0000000..922d5a6 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/setup_assistant.launch b/mypalletizer_260/mypalletizer_260_moveit/launch/setup_assistant.launch new file mode 100644 index 0000000..8191e48 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/setup_assistant.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/trajectory_execution.launch.xml b/mypalletizer_260/mypalletizer_260_moveit/launch/trajectory_execution.launch.xml new file mode 100644 index 0000000..fca6d89 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/trajectory_execution.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/warehouse.launch b/mypalletizer_260/mypalletizer_260_moveit/launch/warehouse.launch new file mode 100644 index 0000000..e3ce98a --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/launch/warehouse_settings.launch.xml b/mypalletizer_260/mypalletizer_260_moveit/launch/warehouse_settings.launch.xml new file mode 100644 index 0000000..e473b08 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/package.xml b/mypalletizer_260/mypalletizer_260_moveit/package.xml new file mode 100644 index 0000000..1564b22 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/package.xml @@ -0,0 +1,40 @@ + + + mypalletizer_260_moveit + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the firefighter with the MoveIt! Motion Planning Framework + + zachary + zachary + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + moveit_ros_move_group + moveit_fake_controller_manager + moveit_kinematics + moveit_planners_ompl + moveit_ros_visualization + moveit_setup_assistant + joint_state_publisher + robot_state_publisher + xacro + + + roscpp + rospy + std_msgs + actionlib + moveit_msgs + mycobot_description + + mycobot_description + + + diff --git a/mypalletizer_260/mypalletizer_260_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/mypalletizer_260/mypalletizer_260_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py new file mode 100644 index 0000000..3584da0 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import rospy, roslib, sys +import moveit_commander +from moveit_msgs.msg import RobotTrajectory +from trajectory_msgs.msg import JointTrajectoryPoint + +from geometry_msgs.msg import PoseStamped, Pose +from tf.transformations import euler_from_quaternion, quaternion_from_euler + + +class MoveItPlanningDemo: + def __init__(self): + # API to initialize move_group. 初始化move_group的API + moveit_commander.roscpp_initialize(sys.argv) + + # Initialize the ROS node,初始化ROS节点 + rospy.init_node("moveit_ik_demo") + + # Initialize the scene object to listen for changes in the external environment + # 初始化场景对象，用来监听外部环境的变化 + self.scene = moveit_commander.PlanningSceneInterface() + rospy.sleep(1) + + # Initialize self.arm group in the robotic arm that needs to be controlled by move group + # 初始化需要使用move group控制的机械臂中的self.arm group + self.arm = moveit_commander.MoveGroupCommander("arm_group") + + # Get the name of the terminal link. 获取终端link的名称 + self.end_effector_link = self.arm.get_end_effector_link() + + # Set the reference coordinate system used for the target position. + # 设置目标位置所使用的参考坐标系 + self.reference_frame = "joint1" + self.arm.set_pose_reference_frame(self.reference_frame) + + # Allow replanning when motion planning fails. + # 当运动规划失败后，允许重新规划 + self.arm.allow_replanning(True) + + # Set the allowable error of position (unit: meter) and attitude (unit: radian). + # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 + self.arm.set_goal_position_tolerance(0.01) + self.arm.set_goal_orientation_tolerance(0.05) + + def moving(self): + # Control the robotic arm to return to the initialization position first. 控制机械臂先回到初始化位置 + self.arm.set_named_target("init_pose") + self.arm.go() + rospy.sleep(2) + + # Set the target pose in the robotic arm workspace, the position is described by x, y, z coordinates, + # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述， + # Pose is described by quaternion, based on base_link coordinate system + # 姿态使用四元数描述，基于base_link坐标系 + target_pose = PoseStamped() + target_pose.header.frame_id = self.reference_frame + target_pose.header.stamp = rospy.Time.now() + target_pose.pose.position.x = 0.132 + target_pose.pose.position.y = -0.150 + target_pose.pose.position.z = 0.075 + target_pose.pose.orientation.x = 0.026 + target_pose.pose.orientation.y = 1.0 + target_pose.pose.orientation.z = 0.0 + target_pose.pose.orientation.w = 0.014 + + # Set the current state of the robot arm as the initial state of motion. 设置机器臂当前的状态作为运动初始状态 + self.arm.set_start_state_to_current_state() + + # Set the target pose of the terminal motion of the robotic arm. 设置机械臂终端运动的目标位姿 + self.arm.set_pose_target(target_pose, self.end_effector_link) + + traj = self.arm.plan() + + # Control the motion of the robotic arm according to the planned motion path. 按照规划的运动路径控制机械臂运动 + self.arm.execute(traj) + rospy.sleep(1) + + # Control the terminal of the robotic arm to move 5cm to the right. Parameter 1 represents y, 0,1,2,3,4,5 represents xyzrpy + # 控制机械臂终端向右移动5cm 參数1是代表y， 0,1,2,3,4,5 代表xyzrpy + self.arm.shift_pose_target(1, 0.12, self.end_effector_link) + self.arm.go() + rospy.sleep(1) + + self.arm.shift_pose_target(1, 0.1, self.end_effector_link) + self.arm.go() + rospy.sleep(1) + + # Control the end of the robotic arm to rotate 90 degrees in reverse, 0,1,2,3,4,5 represents xyzrpy + # 控制机械臂终端反向旋转90度, 0,1,2,3,4,5 代表xyzrpy + # self.arm.shift_pose_target(3, -1.57, end_effector_link) + # self.arm.go() + # rospy.sleep(1) + + def run(self): + self.scene.remove_world_object("suit") + + self.moving() + + # Add the environment. 添加环境 + quat = quaternion_from_euler(3.1415, 0, -1.57) + + suit_post = PoseStamped() + suit_post.header.frame_id = self.reference_frame + suit_post.pose.position.x = 0.0 + suit_post.pose.position.y = 0.0 + suit_post.pose.position.z = -0.02 + suit_post.pose.orientation.x = quat[0] + suit_post.pose.orientation.y = quat[1] + suit_post.pose.orientation.z = quat[2] + suit_post.pose.orientation.w = quat[3] + + suit_path = ( + roslib.packages.get_pkg_dir("mycobot_description") + + "/urdf/mycobot/suit_env.dae" + ) + # need `pyassimp==3.3` + self.scene.add_mesh("suit", suit_post, suit_path) + rospy.sleep(2) + + # Run again if there is environmental impact. 有环境影响后再运行一次 + self.moving() + + # Close and exit moveit. 关闭并退出moveit + moveit_commander.roscpp_shutdown() + moveit_commander.os._exit(0) + + +if __name__ == "__main__": + o = MoveItPlanningDemo() + o.run() diff --git a/mypalletizer_260/mypalletizer_260_moveit/scripts/sync_plan.py b/mypalletizer_260/mypalletizer_260_moveit/scripts/sync_plan.py new file mode 100644 index 0000000..75d59d4 --- /dev/null +++ b/mypalletizer_260/mypalletizer_260_moveit/scripts/sync_plan.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python2 +import time +import rospy +from sensor_msgs.msg import JointState + +# from pymycobot.mycobot import MyCobot +from pymycobot.mypalletizer import MyPalletizer + + +mc = None + + +def callback(data): + rospy.loginfo(rospy.get_caller_id() + "%s", data) + data_list = [] + for index, value in enumerate(data.position): + # if index != 2: + # value *= -1 + data_list.append(value) + # print("data_list:",data_list) + mc.send_radians(data_list, 80) + + +def listener(): + global mc + rospy.init_node("mypal_reciver", anonymous=True) + + port = rospy.get_param("~port", "/dev/ttyUSB0") + baud = rospy.get_param("~baud", 115200) + print(port, baud) + mc = MyPalletizer(port, baud) + + rospy.Subscriber("joint_states", JointState, callback) + + # spin() simply keeps python from exiting until this node is stopped + # spin()只是阻止python退出，直到该节点停止 + rospy.spin() + + +if __name__ == "__main__": + listener() diff --git a/mypalletizer_260/mypalletizer_communication/CMakeLists.txt b/mypalletizer_260/mypalletizer_communication/CMakeLists.txt new file mode 100644 index 0000000..c501f28 --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/CMakeLists.txt @@ -0,0 +1,216 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mypalletizer_communication) +add_compile_options(-std=c++11) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + genmsg + message_generation +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +add_message_files(FILES + MypalAngles.msg + MypalCoords.msg + MypalSetAngles.msg + MypalSetCoords.msg + MypalGripperStatus.msg + MypalPumpStatus.msg +) + +## Generate services in the 'srv' folder +add_service_files(FILES + GetAngles.srv + SetAngles.srv + GetCoords.srv + SetCoords.srv + GripperStatus.srv + PumpStatus.srv +) + +## Generate added messages and services +generate_messages(DEPENDENCIES std_msgs) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +## Declare a catkin package +catkin_package( + CATKIN_DEPENDS message_runtime std_msgs +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include +# ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/Mypal_commuication.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/Mypal_commuication_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +catkin_install_python(PROGRAMS + scripts/mypal_services.py + scripts/mypal_topics.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_Mypal_commuication.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/mypalletizer_260/mypalletizer_communication/LICENSE b/mypalletizer_260/mypalletizer_communication/LICENSE new file mode 100644 index 0000000..b8468e6 --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/LICENSE @@ -0,0 +1,25 @@ +BSD 2-Clause License + +Copyright (c) 2020, Elephant Robotics +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/mypalletizer_260/mypalletizer_communication/launch/communication_jsnn.launch b/mypalletizer_260/mypalletizer_communication/launch/communication_jsnn.launch new file mode 100644 index 0000000..5668bab --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/launch/communication_jsnn.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_communication/launch/communication_seeed.launch b/mypalletizer_260/mypalletizer_communication/launch/communication_seeed.launch new file mode 100644 index 0000000..f40df63 --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/launch/communication_seeed.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_communication/launch/communication_service.launch b/mypalletizer_260/mypalletizer_communication/launch/communication_service.launch new file mode 100644 index 0000000..92f77b5 --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/launch/communication_service.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_communication/launch/communication_topic.launch b/mypalletizer_260/mypalletizer_communication/launch/communication_topic.launch new file mode 100644 index 0000000..4ea191b --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/launch/communication_topic.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_communication/launch/communication_topic_pi.launch b/mypalletizer_260/mypalletizer_communication/launch/communication_topic_pi.launch new file mode 100644 index 0000000..c436d28 --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/launch/communication_topic_pi.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_communication/msg/MypalAngles.msg b/mypalletizer_260/mypalletizer_communication/msg/MypalAngles.msg new file mode 100644 index 0000000..bba2181 --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/msg/MypalAngles.msg @@ -0,0 +1,7 @@ +# float32 base +float32 joint_1 +float32 joint_2 +float32 joint_3 +float32 joint_4 +# float32 joint_5 +# float32 joint_6 diff --git a/mypalletizer_260/mypalletizer_communication/msg/MypalCoords.msg b/mypalletizer_260/mypalletizer_communication/msg/MypalCoords.msg new file mode 100644 index 0000000..f2e98b9 --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/msg/MypalCoords.msg @@ -0,0 +1,6 @@ +float32 x +float32 y +float32 z +float32 rx +# float32 ry +# float32 rz \ No newline at end of file diff --git a/mypalletizer_260/mypalletizer_communication/msg/MypalGripperStatus.msg b/mypalletizer_260/mypalletizer_communication/msg/MypalGripperStatus.msg new file mode 100644 index 0000000..18d6277 --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/msg/MypalGripperStatus.msg @@ -0,0 +1 @@ +bool Status \ No newline at end of file diff --git a/mypalletizer_260/mypalletizer_communication/msg/MypalPumpStatus.msg b/mypalletizer_260/mypalletizer_communication/msg/MypalPumpStatus.msg new file mode 100644 index 0000000..2373241 --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/msg/MypalPumpStatus.msg @@ -0,0 +1,3 @@ +bool Status +int8 Pin1 +int8 Pin2 diff --git a/mypalletizer_260/mypalletizer_communication/msg/MypalSetAngles.msg b/mypalletizer_260/mypalletizer_communication/msg/MypalSetAngles.msg new file mode 100644 index 0000000..3580a2e --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/msg/MypalSetAngles.msg @@ -0,0 +1,9 @@ +# float32 base +float32 joint_1 +float32 joint_2 +float32 joint_3 +float32 joint_4 +# float32 joint_5 +# float32 joint_6 + +int8 speed diff --git a/mypalletizer_260/mypalletizer_communication/msg/MypalSetCoords.msg b/mypalletizer_260/mypalletizer_communication/msg/MypalSetCoords.msg new file mode 100644 index 0000000..15c601f --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/msg/MypalSetCoords.msg @@ -0,0 +1,9 @@ +float32 x +float32 y +float32 z +float32 rx +# float32 ry +# float32 rz + +int8 speed +int8 model diff --git a/mypalletizer_260/mypalletizer_communication/package.xml b/mypalletizer_260/mypalletizer_communication/package.xml new file mode 100644 index 0000000..a7c96fc --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/package.xml @@ -0,0 +1,86 @@ + + + mypalletizer_communication + 0.1.0 + The mypalletizer_commuication package + + + + + zachary + + + + + + BSD + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + rospy + std_msgs + message_generation + roscpp + rospy + actionlib + mycobot_description + + + + mycobot_description + + roscpp + rospy + std_msgs + actionlib + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + xacro + joy + rviz + controller_manager + python-tk + mycobot_description + message_runtime + + + + + + + + diff --git a/mypalletizer_260/mypalletizer_communication/scripts/mypal_services.py b/mypalletizer_260/mypalletizer_communication/scripts/mypal_services.py new file mode 100644 index 0000000..4ea468d --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/scripts/mypal_services.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python2 +import time +import rospy +# from mycobot_communication.srv import * +from mypalletizer_communication.srv import * + +# from pymycobot.mycobot import MyCobot +from pymycobot.mypalletizer import MyPalletizer + +mc = None + + +def create_handle(): + global mc + rospy.init_node("mypal_services") + rospy.loginfo("start ...") + port = rospy.get_param("~port") + baud = rospy.get_param("~baud") + rospy.loginfo("%s,%s" % (port, baud)) + mc = MyPalletizer(port, baud) + + +def create_services(): + rospy.Service("set_joint_angles", SetAngles, set_angles) + rospy.Service("get_joint_angles", GetAngles, get_angles) + rospy.Service("set_joint_coords", SetCoords, set_coords) + rospy.Service("get_joint_coords", GetCoords, get_coords) + rospy.Service("switch_gripper_status", GripperStatus, switch_status) + rospy.Service("switch_pump_status", PumpStatus, toggle_pump) + rospy.loginfo("ready") + rospy.spin() + + +def set_angles(req): + angles = [ + req.joint_1, + req.joint_2, + req.joint_3, + req.joint_4, + # req.joint_5, + # req.joint_6, + ] + sp = req.speed + + if mc: + mc.send_angles(angles, sp) + + return SetAnglesResponse(True) + + +def get_angles(req): + if mc: + angles = mc.get_angles() + return GetAnglesResponse(*angles) + + +def set_coords(req): + coords = [ + req.x, + req.y, + req.z, + req.rx, + # req.ry, + # req.rz, + ] + sp = req.speed + mod = req.model + + if mc: + mc.send_coords(coords, sp, mod) + + return SetCoordsResponse(True) + + +def get_coords(req): + if mc: + coords = mc.get_coords() + return GetCoordsResponse(*coords) + + +def switch_status(req): + """Gripper switch,夹爪开关""" + if mc: + if req.Status: + mc.set_gripper_state(0, 80) + else: + mc.set_gripper_state(1, 80) + + return GripperStatusResponse(True) + + +def toggle_pump(req): + if mc: + if req.Status: + mc.set_basic_output(req.Pin1, 0) + mc.set_basic_output(req.Pin2, 0) + else: + mc.set_basic_output(req.Pin1, 1) + mc.set_basic_output(req.Pin2, 1) + + return PumpStatusResponse(True) + + +robot_msg = """ +Mypalletizer Status +-------------------------------- +Joint Limit: + joint 1: -160 ~ +160 + joint 2: -0.87 ~ +100.01 + joint 3: -17.13 ~ +60 + joint 4: simple show + joint 5: -170 ~ +170 + +Connect Status: %s + +Servo Infomation: %s + +Servo Temperature: %s + +Atom Version: %s +""" + + +def output_robot_message(): + connect_status = False + servo_infomation = "unknown" + servo_temperature = "unknown" + atom_version = "unknown" + + if mc: + cn = mc.is_controller_connected() + if cn == 1: + connect_status = True + time.sleep(0.1) + si = mc.is_all_servo_enable() + if si == 1: + servo_infomation = "all connected" + + print( + robot_msg % (connect_status, servo_infomation, + servo_temperature, atom_version) + ) + + +if __name__ == "__main__": + # print(MyCobot.__dict__) + create_handle() + output_robot_message() + create_services() diff --git a/mypalletizer_260/mypalletizer_communication/scripts/mypal_topics.py b/mypalletizer_260/mypalletizer_communication/scripts/mypal_topics.py new file mode 100644 index 0000000..0ec7dcd --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/scripts/mypal_topics.py @@ -0,0 +1,216 @@ +#!/usr/bin/env python2 +import time +import os +import sys +import signal +import threading + +import rospy +from pymycobot.mypalletizer import MyPalletizer + +from mypalletizer_communication import( + MypalAngles, + MypalCoords, + MypalSetAngles, + MypalSetCoords, + MypalGripperStatus, + MypalPumpStatus, +) + + +class Watcher: + """this class solves two problems with multithreaded + programs in Python, (1) a signal might be delivered + to any thread (which is just a malfeature) and (2) if + the thread that gets the signal is waiting, the signal + is ignored (which is a bug). + + The watcher is a concurrent process (not thread) that + waits for a signal and the process that contains the + threads. See Appendix A of The Little Book of Semaphores. + http://greenteapress.com/semaphores/ + + I have only tested this on Linux. I would expect it to + work on the Macintosh and not work on Windows. + """ + + def __init__(self): + """Creates a child thread, which returns. The parent + thread waits for a KeyboardInterrupt and then kills + the child thread. + """ + self.child = os.fork() + if self.child == 0: + return + else: + self.watch() + + def watch(self): + try: + os.wait() + except KeyboardInterrupt: + # I put the capital B in KeyBoardInterrupt so I can + # tell when the Watcher gets the SIGINT + print("KeyBoardInterrupt") + self.kill() + sys.exit() + + def kill(self): + try: + os.kill(self.child, signal.SIGKILL) + except OSError: + pass + + +class MypalTopics(object): + def __init__(self): + super(MypalTopics, self).__init__() + + rospy.init_node("Mypal_topics") + rospy.loginfo("start ...") + port = rospy.get_param("~port", "/dev/ttyUSB0") + baud = rospy.get_param("~baud", 115200) + rospy.loginfo("%s,%s" % (port, baud)) + self.mc = MyPalletizer(port, baud) + self.lock = threading.Lock() + + def start(self): + pa = threading.Thread(target=self.pub_real_angles) + pb = threading.Thread(target=self.pub_real_coords) + sa = threading.Thread(target=self.sub_set_angles) + sb = threading.Thread(target=self.sub_set_coords) + sg = threading.Thread(target=self.sub_gripper_status) + sp = threading.Thread(target=self.sub_pump_status) + + pa.setDaemon(True) + pa.start() + pb.setDaemon(True) + pb.start() + sa.setDaemon(True) + sa.start() + sb.setDaemon(True) + sb.start() + sg.setDaemon(True) + sg.start() + sp.setDaemon(True) + sp.start() + + pa.join() + pb.join() + sa.join() + sb.join() + sg.join() + sp.join() + + def pub_real_angles(self): + """Publish real angle""" + """发布真实角度""" + pub = rospy.Publisher("Mypal/angles_real", + MypalAngles, queue_size=5) + ma = MypalAngles() + while not rospy.is_shutdown(): + self.lock.acquire() + angles = self.mc.get_angles() + self.lock.release() + if angles: + ma.joint_1 = angles[0] + ma.joint_2 = angles[1] + ma.joint_3 = angles[2] + ma.joint_4 = angles[3] + # ma.joint_5 = angles[4] + # ma.joint_6 = angles[5] + pub.publish(ma) + time.sleep(0.25) + + def pub_real_coords(self): + """publish real coordinates""" + """发布真实坐标""" + pub = rospy.Publisher("Mypal/coords_real", + MypalCoords, queue_size=5) + ma = MypalCoords() + + while not rospy.is_shutdown(): + self.lock.acquire() + coords = self.mc.get_coords() + self.lock.release() + if coords: + ma.x = coords[0] + ma.y = coords[1] + ma.z = coords[2] + ma.rx = coords[3] + # ma.ry = coords[4] + # ma.rz = coords[5] + pub.publish(ma) + time.sleep(0.25) + + def sub_set_angles(self): + """subscription angles""" + """订阅角度""" + def callback(data): + angles = [ + data.joint_1, + data.joint_2, + data.joint_3, + data.joint_4, + # data.joint_5, + # data.joint_6, + ] + sp = int(data.speed) + self.mc.send_angles(angles, sp) + + sub = rospy.Subscriber( + "Mypal/angles_goal", MypalSetAngles, callback=callback + ) + rospy.spin() + + def sub_set_coords(self): + def callback(data): + # angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz] + angles = [data.x, data.y, data.z, data.rx] + + sp = int(data.speed) + model = int(data.model) + self.mc.send_coords(angles, sp, model) + + sub = rospy.Subscriber( + "Mypal/coords_goal", MypalSetCoords, callback=callback + ) + rospy.spin() + + def sub_gripper_status(self): + """Subscribe to Gripper Status""" + """订阅夹爪状态""" + def callback(data): + if data.Status: + self.mc.set_gripper_state(0, 80) + else: + self.mc.set_gripper_state(1, 80) + + sub = rospy.Subscriber( + "Mypal/gripper_status", MypalGripperStatus, callback=callback + ) + rospy.spin() + + def sub_pump_status(self): + def callback(data): + if data.Status: + self.mc.set_basic_output(data.Pin1, 0) + self.mc.set_basic_output(data.Pin2, 0) + else: + self.mc.set_basic_output(data.Pin1, 1) + self.mc.set_basic_output(data.Pin2, 1) + + sub = rospy.Subscriber( + "Mypal/pump_status", MypalPumpStatus, callback=callback + ) + rospy.spin() + + +if __name__ == "__main__": + Watcher() + mc_topics = MypalTopics() + mc_topics.start() + # while True: + # mc_topics.pub_real_coords() + # mc_topics.sub_set_angles() + pass diff --git a/mypalletizer_260/mypalletizer_communication/scripts/mypal_topics_seeed.py b/mypalletizer_260/mypalletizer_communication/scripts/mypal_topics_seeed.py new file mode 100644 index 0000000..c4754cf --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/scripts/mypal_topics_seeed.py @@ -0,0 +1,212 @@ +#!/usr/bin/env python2 +import time +import os +import sys +import signal +import threading + +import rospy + +from mypalletizer_communication import ( + MypalAngles, + MypalCoords, + MypalSetAngles, + MypalSetCoords, + MypalGripperStatus, + MypalPumpStatus, +) +from pymycobot.mypalletizer import MyPalletizer + + +class Watcher: + """this class solves two problems with multithreaded + programs in Python, (1) a signal might be delivered + to any thread (which is just a malfeature) and (2) if + the thread that gets the signal is waiting, the signal + is ignored (which is a bug). + + The watcher is a concurrent process (not thread) that + waits for a signal and the process that contains the + threads. See Appendix A of The Little Book of Semaphores. + http://greenteapress.com/semaphores/ + + I have only tested this on Linux. I would expect it to + work on the Macintosh and not work on Windows. + """ + + def __init__(self): + """Creates a child thread, which returns. The parent + thread waits for a KeyboardInterrupt and then kills + the child thread. + """ + self.child = os.fork() + if self.child == 0: + return + else: + self.watch() + + def watch(self): + try: + os.wait() + except KeyboardInterrupt: + # I put the capital B in KeyBoardInterrupt so I can + # tell when the Watcher gets the SIGINT + print("KeyBoardInterrupt") + self.kill() + sys.exit() + + def kill(self): + try: + os.kill(self.child, signal.SIGKILL) + except OSError: + pass + + +class MypalTopics(object): + def __init__(self): + super(MypalTopics, self).__init__() + + rospy.init_node("Mypal_topics") + rospy.loginfo("start ...") + # problem + port = rospy.get_param("~port", "/dev/ttyUSB0") + baud = rospy.get_param("~baud", 115200) + rospy.loginfo("%s,%s" % (port, baud)) + self.mc = MyPalletizer(port,baud) + self.lock = threading.Lock() + + def start(self): + pa = threading.Thread(target=self.pub_real_angles) + pb = threading.Thread(target=self.pub_real_coords) + sa = threading.Thread(target=self.sub_set_angles) + sb = threading.Thread(target=self.sub_set_coords) + sg = threading.Thread(target=self.sub_gripper_status) + sp = threading.Thread(target=self.sub_pump_status) + + pa.setDaemon(True) + pa.start() + pb.setDaemon(True) + pb.start() + sa.setDaemon(True) + sa.start() + sb.setDaemon(True) + sb.start() + sg.setDaemon(True) + sg.start() + sp.setDaemon(True) + sp.start() + + pa.join() + pb.join() + sa.join() + sb.join() + sg.join() + sp.join() + + def pub_real_angles(self): + """Publish real angle""" + """发布真实角度""" + pub = rospy.Publisher("Mypal/angles_real", MypalAngles, queue_size=5) + ma = MypalAngles() + while not rospy.is_shutdown(): + self.lock.acquire() + angles = self.mc.get_angles() + self.lock.release() + if angles: + ma.joint_1 = angles[0] + ma.joint_2 = angles[1] + ma.joint_3 = angles[2] + ma.joint_4 = angles[3] + # ma.joint_5 = angles[4] + # ma.joint_6 = angles[5] + pub.publish(ma) + time.sleep(0.25) + + def pub_real_coords(self): + """publish real coordinates""" + """发布真实坐标""" + pub = rospy.Publisher("Mypal/coords_real", MypalCoords, queue_size=5) + ma = MypalCoords() + + while not rospy.is_shutdown(): + self.lock.acquire() + coords = self.mc.get_coords() + self.lock.release() + if coords: + ma.x = coords[0] + ma.y = coords[1] + ma.z = coords[2] + ma.rx = coords[3] + # ma.ry = coords[4] + # ma.rz = coords[5] + pub.publish(ma) + time.sleep(0.25) + + def sub_set_angles(self): + """subscription angles""" + """订阅角度""" + def callback(data): + angles = [ + data.joint_1, + data.joint_2, + data.joint_3, + data.joint_4, + # data.joint_5, + # data.joint_6, + ] + sp = int(data.speed) + self.mc.send_angles(angles, sp) + + sub = rospy.Subscriber( + "Mypal/angles_goal", MypalSetAngles, callback=callback + ) + rospy.spin() + + def sub_set_coords(self): + def callback(data): + angles = [data.x, data.y, data.z, data.rx] + sp = int(data.speed) + model = int(data.model) + self.mc.send_coords(angles, sp, model) + + sub = rospy.Subscriber( + "Mypal/coords_goal", MypalSetCoords, callback=callback + ) + rospy.spin() + + def sub_gripper_status(self): + """Subscribe to Gripper Status""" + """订阅夹爪状态""" + def callback(data): + if data.Status: + self.mc.set_gripper_state(0, 80) + else: + self.mc.set_gripper_state(1, 80) + + sub = rospy.Subscriber( + "Mypal/gripper_status", MypalGripperStatus, callback=callback + ) + rospy.spin() + + def sub_pump_status(self): + def callback(data): + if data.Status: + self.mc.set_basic_output(data.Pin1, 0) + self.mc.set_basic_output(data.Pin2, 0) + else: + self.mc.set_basic_output(data.Pin1, 1) + self.mc.set_basic_output(data.Pin2, 1) + + sub = rospy.Subscriber( + "Mypal/pump_status", MypalPumpStatus, callback=callback + ) + rospy.spin() + +if __name__ == "__main__": + Watcher() + mc_topics = MypalTopics() + mc_topics.start() + # while True: + # mc_topics.pub_real_coords() + # mc_topics.sub_set_angles() + pass diff --git a/mypalletizer_260/mypalletizer_communication/scripts/test.py b/mypalletizer_260/mypalletizer_communication/scripts/test.py new file mode 100644 index 0000000..e9cb605 --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/scripts/test.py @@ -0,0 +1,7 @@ +from pymycobot.mypalletizer import MyPalletizer +mp = MyPalletizer("/dev/ttyUSB0", 115200) +n = mp.get_angles() +# while(1): +# print(mp.get_angles()) + # print(mp.get_radians()) +mp.send_angles([100,0,0,150],30) \ No newline at end of file diff --git a/mypalletizer_260/mypalletizer_communication/srv/GetAngles.srv b/mypalletizer_260/mypalletizer_communication/srv/GetAngles.srv new file mode 100644 index 0000000..dcf5f2a --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/srv/GetAngles.srv @@ -0,0 +1,11 @@ + +--- +# float32 base +float32 joint_1 +float32 joint_2 +float32 joint_3 +float32 joint_4 +# float32 joint_5 +# float32 joint_6 + + diff --git a/mypalletizer_260/mypalletizer_communication/srv/GetCoords.srv b/mypalletizer_260/mypalletizer_communication/srv/GetCoords.srv new file mode 100644 index 0000000..aabe929 --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/srv/GetCoords.srv @@ -0,0 +1,9 @@ + +--- + +float32 x +float32 y +float32 z +float32 rx +# float32 ry +# float32 rz \ No newline at end of file diff --git a/mypalletizer_260/mypalletizer_communication/srv/GripperStatus.srv b/mypalletizer_260/mypalletizer_communication/srv/GripperStatus.srv new file mode 100644 index 0000000..3887d75 --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/srv/GripperStatus.srv @@ -0,0 +1,5 @@ +bool Status + +--- + +bool Flag \ No newline at end of file diff --git a/mypalletizer_260/mypalletizer_communication/srv/PumpStatus.srv b/mypalletizer_260/mypalletizer_communication/srv/PumpStatus.srv new file mode 100644 index 0000000..8bd4b53 --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/srv/PumpStatus.srv @@ -0,0 +1,7 @@ +bool Status +int8 Pin1 +int8 Pin2 + +--- + +bool Flag \ No newline at end of file diff --git a/mypalletizer_260/mypalletizer_communication/srv/SetAngles.srv b/mypalletizer_260/mypalletizer_communication/srv/SetAngles.srv new file mode 100644 index 0000000..539dcc3 --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/srv/SetAngles.srv @@ -0,0 +1,15 @@ +# float32 base +float32 joint_1 +float32 joint_2 +float32 joint_3 +float32 joint_4 +# float32 joint_5 +# float32 joint_6 + + + +int8 speed + +--- + +bool Flag \ No newline at end of file diff --git a/mypalletizer_260/mypalletizer_communication/srv/SetCoords.srv b/mypalletizer_260/mypalletizer_communication/srv/SetCoords.srv new file mode 100644 index 0000000..3989373 --- /dev/null +++ b/mypalletizer_260/mypalletizer_communication/srv/SetCoords.srv @@ -0,0 +1,13 @@ +float32 x +float32 y +float32 z +float32 rx +# float32 ry +# float32 rz + +int8 speed +int8 model + +--- + +bool Flag \ No newline at end of file