diff --git a/Leaf_robot_ros/LICENSE b/Leaf_robot_ros/LICENSE new file mode 100644 index 0000000..0a22956 --- /dev/null +++ b/Leaf_robot_ros/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2020 吴雪铭 + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/Leaf_robot_ros/README.md b/Leaf_robot_ros/README.md new file mode 100644 index 0000000..7618f55 --- /dev/null +++ b/Leaf_robot_ros/README.md @@ -0,0 +1,4 @@ +# Leaf_robot_ros +ros机械臂代码上位机部分 +使用smartarm作为机械臂载体 +ros_moviet控制 diff --git a/Leaf_robot_ros/src/image/movit_rviz_image.png b/Leaf_robot_ros/src/image/movit_rviz_image.png new file mode 100644 index 0000000..5449985 Binary files /dev/null and b/Leaf_robot_ros/src/image/movit_rviz_image.png differ diff --git a/Leaf_robot_ros/src/image/rosgraph.bmp b/Leaf_robot_ros/src/image/rosgraph.bmp new file mode 100644 index 0000000..fe6608a Binary files /dev/null and b/Leaf_robot_ros/src/image/rosgraph.bmp differ diff --git a/Leaf_robot_ros/src/image/rosgraph.dot b/Leaf_robot_ros/src/image/rosgraph.dot new file mode 100644 index 0000000..b140f06 --- /dev/null +++ b/Leaf_robot_ros/src/image/rosgraph.dot @@ -0,0 +1,271 @@ +digraph graphname { + graph [bb="0,0,1532.5,748", + compound=True, + rank=same, + rankdir=LR, + ranksep=0.2 + ]; + node [label="\N"]; + subgraph cluster_leaf { + graph [bb="278.18,611,811.27,740", + compound=True, + label=leaf, + lheight=0.21, + lp="544.73,728.5", + lwidth=0.29, + rank=same, + rankdir=LR, + ranksep=0.2, + style=bold + ]; + t___leaf__joint_states [URL=topic_3A__leaf__joint_states, + height=0.5, + label="/leaf/joint_states", + pos="640.77,637", + shape=box, + tooltip="topic:/leaf/joint_states", + width=1.4722]; + n___leaf__leaf_controller__follow_joint_trajectory__action_topics [URL=topic_3Aleaf__leaf_controller__follow_joint_trajectory__action_topics, + height=0.5, + label="leaf/leaf_controller/follow_joint_trajectory/action_topics", + pos="640.77,691", + shape=box, + tooltip="topic:leaf/leaf_controller/follow_joint_trajectory/action_topics", + width=4.5139]; + n___leaf__leaf_driver_node [URL=__leaf__leaf_driver_node, + height=0.5, + label="/leaf/leaf_driver_node", + pos="375.23,691", + shape=ellipse, + tooltip="/leaf/leaf_driver_node", + width=2.4734]; + n___leaf__leaf_controller__follow_joint_trajectory__action_topics -> n___leaf__leaf_driver_node [penwidth=1, + pos="e,458.89,697.31 477.9,697.59 474.97,697.56 472.06,697.52 469.18,697.48"]; + n___leaf__leaf_driver_node -> t___leaf__joint_states [penwidth=1, + pos="e,587.53,644.47 427.65,676.4 443.78,672.08 461.7,667.57 478.27,664 510.85,656.98 547.46,650.73 577.58,646.01"]; + n___leaf__leaf_driver_node -> n___leaf__leaf_controller__follow_joint_trajectory__action_topics [penwidth=1, + pos="e,477.9,684.41 458.89,684.69 461.79,684.63 464.73,684.58 467.7,684.54"]; + } + subgraph cluster_move_group { + graph [bb="1296.5,260,1524.5,389", + compound=True, + label=move_group, + lheight=0.21, + lp="1410.5,377.5", + lwidth=0.97, + rank=same, + rankdir=LR, + ranksep=0.2, + style=bold + ]; + t___move_group__display_planned_path [URL=topic_3A__move_group__display_planned_path, + height=0.5, + label="/move_group/display_planned_path", + pos="1410.5,340", + shape=box, + tooltip="topic:/move_group/display_planned_path", + width=2.9444]; + n___move_group__action_topics [URL=topic_3Amove_group__action_topics, + height=0.5, + label="move_group/action_topics", + pos="1410.5,286", + shape=box, + tooltip="topic:move_group/action_topics", + width=2.2361]; + } + subgraph cluster_pickup { + graph [bb="568.27,8,713.27,83", + compound=True, + label=pickup, + lheight=0.21, + lp="640.77,71.5", + lwidth=0.51, + rank=same, + rankdir=LR, + ranksep=0.2, + style=bold + ]; + n___pickup__action_topics [URL=topic_3Apickup__action_topics, + height=0.5, + label="pickup/action_topics", + pos="640.77,34", + shape=box, + tooltip="topic:pickup/action_topics", + width=1.7917]; + } + subgraph cluster_place { + graph [bb="572.27,174,709.27,249", + compound=True, + label=place, + lheight=0.21, + lp="640.77,237.5", + lwidth=0.40, + rank=same, + rankdir=LR, + ranksep=0.2, + style=bold + ]; + n___place__action_topics [URL=topic_3Aplace__action_topics, + height=0.5, + label="place/action_topics", + pos="640.77,200", + shape=box, + tooltip="topic:place/action_topics", + width=1.6806]; + } + subgraph cluster_execute_trajectory { + graph [bb="536.77,91,744.77,166", + compound=True, + label=execute_trajectory, + lheight=0.21, + lp="640.77,154.5", + lwidth=1.39, + rank=same, + rankdir=LR, + ranksep=0.2, + style=bold + ]; + n___execute_trajectory__action_topics [URL=topic_3Aexecute_trajectory__action_topics, + height=0.5, + label="execute_trajectory/action_topics", + pos="640.77,117", + shape=box, + tooltip="topic:execute_trajectory/action_topics", + width=2.6667]; + } + t___tf_static [URL=topic_3A__tf_static, + height=0.5, + label="/tf_static", + pos="640.77,475", + shape=box, + tooltip="topic:/tf_static", + width=0.88889]; + n___move_group [URL=__move_group, + height=0.5, + label="/move_group", + pos="1053.9,340", + shape=ellipse, + tooltip="/move_group", + width=1.6068]; + t___tf_static -> n___move_group [penwidth=1, + pos="e,1024.5,355.68 672.93,472.5 707.35,469.18 764.14,462.02 811.27,448 886.46,425.63 968.84,385.06 1015.4,360.52"]; + n___joint_state_publisher [URL=__joint_state_publisher, + height=0.5, + label="/joint_state_publisher", + pos="87.092,547", + shape=ellipse, + tooltip="/joint_state_publisher", + width=2.4192]; + t___leaf__joint_states -> n___joint_state_publisher [penwidth=1, + pos="e,156.02,558.08 587.66,628.5 490.32,612.62 280.39,578.37 166.1,559.73"]; + n___leaf__leaf_controller__follow_joint_trajectory__action_topics -> n___move_group [penwidth=1, + pos="e,1043.8,357.76 768.75,672.99 783.72,668.67 798.27,662.87 811.27,655 929.81,583.3 1009.2,429.48 1039.4,366.98"]; + t___tf [URL=topic_3A__tf, + height=0.5, + label="/tf", + pos="640.77,529", + shape=box, + tooltip="topic:/tf", + width=0.75]; + t___tf -> n___move_group [penwidth=1, + pos="e,1033.9,357.08 667.95,528.3 702.05,526.62 763.01,520.96 811.27,502 897.39,468.17 983.99,399.8 1026.3,363.65"]; + n___move_group__action_topics -> n___move_group [penwidth=1, + pos="e,1099.1,328.63 1329.9,292.38 1262.5,301.09 1167.8,315.75 1108.9,326.76"]; + n___rviz_wuxueming_n8xejek_32389_3491311782329954517 [height=0.5, + pos="1053.9,158", + width=6.5175]; + n___move_group__action_topics -> n___rviz_wuxueming_n8xejek_32389_3491311782329954517 [penwidth=1, + pos="e,1117.9,175.43 1371.7,267.97 1313.6,244.32 1201.7,203.51 1127.6,178.68"]; + t___planning_scene_world [URL=topic_3A__planning_scene_world, + height=0.5, + label="/planning_scene_world", + pos="640.77,421", + shape=box, + tooltip="topic:/planning_scene_world", + width=1.9861]; + t___planning_scene_world -> n___move_group [penwidth=1, + pos="e,1007.6,350.99 712.45,410.74 742.93,406.06 778.93,400.18 811.27,394 875.65,381.69 949.07,364.88 997.91,353.31"]; + t___planning_scene [URL=topic_3A__planning_scene, + height=0.5, + label="/planning_scene", + pos="640.77,367", + shape=box, + tooltip="topic:/planning_scene", + width=1.4444]; + t___planning_scene -> n___move_group [penwidth=1, + pos="e,997.06,343.67 693.01,363.64 767.32,358.75 905.05,349.71 986.88,344.34"]; + t___attached_collision_object [URL=topic_3A__attached_collision_object, + height=0.5, + label="/attached_collision_object", + pos="640.77,313", + shape=box, + tooltip="topic:/attached_collision_object", + width=2.1944]; + t___attached_collision_object -> n___move_group [penwidth=1, + pos="e,996.98,336.33 720.07,318.14 797.23,323.21 913.84,330.87 986.57,335.64"]; + t___joint_states [URL=topic_3A__joint_states, + height=0.5, + label="/joint_states", + pos="229.18,502", + shape=box, + tooltip="topic:/joint_states", + width=1.1389]; + n___robot_state_publisher [URL=__robot_state_publisher, + height=0.5, + label="/robot_state_publisher", + pos="375.23,502", + shape=ellipse, + tooltip="/robot_state_publisher", + width=2.4734]; + t___joint_states -> n___robot_state_publisher [penwidth=1, + pos="e,285.93,502 270.31,502 272.03,502 273.77,502 275.54,502"]; + t___joint_states -> n___move_group [penwidth=1, + pos="e,1017.2,326.1 243.23,483.94 276.41,439.75 369.16,326.84 478.27,286 664.9,216.14 908.36,287.41 1007.7,322.7"]; + t___trajectory_execution_event [URL=topic_3A__trajectory_execution_event, + height=0.5, + label="/trajectory_execution_event", + pos="640.77,583", + shape=box, + tooltip="topic:/trajectory_execution_event", + width=2.3194]; + t___trajectory_execution_event -> n___move_group [penwidth=1, + pos="e,1039.4,357.64 724.33,579.34 752.92,575.64 784.49,568.73 811.27,556 908.39,509.84 995.16,412.23 1032.9,365.7"]; + n___pickup__action_topics -> n___move_group [penwidth=1, + pos="e,1022.9,324.61 705.5,29.213 742.25,33.318 786.04,46.457 811.27,78 824.92,95.064 808.16,157.18 819.27,176 863.38,250.74 959.37,296.56 1013.6,320.53"]; + n___pickup__action_topics -> n___rviz_wuxueming_n8xejek_32389_3491311782329954517 [penwidth=1, + pos="e,1008.6,140.26 705.56,48.19 786.07,70.108 923.16,111.56 998.79,136.94"]; + n___place__action_topics -> n___move_group [penwidth=1, + pos="e,1025.5,324.1 701.59,210.05 734.13,219 775.03,231.82 811.27,244 884.41,268.59 969.1,299.7 1016.3,320.05"]; + n___place__action_topics -> n___rviz_wuxueming_n8xejek_32389_3491311782329954517 [penwidth=1, + pos="e,873.76,169.6 701.55,188.96 744.5,183.24 804.94,176.46 863.6,170.61"]; + n___execute_trajectory__action_topics -> n___move_group [penwidth=1, + pos="e,1028.5,323.75 737.08,124.26 763.21,131.75 790.14,143.49 811.27,161 817.09,165.82 814.19,170.41 819.27,176 878.46,241.15 970.61,291.61 1019.6,318.79"]; + n___execute_trajectory__action_topics -> n___rviz_wuxueming_n8xejek_32389_3491311782329954517 [penwidth=1, + pos="e,950.39,141.85 736.86,120.69 797.54,125.58 876.48,133.35 940.28,140.67"]; + n___robot_state_publisher -> t___tf_static [penwidth=1, + pos="e,608.67,478.19 454.94,493.94 502.08,489.11 560.09,483.16 598.41,479.24"]; + n___robot_state_publisher -> t___tf [penwidth=1, + pos="e,613.48,526.31 454.94,510.06 504.16,515.11 565.23,521.36 603.36,525.27"]; + n___joint_state_publisher -> t___joint_states [penwidth=1, + pos="e,188.06,514.89 135.13,531.89 149.08,527.41 164.4,522.49 178.51,517.96"]; + n___move_group -> n___leaf__leaf_controller__follow_joint_trajectory__action_topics [penwidth=1, + pos="e,803.45,677.39 1047,357.9 1025.4,414.13 945.4,586.39 812.29,671.89"]; + n___move_group -> t___move_group__display_planned_path [penwidth=1, + pos="e,1304.4,340 1111.9,340 1160.9,340 1233.3,340 1294.3,340"]; + n___move_group -> n___move_group__action_topics [penwidth=1, + pos="e,1330,303.85 1110.7,336.58 1166.8,329.92 1254.2,316.9 1320.1,305.58"]; + n___move_group -> n___pickup__action_topics [penwidth=1, + pos="e,705.5,44.801 1005.8,329.77 950.56,310.23 861.33,265.26 819.27,194 808.16,175.18 824.92,113.06 811.27,96 788.31,67.291 749.97,53.827 715.55,46.727"]; + n___move_group -> n___place__action_topics [penwidth=1, + pos="e,678.62,218.01 1006.8,329.45 958.31,313.74 879.73,285.02 811.27,262 769.91,248.09 722.47,233.37 688.32,221.45"]; + n___move_group -> n___execute_trajectory__action_topics [penwidth=1, + pos="e,714.12,135.01 1012.6,327.37 962.18,304.92 875.73,256.15 819.27,194 814.19,188.41 817.09,183.82 811.27,179 786.63,158.59 754.12,146.01 724.16,137.68"]; + n___rviz_wuxueming_n8xejek_32389_3491311782329954517 -> n___move_group__action_topics [penwidth=1, + pos="e,1344.7,267.94 1092.2,175.8 1149.5,199.2 1260.6,239.7 1334.9,264.68"]; + n___rviz_wuxueming_n8xejek_32389_3491311782329954517 -> n___pickup__action_topics [penwidth=1, + pos="e,686.94,52.042 978.5,140.9 897.36,118.48 769.38,79.694 696.81,55.374"]; + n___rviz_wuxueming_n8xejek_32389_3491311782329954517 -> n___place__action_topics [penwidth=1, + pos="e,701.55,198.82 951.66,174.25 877.14,183.18 777.73,193.11 711.83,198.07"]; + n___rviz_wuxueming_n8xejek_32389_3491311782329954517 -> n___execute_trajectory__action_topics [penwidth=1, + pos="e,736.86,132.28 871.81,146.65 829.1,142.54 784.97,137.89 746.91,133.46"]; +} diff --git a/Leaf_robot_ros/src/leaf_demo/CMakeLists.txt b/Leaf_robot_ros/src/leaf_demo/CMakeLists.txt new file mode 100644 index 0000000..4394ca7 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_demo/CMakeLists.txt @@ -0,0 +1,212 @@ +cmake_minimum_required(VERSION 2.8.3) +project(leaf_demo) + +## 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 + moveit_msgs + moveit_ros_perception + moveit_ros_planning_interface + roscpp + rospy + trajectory_msgs +) + +## 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 +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## 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 +# moveit_msgs# trajectory_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 +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES leaf_demo +# CATKIN_DEPENDS moveit_msgs moveit_ros_perception moveit_ros_planning_interface roscpp rospy trajectory_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include ${catkin_INCLUDE_DIRS} +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/leaf_demo.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/leaf_demo_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 +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_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_leaf_demo.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) + + +add_executable(euler_orientation + src/euler_orientation.cpp +) +add_dependencies(euler_orientation ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(euler_orientation + ${catkin_LIBRARIES} +) \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_demo/package.xml b/Leaf_robot_ros/src/leaf_demo/package.xml new file mode 100644 index 0000000..58f168d --- /dev/null +++ b/Leaf_robot_ros/src/leaf_demo/package.xml @@ -0,0 +1,70 @@ + + + leaf_demo + 0.0.0 + The leaf_demo package + + + + + wuxueming + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + moveit_core + moveit_ros_planning_interface + + roscpp + rospy + roscpp + rospy + moveit_core + moveit_ros_planning_interface + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_demo/scripts/moveit_cartesian_demo.py b/Leaf_robot_ros/src/leaf_demo/scripts/moveit_cartesian_demo.py new file mode 100644 index 0000000..e261fb3 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_demo/scripts/moveit_cartesian_demo.py @@ -0,0 +1,157 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import rospy, sys +import moveit_commander +from moveit_commander import MoveGroupCommander +from geometry_msgs.msg import Pose +from copy import deepcopy + +class MoveItCartesianDemo: + def __init__(self): + # 初始化move_group的API + moveit_commander.roscpp_initialize(sys.argv) + + # 初始化ROS节点 + rospy.init_node('moveit_cartesian_demo', anonymous=True) + + # 是否需要使用笛卡尔空间的运动规划 + cartesian = rospy.get_param('~cartesian', True) + + # 初始化需要使用move group控制的机械臂中的arm group + arm = MoveGroupCommander('manipulator') + + # 当运动规划失败后，允许重新规划 + arm.allow_replanning(True) + + # 设置目标位置所使用的参考坐标系 + arm.set_pose_reference_frame('link1') + + # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 + arm.set_goal_position_tolerance(0.01) + arm.set_goal_orientation_tolerance(0.01) + + # 设置允许的最大速度和加速度 + arm.set_max_acceleration_scaling_factor(0.02) + arm.set_max_velocity_scaling_factor(0.02) + + # 获取终端link的名称 + end_effector_link = arm.get_end_effector_link() + + # 控制机械臂先回到初始化位置 + arm.set_named_target('stand') + arm.go() + rospy.sleep(1) + + # 获取当前位姿数据最为机械臂运动的起始位姿 + start_pose = arm.get_current_pose(end_effector_link).pose + + # 初始化路点列表 + waypoints = [] + + # 将初始位姿加入路点列表 + if cartesian: + waypoints.append(start_pose) + + # 设置路点数据，并加入路点列表 + wpose = deepcopy(start_pose) + wpose.position.z -= 0.1 + + if cartesian: + waypoints.append(deepcopy(wpose)) + else: + arm.set_pose_target(wpose) + arm.go() + rospy.sleep(1) + + wpose.position.x += 0.1 + + if cartesian: + waypoints.append(deepcopy(wpose)) + else: + arm.set_pose_target(wpose) + arm.go() + rospy.sleep(1) + + wpose.position.y += 0.05 + + if cartesian: + waypoints.append(deepcopy(wpose)) + else: + arm.set_pose_target(wpose) + arm.go() + rospy.sleep(1) + + wpose.position.x -= 0.1 + wpose.position.y -= 0.05 + + if cartesian: + waypoints.append(deepcopy(wpose)) + else: + arm.set_pose_target(wpose) + arm.go() + rospy.sleep(1) + + wpose.position.z += 0.1 + + if cartesian: + waypoints.append(deepcopy(wpose)) + else: + arm.set_pose_target(wpose) + arm.go() + rospy.sleep(1) + + + + if cartesian: + fraction = 0.0 #路径规划覆盖率 + maxtries = 100 #最大尝试规划次数 + attempts = 0 #已经尝试规划次数 + + # 设置机器臂当前的状态作为运动初始状态 + arm.set_start_state_to_current_state() + + # 尝试规划一条笛卡尔空间下的路径，依次通过所有路点 + while fraction < 1.0 and attempts < maxtries: + (plan, fraction) = arm.compute_cartesian_path ( + waypoints, # waypoint poses，路点列表 + 0.01, # eef_step，终端步进值 + 0.0, # jump_threshold，跳跃阈值 + True) # avoid_collisions，避障规划 + + # 尝试次数累加 + attempts += 1 + + # 打印运动规划进程 + if attempts % 10 == 0: + rospy.loginfo("Still trying after " + str(attempts) + " attempts...") + + # 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动 + if fraction == 1.0: + rospy.loginfo("Path computed successfully. Moving the arm.") + arm.execute(plan) + rospy.loginfo("Path execution complete.") + # 如果路径规划失败，则打印失败信息 + else: + rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") + + rospy.sleep(10) + + # 控制机械臂先回到初始化位置 + + print ('home') + arm.set_named_target('home') + arm.go() + rospy.sleep(1) + + + + # 关闭并退出moveit + moveit_commander.roscpp_shutdown() + moveit_commander.os._exit(0) + +if __name__ == "__main__": + try: + MoveItCartesianDemo() + except rospy.ROSInterruptException: + pass diff --git a/Leaf_robot_ros/src/leaf_demo/scripts/moveit_fk_demo.py b/Leaf_robot_ros/src/leaf_demo/scripts/moveit_fk_demo.py new file mode 100644 index 0000000..6d41abd --- /dev/null +++ b/Leaf_robot_ros/src/leaf_demo/scripts/moveit_fk_demo.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import rospy, sys +import moveit_commander + +class MoveItFkDemo: + def __init__(self): + # 初始化move_group的API + moveit_commander.roscpp_initialize(sys.argv) + + # 初始化ROS节点 + rospy.init_node('moveit_fk_demo', anonymous=True) + + # 初始化需要使用move group控制的机械臂中的arm group + arm = moveit_commander.MoveGroupCommander('manipulator') + + # 设置机械臂运动的允许误差值 + arm.set_goal_joint_tolerance(0.01) + + # 设置允许的最大速度和加速度 + arm.set_max_acceleration_scaling_factor(0.02) + arm.set_max_velocity_scaling_factor(0.02) + + # 控制机械臂先回到初始化位置 + arm.set_named_target('zero') + arm.go() + rospy.sleep(1) + + # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度） + joint_positions = [0.391410, -0.676384, -0.376217, 0.0, 1.052834, 0.454125] + arm.set_joint_value_target(joint_positions) + + # 控制机械臂完成运动 + arm.go() + rospy.sleep(1) + + # 控制机械臂先回到初始化位置 + arm.set_named_target('home') + arm.go() + rospy.sleep(1) + + # 关闭并退出moveit + moveit_commander.roscpp_shutdown() + moveit_commander.os._exit(0) + +if __name__ == "__main__": + try: + MoveItFkDemo() + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_demo/scripts/moveit_ik_demo.py b/Leaf_robot_ros/src/leaf_demo/scripts/moveit_ik_demo.py new file mode 100644 index 0000000..51b75b1 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_demo/scripts/moveit_ik_demo.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import rospy, sys +import moveit_commander +from geometry_msgs.msg import PoseStamped, Pose + + +class MoveItIkDemo: + def __init__(self): + # 初始化move_group的API + moveit_commander.roscpp_initialize(sys.argv) + + # 初始化ROS节点 + rospy.init_node('moveit_ik_demo') + + # 初始化需要使用move group控制的机械臂中的arm group + arm = moveit_commander.MoveGroupCommander('manipulator') + + # 获取终端link的名称 + end_effector_link = arm.get_end_effector_link() + + # 设置目标位置所使用的参考坐标系 + reference_frame = 'link1' + arm.set_pose_reference_frame(reference_frame) + + # 当运动规划失败后，允许重新规划 + arm.allow_replanning(True) + + # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 + arm.set_goal_position_tolerance(0.01) + arm.set_goal_orientation_tolerance(0.01) + + # 设置允许的最大速度和加速度 + arm.set_max_acceleration_scaling_factor(0.02) + arm.set_max_velocity_scaling_factor(0.02) + + # 控制机械臂先回到初始化位置 + arm.set_named_target('zero') + print ("Zero...") + arm.go() + print ("Arrive Zero") + rospy.sleep(1) + + # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述， + # 姿态使用四元数描述，基于base_link坐标系 + target_pose = PoseStamped() + target_pose.header.frame_id = reference_frame + target_pose.header.stamp = rospy.Time.now() + target_pose.pose.position.x = 0.2 + target_pose.pose.position.y = 0.0 + target_pose.pose.position.z = 0.20 + target_pose.pose.orientation.x = 0.0 + target_pose.pose.orientation.y = 0.70692 + target_pose.pose.orientation.z = 0.0 + target_pose.pose.orientation.w = 0.70729 + + # 设置机器臂当前的状态作为运动初始状态 + arm.set_start_state_to_current_state() + + # 设置机械臂终端运动的目标位姿 + arm.set_pose_target(target_pose, end_effector_link) + + # 规划运动路径 + traj = arm.plan() + + # 按照规划的运动路径控制机械臂运动 + arm.execute(traj) + rospy.sleep(1) + + # 控制机械臂回到初始化位置 + arm.set_named_target('home') + arm.go() + + # 关闭并退出moveit + moveit_commander.roscpp_shutdown() + moveit_commander.os._exit(0) + +if __name__ == "__main__": + MoveItIkDemo() + + + diff --git a/Leaf_robot_ros/src/leaf_demo/src/euler_orientation.cpp b/Leaf_robot_ros/src/leaf_demo/src/euler_orientation.cpp new file mode 100644 index 0000000..e860792 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_demo/src/euler_orientation.cpp @@ -0,0 +1,40 @@ + +#include +#include + +using namespace std; + +int main(int argc, char *argv[]) +{ + double ex = 0; + double ey = 0; + double ez = 0; + + double x=0; + double y=0; + double z=0; + double w=0; + + cout << "Roll Pitch Taw (Z Y X) 弧度制 :\n"; + cin >> ez >> ey >> ex; + + + double cy = cos(ex * 0.5); + double sy = sin(ex * 0.5); + + double cp = cos(ey * 0.5); + double sp = sin(ey * 0.5); + + double cr = cos(ez * 0.5); + double sr = sin(ez * 0.5); + + w = cy * cp * cr + sy * sp * sr; + x = cy * cp * sr - sy * sp * cr; + y = sy * cp * sr + cr * sp * cr; + z = sy * cp * cr - cy * sp * sr; + + cout << "x y z w "<< x << " " << y << " " << z << " " << w << endl; + + + return 0; +} \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_description/CMakeLists.txt b/Leaf_robot_ros/src/leaf_description/CMakeLists.txt new file mode 100644 index 0000000..d435e55 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_description/CMakeLists.txt @@ -0,0 +1,199 @@ +cmake_minimum_required(VERSION 2.8.3) +project(leaf_description) + +## 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 + roscpp + rospy + std_msgs +) + +## 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 +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## 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 +# ) + +################################################ +## 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 +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES leaf_description +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## 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}/leaf_description.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/leaf_description_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 +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_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_leaf_description.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/Leaf_robot_ros/src/leaf_description/SmallRobotArm.PNG b/Leaf_robot_ros/src/leaf_description/SmallRobotArm.PNG new file mode 100644 index 0000000..a50cd8c Binary files /dev/null and b/Leaf_robot_ros/src/leaf_description/SmallRobotArm.PNG differ diff --git a/Leaf_robot_ros/src/leaf_description/config/urdf.rviz b/Leaf_robot_ros/src/leaf_description/config/urdf.rviz new file mode 100644 index 0000000..25641a6 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_description/config/urdf.rviz @@ -0,0 +1,220 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 359 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + - /TF1/Frames1 + Splitter Ratio: 0.5 + Tree Height: 775 +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 0; 0; 0 + 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 + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: true + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + 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 + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + link1: + Value: false + link2: + Value: false + link3: + Value: false + link4: + Value: false + link5: + Value: false + link6: + Value: false + link7: + Value: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + link1: + link2: + link3: + link4: + link5: + link6: + link7: + {} + Update Interval: 0 + Value: true + - Class: rviz/Axes + Enabled: false + Length: 1 + Name: Axes + Radius: 0.100000001 + Reference Frame: + Value: false + Enabled: true + Global Options: + Background Color: 200; 255; 178 + Default Light: true + Fixed Frame: link1 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.20125794 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.145467401 + Y: -0.0443756618 + Z: 0.00388843659 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 1.15479636 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.46041584 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001b200000396fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000100044006900730070006c006100790073010000002800000396000000dd00ffffff000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005c80000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1920 + X: 0 + Y: 24 diff --git a/Leaf_robot_ros/src/leaf_description/launch/leaf_description.launch b/Leaf_robot_ros/src/leaf_description/launch/leaf_description.launch new file mode 100644 index 0000000..ef5171b --- /dev/null +++ b/Leaf_robot_ros/src/leaf_description/launch/leaf_description.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_description/launch/leaf_display.launch b/Leaf_robot_ros/src/leaf_description/launch/leaf_display.launch new file mode 100644 index 0000000..9ff2939 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_description/launch/leaf_display.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_description/meshes/link1.STL b/Leaf_robot_ros/src/leaf_description/meshes/link1.STL new file mode 100644 index 0000000..38bee79 Binary files /dev/null and b/Leaf_robot_ros/src/leaf_description/meshes/link1.STL differ diff --git a/Leaf_robot_ros/src/leaf_description/meshes/link2.STL b/Leaf_robot_ros/src/leaf_description/meshes/link2.STL new file mode 100644 index 0000000..6715c73 Binary files /dev/null and b/Leaf_robot_ros/src/leaf_description/meshes/link2.STL differ diff --git a/Leaf_robot_ros/src/leaf_description/meshes/link3.STL b/Leaf_robot_ros/src/leaf_description/meshes/link3.STL new file mode 100644 index 0000000..ca5f7cb Binary files /dev/null and b/Leaf_robot_ros/src/leaf_description/meshes/link3.STL differ diff --git a/Leaf_robot_ros/src/leaf_description/meshes/link4.STL b/Leaf_robot_ros/src/leaf_description/meshes/link4.STL new file mode 100644 index 0000000..f20a089 Binary files /dev/null and b/Leaf_robot_ros/src/leaf_description/meshes/link4.STL differ diff --git a/Leaf_robot_ros/src/leaf_description/meshes/link5.STL b/Leaf_robot_ros/src/leaf_description/meshes/link5.STL new file mode 100644 index 0000000..9145ca7 Binary files /dev/null and b/Leaf_robot_ros/src/leaf_description/meshes/link5.STL differ diff --git a/Leaf_robot_ros/src/leaf_description/meshes/link6.STL b/Leaf_robot_ros/src/leaf_description/meshes/link6.STL new file mode 100644 index 0000000..861c0bb Binary files /dev/null and b/Leaf_robot_ros/src/leaf_description/meshes/link6.STL differ diff --git a/Leaf_robot_ros/src/leaf_description/meshes/link7.STL b/Leaf_robot_ros/src/leaf_description/meshes/link7.STL new file mode 100644 index 0000000..a148d98 Binary files /dev/null and b/Leaf_robot_ros/src/leaf_description/meshes/link7.STL differ diff --git "a/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link1.STL" "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link1.STL" new file mode 100644 index 0000000..38bee79 Binary files /dev/null and "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link1.STL" differ diff --git "a/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link2.STL" "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link2.STL" new file mode 100644 index 0000000..6715c73 Binary files /dev/null and "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link2.STL" differ diff --git "a/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link3.STL" "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link3.STL" new file mode 100644 index 0000000..ca5f7cb Binary files /dev/null and "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link3.STL" differ diff --git "a/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link4.STL" "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link4.STL" new file mode 100644 index 0000000..f20a089 Binary files /dev/null and "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link4.STL" differ diff --git "a/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link5.STL" "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link5.STL" new file mode 100644 index 0000000..9145ca7 Binary files /dev/null and "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link5.STL" differ diff --git "a/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link6.STL" "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link6.STL" new file mode 100644 index 0000000..861c0bb Binary files /dev/null and "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link6.STL" differ diff --git "a/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link7.STL" "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link7.STL" new file mode 100644 index 0000000..a148d98 Binary files /dev/null and "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/stl/link7.STL" differ diff --git "a/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link1.txt" "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link1.txt" new file mode 100644 index 0000000..fcaacdc --- /dev/null +++ "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link1.txt" @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git "a/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link2.txt" "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link2.txt" new file mode 100644 index 0000000..0703b49 --- /dev/null +++ "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link2.txt" @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git "a/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link3.txt" "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link3.txt" new file mode 100644 index 0000000..f6a1821 --- /dev/null +++ "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link3.txt" @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git "a/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link4.txt" "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link4.txt" new file mode 100644 index 0000000..56cc833 --- /dev/null +++ "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link4.txt" @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git "a/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link5.txt" "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link5.txt" new file mode 100644 index 0000000..54990a5 --- /dev/null +++ "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link5.txt" @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git "a/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link6.txt" "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link6.txt" new file mode 100644 index 0000000..fbfa9ed --- /dev/null +++ "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link6.txt" @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git "a/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link7.txt" "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link7.txt" new file mode 100644 index 0000000..c6385a3 --- /dev/null +++ "b/Leaf_robot_ros/src/leaf_description/meshes/rviz\351\207\215\351\205\215\347\275\256/\350\260\203\346\225\264/link7.txt" @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git "a/Leaf_robot_ros/src/leaf_description/meshes/\345\244\207\344\273\275/meshes.zip" "b/Leaf_robot_ros/src/leaf_description/meshes/\345\244\207\344\273\275/meshes.zip" new file mode 100644 index 0000000..c589b45 Binary files /dev/null and "b/Leaf_robot_ros/src/leaf_description/meshes/\345\244\207\344\273\275/meshes.zip" differ diff --git a/Leaf_robot_ros/src/leaf_description/package.xml b/Leaf_robot_ros/src/leaf_description/package.xml new file mode 100644 index 0000000..a5d8c1f --- /dev/null +++ b/Leaf_robot_ros/src/leaf_description/package.xml @@ -0,0 +1,68 @@ + + + leaf_description + 0.0.0 + The leaf_description package + + + + + wuxueming + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_description/urdf/leaf.urdf b/Leaf_robot_ros/src/leaf_description/urdf/leaf.urdf new file mode 100644 index 0000000..cf85abb --- /dev/null +++ b/Leaf_robot_ros/src/leaf_description/urdf/leaf.urdf @@ -0,0 +1,184 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_description/urdf/save.txt b/Leaf_robot_ros/src/leaf_description/urdf/save.txt new file mode 100644 index 0000000..c10b9c0 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_description/urdf/save.txt @@ -0,0 +1,184 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_driver/CMakeLists.txt b/Leaf_robot_ros/src/leaf_driver/CMakeLists.txt new file mode 100644 index 0000000..7bf914b --- /dev/null +++ b/Leaf_robot_ros/src/leaf_driver/CMakeLists.txt @@ -0,0 +1,222 @@ +cmake_minimum_required(VERSION 2.8.3) +project(leaf_driver) + +## 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 + roscpp + serial + joint_state_publisher + robot_state_publisher + actionlib + moveit_msgs + control_msgs + sensor_msgs +) + +## 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 +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## 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 +# ) + +################################################ +## 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 +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES leaf_driver +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include ${catkin_INCLUDE_DIRS} +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/leaf_driver.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/leaf_driver_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 +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_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_leaf_driver.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) + + + +add_executable(main + include/leaf_driver/common.h + include/leaf_driver/UDP.h + include/leaf_driver/ManipulatorProtocol.h + include/leaf_driver/LeafRobot.h + src/UDP.cpp + src/ManipulatorProtocol.cpp + src/LeafRobot.cpp + src/main.cpp +) +add_dependencies(main ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(main + ${catkin_LIBRARIES} +) \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_driver/config/controllers.yaml b/Leaf_robot_ros/src/leaf_driver/config/controllers.yaml new file mode 100644 index 0000000..56aa994 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_driver/config/controllers.yaml @@ -0,0 +1,13 @@ +#controller_manager_ns: controller_manager +controller_list: + - name: leaf/leaf_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_driver/include/leaf_driver/LeafRobot.h b/Leaf_robot_ros/src/leaf_driver/include/leaf_driver/LeafRobot.h new file mode 100644 index 0000000..cd9ece7 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_driver/include/leaf_driver/LeafRobot.h @@ -0,0 +1,95 @@ +#ifndef LEAFROBOT_H +#define LEAFROBOT_H + +#include "leaf_driver/ManipulatorProtocol.h" + + +//相当于MyRbot +class LeafRobot +{ +public: + LeafRobot( string name ); //传入action的名称 + virtual ~LeafRobot(); + void home(); //收起来 + void zero(); //直立起来 + bool read(); //0为正常读取到数据，-1为没有读取到数据或数据出错 + int write(); //数据写入 + + //timer回调函数 + void timerCallback(const ros::TimerEvent& e); + + //goal回调函数 + void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal); + + //添加路点到容器中 并重新排序 + void reorder_traj_joints(trajectory_msgs::JointTrajectory trajectory_); + + //执行轨迹上的所有路点 + void trackMove(); + + //某个路点已经到达 + bool isArrived(); + + //机械臂当前还处于运动状态 + bool isMoving(); + + //关节状态更新 + void jointStateUpdate(); + +private: + //句柄实例 + ros::NodeHandle nh_; + + //ros系统时间 + ros::Time time_current, time_prev; + + //ros定时器 + ros::Timer timer; + double clock; + + string action_name; + //定义action服务端 + actionlib::SimpleActionServer as_; + //actionlib::ActionServer as_; + + //反馈 + control_msgs::FollowJointTrajectoryFeedback feedback_; + + //用来反馈action目标的执行情况，客户端由此可以得知服务端是否执行成功了 + control_msgs::FollowJointTrajectoryResult result_; + + + //关节状态发布 + ros::Publisher joint_pub_; + sensor_msgs::JointState msg; + sensor_msgs::JointState msg_pre; + + //串口 + string serialport_name; + int baudrate; + + //通讯协议对象 + ManipulatorProtocol * manipulatorProtocolPtr; + + //通讯质量 + int errortimes; + long reads; + long writes; + + //自由度 + const int joint_count; + string * joint_name; + + + //路径点容器 + vector wayPoints; + + //下一个路径点对应的数据 + vector nextPoint; + + //各个关节转PI/2需要的节拍 + int * plu2angel; + int * zeroPlu; +}; + +#endif //LEAFROBOT diff --git a/Leaf_robot_ros/src/leaf_driver/include/leaf_driver/ManipulatorProtocol.h b/Leaf_robot_ros/src/leaf_driver/include/leaf_driver/ManipulatorProtocol.h new file mode 100644 index 0000000..09e81ef --- /dev/null +++ b/Leaf_robot_ros/src/leaf_driver/include/leaf_driver/ManipulatorProtocol.h @@ -0,0 +1,49 @@ +#ifndef MANIPULATOR_PROTOCOL_H +#define MANIPULATOR_PROTOCOL_H + +#include "leaf_driver/UDP.h" + +//具体应用，结构应和具体应用场景一样 +struct StepMotor +{ + //步进电机 + int position; //位置，基本控制(节拍) + int velocity; //速度，中等控制，(节拍/秒) + int effort; //电流，高级控制，(ps 计算机专业 知识有限) 目前不会控制 (mA) +}; + +struct Manipulator +{ + //机械臂 + int degrees; //自由度，确定关节以及电机数量 + vector stepMotorList; //关节列表 + uint8_t * stream; //数据流，用于嵌入下层通讯协议字段中 +}; + +class ManipulatorProtocol +{ +public: + ManipulatorProtocol(); + virtual ~ManipulatorProtocol(); + + void setSerialPort( string serialport_name, int baudrate ); + + void manipulatorInit(int degrees); + + bool read(); //调用完毕后，从manipulator对象中读取数据 + int write(); //调用前，将数据放入manipulator对象中 + + //机械臂对象 + Manipulator readManipulator; + Manipulator writeManipulator; + + //下层通讯句柄 + UDP *udpHandle; + +private: + + //消息 + message msg; +}; + +#endif // MANIPULATOR_PROTOCOL \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_driver/include/leaf_driver/UDP.h b/Leaf_robot_ros/src/leaf_driver/include/leaf_driver/UDP.h new file mode 100644 index 0000000..381b5cf --- /dev/null +++ b/Leaf_robot_ros/src/leaf_driver/include/leaf_driver/UDP.h @@ -0,0 +1,82 @@ +#ifndef UDP_H +#define UDP_H + +#include "leaf_driver/common.h" +struct message +{ + int valueSize; + uint8_t start; + uint8_t * value; + uint8_t crc; + uint8_t end; +}; + +class UDP +{ +public: + //构造函数 + UDP(); + + virtual ~UDP(); + + //设置串口 + void setSerialPort( string serialport_name, int baudrate ); + + //message + void setMessageValueSize(message & msg, int size); + void setStart(message & msg, uint8_t signal_); + void setCrc(message & msg); + void setEnd(message & msg, uint8_t signal_); + int getMessageValueSize(message & msg) const; + uint8_t getStart(message & msg) const; + uint8_t getCrc(message & msg) const; + uint8_t getEnd(message & msg) const; + + //缓存数组最大长度 + void setBufferSize(int size); + int getBufferSize() const; + + int write( message & msg ); + bool read( message & msg ); + + //串口 + Serial * serialPort; + +private: + + //清理UDP缓存 + void flushTxBuffer(); + void flushRxBuffer(); + void flush(); + + void printBuffer(uint8_t * buf, int size); + + //接收报文数据，包括接收数据和接收反馈,并写入rxBuffer中。 + //需要 起始界符 末尾界符 总长度 + int recieve(uint8_t startSignal, uint8_t endSignal, int size); + bool simpleRecieve(uint8_t startSignal, uint8_t endSignal, int size); + + void msg2udp( message & msg );//msg -> txBuffer 发送数据时使用 + void udp2msg( message & msg );//rxBuffer -> msg 接收数据时使用 + + //缓存数组最大长度 + int bufferSize; + + //UDP发送缓存 + uint8_t * txBuffer; + + //UDP接收缓存 + uint8_t * rxBuffer; + + //记录UDP缓存区中已用的位数 + int rxIndex; + int txIndex; + + //记录数据传输过程中通讯状态,为recieve函数提供服务 + // 0 良好 1 不好 + int recieveStatus; + uint8_t * udata; + +}; + +#endif // UDP \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_driver/include/leaf_driver/common.h b/Leaf_robot_ros/src/leaf_driver/include/leaf_driver/common.h new file mode 100644 index 0000000..1637b8e --- /dev/null +++ b/Leaf_robot_ros/src/leaf_driver/include/leaf_driver/common.h @@ -0,0 +1,42 @@ +#ifndef COMMON_H +#define COMMON_H + +//所以平时需要用到的头文件都放在这里 +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include +#include +#include +#include "serial/serial.h" + + +//ros头文件 +#include "ros/ros.h" +#include "std_msgs/UInt16.h" +#include "sensor_msgs/JointState.h" +#include "robot_state_publisher/robot_state_publisher.h" + +//动作编程 +#include "actionlib/server/action_server.h" +#include "actionlib/server/simple_action_server.h" +#include "actionlib/server/server_goal_handle.h" +#include "control_msgs/FollowJointTrajectoryAction.h" + +using namespace std; +using namespace serial; + +//常量定义 +const double PI = 3.1415926; + + +#endif // __COMMON__ \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_driver/launch/leaf_bringup.launch b/Leaf_robot_ros/src/leaf_driver/launch/leaf_bringup.launch new file mode 100644 index 0000000..52dde3e --- /dev/null +++ b/Leaf_robot_ros/src/leaf_driver/launch/leaf_bringup.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_driver/launch/leaf_hardware.launch b/Leaf_robot_ros/src/leaf_driver/launch/leaf_hardware.launch new file mode 100644 index 0000000..d7556ee --- /dev/null +++ b/Leaf_robot_ros/src/leaf_driver/launch/leaf_hardware.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_driver/launch/load_config.launch b/Leaf_robot_ros/src/leaf_driver/launch/load_config.launch new file mode 100644 index 0000000..13768ff --- /dev/null +++ b/Leaf_robot_ros/src/leaf_driver/launch/load_config.launch @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + [/leaf/joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_driver/package.xml b/Leaf_robot_ros/src/leaf_driver/package.xml new file mode 100644 index 0000000..419b735 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_driver/package.xml @@ -0,0 +1,83 @@ + + + leaf_driver + 0.0.0 + The leaf_driver package + + + + + wuxueming + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + roscpp + joint_state_publisher + robot_state_publisher + actionlib + moveit_msgs + control_msgs + sensor_msgs + + roscpp + joint_state_publisher + robot_state_publisher + actionlib + moveit_msgs + control_msgs + sensor_msgs + + roscpp + joint_state_publisher + robot_state_publisher + actionlib + moveit_msgs + control_msgs + sensor_msgs + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_driver/rules/arduino_mega2560.rules b/Leaf_robot_ros/src/leaf_driver/rules/arduino_mega2560.rules new file mode 100644 index 0000000..4eb35b3 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_driver/rules/arduino_mega2560.rules @@ -0,0 +1 @@ +ATTRS{idVendor}=="2a03", ATTRS{idProduct}=="0042", SYMLINK+="arduino_mega2560" \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_driver/src/LeafRobot.cpp b/Leaf_robot_ros/src/leaf_driver/src/LeafRobot.cpp new file mode 100644 index 0000000..860f55f --- /dev/null +++ b/Leaf_robot_ros/src/leaf_driver/src/LeafRobot.cpp @@ -0,0 +1,341 @@ +#include "leaf_driver/LeafRobot.h" + + +LeafRobot::LeafRobot(string name) : as_(nh_, name, boost::bind(&LeafRobot::executeCB, this, _1), false) ,joint_count(6) +{ + action_name = name; + //私有参数 获取 + ros::NodeHandle nh_private("~"); + nh_private.param("serialport_name", serialport_name, "/dev/ttyACM0"); + nh_private.param("baudrate", baudrate, 230400); + clock = 0.025; + + cout << "串口名 : " << serialport_name << " , 波特率 : " << baudrate << endl; + + //初始化一些变量 + joint_name = new string[joint_count]; + plu2angel = new int[joint_count]; + zeroPlu = new int[joint_count]; + msg.name.resize(joint_count); + msg.position.resize(joint_count); + msg.header.frame_id = "/leaf"; + + //下一个路径点 + StepMotor stepmotor = { 0, 0, 0 }; + for (size_t i = 0; i < joint_count; i ++) + { + nextPoint.push_back(stepmotor); + } + + //重要参数 + //旋转+90°(+1.5707963)，需要的节拍 + plu2angel[0] = -7800; + plu2angel[1] = -6350; + plu2angel[2] = -8000; + plu2angel[3] = -4550; + plu2angel[4] = -3400; + plu2angel[5] = -1600; + + //零点参数 + zeroPlu[0] = 0; + zeroPlu[1] = -5600; + zeroPlu[2] = 6600; + zeroPlu[3] = 0; + zeroPlu[4] = -3400; + zeroPlu[5] = 0; + + for (size_t i = 0; i < joint_count; i++) + { + msg.position[i] = 0; + } + errortimes = 0; + reads = 0; + writes = 0; + + + //关节命名 + stringstream ss; + ss.clear();ss.str(""); + for (size_t i = 0; i < joint_count; i++) + { + ss << "joint" << i + 1; + joint_name[i] = ss.str(); + msg.name[i] = joint_name[i]; + ss.clear();ss.str(""); + } + + //初始化协议 + manipulatorProtocolPtr = new ManipulatorProtocol(); + manipulatorProtocolPtr->manipulatorInit(joint_count); + manipulatorProtocolPtr->setSerialPort(serialport_name, baudrate); + + //关节发布 + joint_pub_ = nh_.advertise("/leaf/joint_states", 1); + + timer = nh_.createTimer(ros::Duration(clock), &LeafRobot::timerCallback, this); + cout << "定时器启动完毕" <udpHandle->serialPort->flush (); + //到达0点 + //zero(); + //write(); + cout << "机械臂到达零点位置..." <writeManipulator.stepMotorList[i].position = 0; + } + cout << "home..." << endl << endl; +} + +void LeafRobot::zero() +{ + for (size_t i = 0; i < joint_count; i ++) + { + manipulatorProtocolPtr->writeManipulator.stepMotorList[i].position = zeroPlu[i]; + } + cout << "zero..." << endl << endl; +} + + +bool LeafRobot::read() +{ + if (manipulatorProtocolPtr->udpHandle->serialPort->available() >= 57) + { + if (manipulatorProtocolPtr->read()) + { + reads++; + //获取时间 + /* + time_current = ros::Time::now(); + double elapsed = (time_current - time_prev).toSec(); + time_prev = time_current; + + cout << "\nErrorTimes : " << errortimes + << " , R : " << reads + << " , W : " << writes + << " , Rate: " << setprecision(8) << (double)(reads-errortimes) * 100 / (double)reads << "%" + << " duration : " << setprecision(8) << elapsed << endl; + */ + return true; + } + else + { + errortimes++; + //cout << "\n\033[31mGetError\033[0m"; + cout << "\033[31mErrorTimes\033[0m : " << errortimes + << " , R : " << reads + << " , W : " << writes + << " , Rate: " << setprecision(8) << (double)(reads-errortimes) * 100 / (double)reads << "%\n"; + timer.stop(); + manipulatorProtocolPtr->udpHandle->serialPort->flush(); + usleep(30000);//停止0.5s + timer.start(); + + } + } + //cout << "没有读到数据" << endl; + return false; +} + +int LeafRobot::write() +{ + writes++; + //从cmd中获取数据 + + /* + for (size_t i = 0; i < joint_count; i++) + { + manipulatorProtocolPtr->writeManipulator.stepMotorList[i].position = round(cmd[i] * 2 * plu2angel[i] / PI) + zeroPlu[i]; + //cout << cmd[i] << " "; + } + time_current = ros::Time::now(); + double elapsed = (time_current - time_prev).toSec(); + time_prev = time_current; + cout << "writing duration : " << elapsed << endl;*/ + return manipulatorProtocolPtr->write(); + //cout << endl ; +} + + +void LeafRobot::timerCallback(const ros::TimerEvent& e) +{ + //cout << "TimerCallback\n"; + //write(); + if(read()) + { + return; + } + usleep(1000); + if(read()) + { + return; + } +} + + +void LeafRobot::executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) +{ + cout << "executeCB\n"; + reorder_traj_joints(goal->trajectory); + trackMove(); + + if (isArrived()) + { + result_.error_code = result_.SUCCESSFUL; + as_.setSucceeded(result_); + } + else + { + result_.error_code = result_.GOAL_TOLERANCE_VIOLATED; + as_.setAborted(result_); + } + + +} + +void LeafRobot::reorder_traj_joints(trajectory_msgs::JointTrajectory trajectory_) +{ + feedback_.header.frame_id = trajectory_.header.frame_id; + //添加路点到容器中 + cout << "Points count : " << trajectory_.points.size() << endl; + for (size_t seq = 0; seq < trajectory_.points.size(); seq ++) + { + wayPoints.push_back(trajectory_.points[seq]); + } + + //Feedback也要进行设计 + feedback_.joint_names.clear(); + + //根据名称进行排序 + for (size_t index = 0; index < joint_count; index ++) + {//joint_names中，逐个索引 + const char *p = trajectory_.joint_names[index].c_str(); + int degree_id = 0; + for (size_t i = 0; i < trajectory_.joint_names[index].length(); i ++) + {//string -> int。 将编号提取出来 + degree_id = degree_id * 10 + (int)(p[i + 5] - '0'); + } + feedback_.joint_names.push_back(trajectory_.joint_names[index]); + } +} + + +void LeafRobot::trackMove() +{ + //从cmd中获取数据 + cout << "TrackMoving \n"; + feedback_.desired.positions.clear(); + + //将路点的终点写入 + for (size_t i = 0; i < joint_count; i++) + { + feedback_.desired.positions.push_back(wayPoints[wayPoints.size() - 1].positions[i]); + } + + for (size_t seq = 0; seq < wayPoints.size(); seq ++) + { + //cout << "Push " << seq << "th point : \n"; + for (size_t i = 0; i < joint_count; i++) + {//需要到达的点 + //feedback_.desired.positions.push_back(wayPoints[seq].positions[i]); + nextPoint[i].position = round(wayPoints[seq].positions[i] * 2 * plu2angel[i] / PI) + zeroPlu[i]; + manipulatorProtocolPtr->writeManipulator.stepMotorList[i].position = nextPoint[i].position; + //cout << manipulatorProtocolPtr->writeManipulator.stepMotorList[i].position << " "; + } + write(); + usleep(80000); //点写入频率最高为14hz，默认10hz + + } + ROS_INFO("LeafRobot: Now , Let's go !"); + usleep(1000000); + while (isMoving()) + {//等待机械臂运动停止 + feedback_.actual.positions.clear(); + for (size_t i = 0; i < joint_count; i++) + {//当前点 + feedback_.actual.positions.push_back(msg.position[i]); + } + feedback_.header.stamp = msg.header.stamp; + as_.publishFeedback(feedback_); + usleep(100000); //10hz速率进行反馈 + } + + ROS_INFO("stop Moving"); + for( size_t tolerance = 0 ; !isArrived() && tolerance < 20 ; tolerance ++) + { + if (!isMoving() && tolerance % 5 == 0) + { //机械臂停了，如果没有到达最终点，那就再写一遍 + ROS_WARN("LeafRbot : write finish goal, it means data loss happend. "); + write(); + } + feedback_.actual.positions.clear(); + for (size_t i = 0; i < joint_count; i++) + {//当前点 + feedback_.actual.positions.push_back(msg.position[i]); + } + feedback_.header.stamp = msg.header.stamp; + as_.publishFeedback(feedback_); + usleep(100000); //10hz速率进行反馈 + } + ROS_INFO("LeafRobot: Done !"); + wayPoints.clear(); +} + + +bool LeafRobot::isArrived() +{ + for (size_t i = 0; i < manipulatorProtocolPtr->writeManipulator.degrees; i ++) + { + if ( manipulatorProtocolPtr->writeManipulator.stepMotorList[i].position != manipulatorProtocolPtr->readManipulator.stepMotorList[i].position) + { + //cout << "第"<< i + 1 << "号关节没有到位\n"; + return false; + } + } + return true; +} + +bool LeafRobot::isMoving() +{ + if ( msg_pre.position == msg.position ) + { + return false; + } + return true; +} + + +void LeafRobot::jointStateUpdate() +{ + msg_pre = msg; + //cout << "jointStateUpdate\n"; + for (size_t i = 0; i < manipulatorProtocolPtr->readManipulator.degrees ; i ++) + {//获取到数据 写入到pos中 + msg.position[i] = (manipulatorProtocolPtr->readManipulator.stepMotorList[i].position - zeroPlu[i]) * PI / (2 * plu2angel[i]); + //cout << manipulatorProtocolPtr->readManipulator.stepMotorList[i].position << " "; + } + msg.header.stamp = ros::Time::now(); + joint_pub_.publish(msg); +} \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_driver/src/ManipulatorProtocol.cpp b/Leaf_robot_ros/src/leaf_driver/src/ManipulatorProtocol.cpp new file mode 100644 index 0000000..415c708 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_driver/src/ManipulatorProtocol.cpp @@ -0,0 +1,99 @@ +#include "leaf_driver/ManipulatorProtocol.h" + +ManipulatorProtocol::ManipulatorProtocol() +{ + udpHandle = new UDP(); +} + +ManipulatorProtocol::~ManipulatorProtocol() +{ + delete []msg.value; + msg.value = NULL; + delete []readManipulator.stream; + readManipulator.stream = NULL; + delete []writeManipulator.stream; + writeManipulator.stream = NULL; +} + +void ManipulatorProtocol::setSerialPort( string serialport_name, int baudrate ) +{ + udpHandle->setSerialPort( serialport_name, baudrate ); +} + +void ManipulatorProtocol::manipulatorInit(int degrees) +{ + readManipulator.degrees = degrees; + writeManipulator.degrees = degrees; + StepMotor stepmotor = { 0, 0, 0 }; + for (size_t i = 0; i < degrees; i ++) + { + readManipulator.stepMotorList.push_back(stepmotor); + writeManipulator.stepMotorList.push_back(stepmotor); + } + readManipulator.stream = new uint8_t[readManipulator.degrees*3*3]; + writeManipulator.stream = new uint8_t[readManipulator.degrees*3*3]; + udpHandle->setMessageValueSize(msg, degrees*3*3); + + //下层设置 + msg.start = (uint8_t)'$'; + msg.end = (uint8_t)'\n'; +} + +bool ManipulatorProtocol::read() +{ + msg.start = (uint8_t)'$'; + msg.end = (uint8_t)'\n'; + if (udpHandle->read(msg)) + { + memcpy(readManipulator.stream, msg.value, msg.valueSize); + for (size_t i = 0; i < readManipulator.degrees; i ++) + { + readManipulator.stepMotorList[i].position = ((readManipulator.stream[i*3*3+1] << 8 ) | readManipulator.stream[i*3*3+2]) & 0xffff; + readManipulator.stepMotorList[i].velocity = ((readManipulator.stream[i*3*3+4] << 8 ) | readManipulator.stream[i*3*3+5]) & 0xffff; + readManipulator.stepMotorList[i].effort = ((readManipulator.stream[i*3*3+7] << 8 ) | readManipulator.stream[i*3*3+8]) & 0xffff; + + readManipulator.stream[i*3*3+0] == 0xf0 ? readManipulator.stepMotorList[i].position *= -1 : readManipulator.stepMotorList[i].position *= 1; + readManipulator.stream[i*3*3+3] == 0xf0 ? readManipulator.stepMotorList[i].velocity *= -1 : readManipulator.stepMotorList[i].velocity *= 1; + readManipulator.stream[i*3*3+6] == 0xf0 ? readManipulator.stepMotorList[i].effort *= -1 : readManipulator.stepMotorList[i].effort *= 1; + } + return true; + } + return false; +} + + +int ManipulatorProtocol::write() +{ + msg.start = (uint8_t)'$'; + msg.end = (uint8_t)'\n'; + + for (size_t i = 0; i < writeManipulator.degrees; i ++) + { + writeManipulator.stream[i*3*3+1] = (abs(writeManipulator.stepMotorList[i].position) >> 8) & 0xff; + writeManipulator.stream[i*3*3+2] = abs(writeManipulator.stepMotorList[i].position) & 0xff; + writeManipulator.stream[i*3*3+4] = (abs(writeManipulator.stepMotorList[i].velocity) >> 8) & 0xff; + writeManipulator.stream[i*3*3+5] = abs(writeManipulator.stepMotorList[i].velocity) & 0xff; + writeManipulator.stream[i*3*3+7] = (abs(writeManipulator.stepMotorList[i].effort) >> 8) & 0xff; + writeManipulator.stream[i*3*3+8] = abs(writeManipulator.stepMotorList[i].effort) & 0xff; + + writeManipulator.stepMotorList[i].position < 0 ? writeManipulator.stream[i*3*3+0] = 0xf0 : writeManipulator.stream[i*3*3+0] = 0x0f; + writeManipulator.stepMotorList[i].velocity < 0 ? writeManipulator.stream[i*3*3+3] = 0xf0 : writeManipulator.stream[i*3*3+3] = 0x0f; + writeManipulator.stepMotorList[i].effort < 0 ? writeManipulator.stream[i*3*3+6] = 0xf0 : writeManipulator.stream[i*3*3+6] = 0x0f; + } + memcpy(msg.value, writeManipulator.stream, msg.valueSize); + /* + for(int i = 0 ; i < msg.valueSize ; i ++) + { + if(writeManipulator.stream[i] >= 32 && writeManipulator.stream[i] <= 126) + { + cout << (char)writeManipulator.stream[i] << " "; + } + else + { + cout << "0x" << hex <<(int)writeManipulator.stream[i] << " "; + } + } + cout << "\n" << dec; + */ + return udpHandle->write(msg); +} diff --git a/Leaf_robot_ros/src/leaf_driver/src/UDP.cpp b/Leaf_robot_ros/src/leaf_driver/src/UDP.cpp new file mode 100644 index 0000000..033d8ef --- /dev/null +++ b/Leaf_robot_ros/src/leaf_driver/src/UDP.cpp @@ -0,0 +1,268 @@ +#include "leaf_driver/UDP.h" + +UDP::UDP() +{ + setBufferSize(8192); + udata = new uint8_t [1]; + recieveStatus = 1; +} + +UDP::~UDP() +{ + serialPort->close(); + //delete []serialPort; + serialPort = NULL; + delete []udata; + udata = NULL; + delete []txBuffer; + txBuffer = NULL; + delete []rxBuffer; + rxBuffer = NULL; +} + +//设置串口 +void UDP::setSerialPort( string serialport_name, int baudrate ) +{ + //串口初始化 + Timeout timeout = Timeout::simpleTimeout(2000); + try + { + serialPort = new Serial(serialport_name, baudrate, timeout); + //serialPort->open(); + + } + catch (exception e) + { + cout << "SerialPort Opened failed" << endl; + exit(0); + } +} + + +void UDP::setMessageValueSize(message& msg, int size) +{ + msg.valueSize = size; + msg.value = new uint8_t[msg.valueSize]; +} + +void UDP::setStart(message& msg, uint8_t signal_) +{ + msg.start = signal_; +} + +void UDP::setCrc(message& msg) +{ + int sum = 0; + sum += msg.start; + sum += msg.end; + for(int i = 0 ; i < msg.valueSize ; i ++) + { + sum += msg.value[i]; + } + msg.crc = (uint8_t)(sum& 0xff); +} + +void UDP::setEnd(message& msg, uint8_t signal_) +{ + msg.end = signal_; +} + +int UDP::getMessageValueSize(message& msg) const +{ + return msg.valueSize; +} + +uint8_t UDP::getStart(message& msg) const +{ + return msg.start; +} + +uint8_t UDP::getCrc(message& msg) const +{ + return msg.crc; +} + +uint8_t UDP::getEnd(message& msg) const +{ + return msg.end; +} + +void UDP::setBufferSize(int size) +{ + bufferSize = size; + txBuffer = new uint8_t[bufferSize]; + rxBuffer = new uint8_t[bufferSize]; +} + +int UDP::getBufferSize() const +{ + return bufferSize; +} + +int UDP::write(message& msg) +{ + //转换到缓存中 + msg2udp(msg); + //写入操作 + return serialPort->write(txBuffer, 3 + msg.valueSize);; +} + +bool UDP::read(message& msg) +{ + if(simpleRecieve(msg.start, msg.end, 3 + msg.valueSize)) + { + udp2msg(msg); + int tempcrc = msg.start + msg.end; + uint8_t crc = 0; + for (size_t i = 0; i < msg.valueSize; i++) + { + tempcrc += msg.value[i]; + } + crc = (uint8_t)(tempcrc & 0xff); + if (crc == msg.crc) + { + //cout << "read success\n"; + return true; + } + } + //printBuffer(rxBuffer, 3 + msg.valueSize); + + //校验位出错 + return false; +} + +void UDP::flushTxBuffer() +{ + txIndex = 0; + for(int i = 0 ; i < bufferSize ; i ++) + { + txBuffer[i] = 0x00; + } +} + +void UDP::flushRxBuffer() +{ + rxIndex = 0; + for(int i = 0 ; i < bufferSize ; i ++) + { + rxBuffer[i] = 0x00; + } +} + +void UDP::flush() +{ + txIndex = 0; + rxIndex = 0; + for(int i = 0 ; i < bufferSize ; i ++) + { + txBuffer[i] = 0x00; + rxBuffer[i] = 0x00; + } +} + +void UDP::printBuffer(uint8_t* buf, int size) +{ + for(int i = 0 ; i < size ; i ++) + { + if(buf[i] >= 32 && buf[i] <= 126) + { + cout << (char)buf[i] << " "; + } + else + { + cout << "0x" << hex <<(int)buf[i] << " "; + } + } + cout << "\n" << dec; +} + +void UDP::msg2udp(message& msg) +{ + flushTxBuffer(); + txBuffer[0] = msg.start; + memcpy(txBuffer + 1, msg.value, msg.valueSize); + txBuffer[ 2 + msg.valueSize] = msg.end; + setCrc(msg); + txBuffer[ 1 + msg.valueSize] = msg.crc; + //printBuffer(txBuffer, 3 + msg.valueSize); +} + +void UDP::udp2msg(message& msg) +{ + msg.start = rxBuffer[0]; + memcpy(msg.value, rxBuffer + 1, msg.valueSize); + msg.crc = rxBuffer[ 1 + msg.valueSize]; + msg.end = rxBuffer[ 2 + msg.valueSize]; + flushRxBuffer(); +} + +int UDP::recieve(uint8_t startSignal, uint8_t endSignal, int size) +{ + flushRxBuffer(); + //cout << "size " << size << endl; + //cout << "startSignal " << (char)startSignal << ", endSignal " << endSignal << endl; + if(recieveStatus == 0) + { + //通讯状态良好，整体接收 + serialPort->read(rxBuffer, size); + if(rxBuffer[0] == startSignal && rxBuffer[size - 1] == endSignal) + { + recieveStatus = 0; + } + else + { + recieveStatus = 1; + } + } + else + { + //通讯状态不好，逐个接收 + while( rxIndex < size) + { + if(serialPort->available() <= 0) + { + return -1; + } + serialPort->read(udata, 1); + rxBuffer[rxIndex] = udata[0]; + cout << rxIndex << " " << (char)udata[0] << endl; + rxIndex += 1; + if (rxBuffer[0] != startSignal) + { + cout << "no right start :" <<(char)startSignal << endl; + flushRxBuffer(); + } + if(rxIndex > size - 1) + { + if( rxBuffer[size - 1] != endSignal) + { + cout << "no right end :" <<(char)endSignal << endl; + flushRxBuffer(); + } + else + { + recieveStatus = 0; + break; + } + } + } + } + return 0; +} + +bool UDP::simpleRecieve(uint8_t startSignal, uint8_t endSignal, int size) +{ + flushRxBuffer(); + if(serialPort->available() >= size) + { + serialPort->read(rxBuffer, serialPort->available()); + if(rxBuffer[0] == startSignal && rxBuffer[size - 1] == endSignal) + { + return true; + } + } + //cout << "\nsimpleRecieve() error\n"; + //serialPort->flushInput(); + + return false; +} \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_driver/src/main.cpp b/Leaf_robot_ros/src/leaf_driver/src/main.cpp new file mode 100644 index 0000000..786e3a6 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_driver/src/main.cpp @@ -0,0 +1,31 @@ +#include "leaf_driver/LeafRobot.h" + + + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "leaf_node"); + LeafRobot robot("leaf_controller/follow_joint_trajectory"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + ros::Time time_current = ros::Time::now(); + ros::Time time_prev = time_current; + double elapsed = 0; + while (ros::ok()) + { + time_current = ros::Time::now(); + elapsed = (time_current - time_prev).toSec(); + if(elapsed >= 0.1) + { + time_prev = time_current; + robot.jointStateUpdate(); + } + usleep(5000); + } + cout << "机械臂程序正在关闭..." << endl; + robot.home(); + robot.write(); + usleep(2000000);//等待2s + return 0; +} diff --git "a/Leaf_robot_ros/src/leaf_driver/src/\345\217\202\346\225\260" "b/Leaf_robot_ros/src/leaf_driver/src/\345\217\202\346\225\260" new file mode 100644 index 0000000..6b48de9 --- /dev/null +++ "b/Leaf_robot_ros/src/leaf_driver/src/\345\217\202\346\225\260" @@ -0,0 +1,8 @@ + MCP->expectStepMotorList[0].position += 0; //-7800 90° + MCP->expectStepMotorList[1].position += 0; //-6350 90° + MCP->expectStepMotorList[2].position += 0; //-8000 90° + MCP->expectStepMotorList[3].position += 0; //-4550 90 + MCP->expectStepMotorList[4].position += 0; //-3400 90 + MCP->expectStepMotorList[5].position += 3200; //-3200 90 + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/CMakeLists.txt b/Leaf_robot_ros/src/leaf_moveit_config/CMakeLists.txt new file mode 100644 index 0000000..a039fe7 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 2.8.3) +project(leaf_moveit_config) + +find_package(catkin REQUIRED) + +catkin_package() + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/Leaf_robot_ros/src/leaf_moveit_config/config/chomp_planning.yaml b/Leaf_robot_ros/src/leaf_moveit_config/config/chomp_planning.yaml new file mode 100644 index 0000000..75258e5 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/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/Leaf_robot_ros/src/leaf_moveit_config/config/fake_controllers.yaml b/Leaf_robot_ros/src/leaf_moveit_config/config/fake_controllers.yaml new file mode 100644 index 0000000..02d1486 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/config/fake_controllers.yaml @@ -0,0 +1,9 @@ +controller_list: + - name: fake_manipulator_controller + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_moveit_config/config/joint_limits.yaml b/Leaf_robot_ros/src/leaf_moveit_config/config/joint_limits.yaml new file mode 100644 index 0000000..d79a2ff --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/config/joint_limits.yaml @@ -0,0 +1,34 @@ +# 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: + has_velocity_limits: true + max_velocity: 0.02 + has_acceleration_limits: false + max_acceleration: 0 + joint2: + has_velocity_limits: true + max_velocity: 0.2 + has_acceleration_limits: false + max_acceleration: 0 + joint3: + has_velocity_limits: true + max_velocity: 0.2 + has_acceleration_limits: false + max_acceleration: 0 + joint4: + has_velocity_limits: true + max_velocity: 0.2 + has_acceleration_limits: false + max_acceleration: 0 + joint5: + has_velocity_limits: true + max_velocity: 0.2 + has_acceleration_limits: false + max_acceleration: 0 + joint6: + has_velocity_limits: true + max_velocity: 0.2 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_moveit_config/config/kinematics.yaml b/Leaf_robot_ros/src/leaf_moveit_config/config/kinematics.yaml new file mode 100644 index 0000000..d60c9cd --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/config/kinematics.yaml @@ -0,0 +1,5 @@ +manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.05 + kinematics_solver_attempts: 5 \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_moveit_config/config/leaf.srdf b/Leaf_robot_ros/src/leaf_moveit_config/config/leaf.srdf new file mode 100644 index 0000000..6cddedf --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/config/leaf.srdf @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/config/ompl_planning.yaml b/Leaf_robot_ros/src/leaf_moveit_config/config/ompl_planning.yaml new file mode 100644 index 0000000..5044d2d --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/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 +manipulator: + default_planner_config: RRT + 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(joint1,joint2) + longest_valid_segment_fraction: 0.005 \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_moveit_config/config/ros_controllers.yaml b/Leaf_robot_ros/src/leaf_moveit_config/config/ros_controllers.yaml new file mode 100644 index 0000000..1037f32 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,26 @@ +leaf: +# MoveIt-specific simulation settings + moveit_sim_hw_interface: + joint_model_group: controllers_initial_group_ + joint_model_group_pose: controllers_initial_pose_ +# Settings for ros_control control loop + generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface + hardware_interface: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - 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/Leaf_robot_ros/src/leaf_moveit_config/config/sensors_3d.yaml b/Leaf_robot_ros/src/leaf_moveit_config/config/sensors_3d.yaml new file mode 100644 index 0000000..d2955dc --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/config/sensors_3d.yaml @@ -0,0 +1,3 @@ +# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it +sensors: + - {} \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/chomp_planning_pipeline.launch.xml b/Leaf_robot_ros/src/leaf_moveit_config/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..799ce1a --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/default_warehouse_db.launch b/Leaf_robot_ros/src/leaf_moveit_config/launch/default_warehouse_db.launch new file mode 100644 index 0000000..5c0ae14 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/demo.launch b/Leaf_robot_ros/src/leaf_moveit_config/launch/demo.launch new file mode 100644 index 0000000..6befdbd --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/demo.launch @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/Leaf_robot_ros/src/leaf_moveit_config/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..a11d754 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/joystick_control.launch b/Leaf_robot_ros/src/leaf_moveit_config/launch/joystick_control.launch new file mode 100644 index 0000000..9411f6e --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/leaf_moveit_controller_manager.launch.xml b/Leaf_robot_ros/src/leaf_moveit_config/launch/leaf_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..5cbf822 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/leaf_moveit_controller_manager.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/leaf_moveit_sensor_manager.launch.xml b/Leaf_robot_ros/src/leaf_moveit_config/launch/leaf_moveit_sensor_manager.launch.xml new file mode 100644 index 0000000..5d02698 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/leaf_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/move_group.launch b/Leaf_robot_ros/src/leaf_moveit_config/launch/move_group.launch new file mode 100644 index 0000000..1a5525c --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/move_group.launch @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/moveit.rviz b/Leaf_robot_ros/src/leaf_moveit_config/launch/moveit.rviz new file mode 100644 index 0000000..5567dbe --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/moveit.rviz @@ -0,0 +1,321 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.742560029 + Tree Height: 421 + - 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 + 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 + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true + 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: manipulator + Query Goal State: true + Query Start State: false + 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 + 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 + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 1 + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + 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 + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.1 s + Trail Step Size: 1 + Trajectory Topic: /move_group/display_planned_path + Value: false + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + 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 + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: link1 + 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: 0.893309653 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.0570338815 + Y: 0.0655728579 + Z: 2.83122631e-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.340202451 + Target Frame: link1 + Value: XYOrbit (rviz) + Yaw: 0.63180238 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002ad000003dafc0200000009fb000000100044006900730070006c00610079007301000000280000023a000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000002680000019a0000018300fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004a00ffffff000004cd000003da00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 24 diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/moveit_rviz.launch b/Leaf_robot_ros/src/leaf_moveit_config/launch/moveit_rviz.launch new file mode 100644 index 0000000..d6ddb6f --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/ompl_planning_pipeline.launch.xml b/Leaf_robot_ros/src/leaf_moveit_config/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 0000000..af24df7 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/planning_context.launch b/Leaf_robot_ros/src/leaf_moveit_config/launch/planning_context.launch new file mode 100644 index 0000000..0b05eb9 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/planning_context.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/planning_pipeline.launch.xml b/Leaf_robot_ros/src/leaf_moveit_config/launch/planning_pipeline.launch.xml new file mode 100644 index 0000000..2de1954 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/ros_controllers.launch b/Leaf_robot_ros/src/leaf_moveit_config/launch/ros_controllers.launch new file mode 100644 index 0000000..52cf8b9 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/ros_controllers.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/run_benchmark_ompl.launch b/Leaf_robot_ros/src/leaf_moveit_config/launch/run_benchmark_ompl.launch new file mode 100644 index 0000000..50b1e4e --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/run_benchmark_ompl.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/sensor_manager.launch.xml b/Leaf_robot_ros/src/leaf_moveit_config/launch/sensor_manager.launch.xml new file mode 100644 index 0000000..3e55b9d --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/setup_assistant.launch b/Leaf_robot_ros/src/leaf_moveit_config/launch/setup_assistant.launch new file mode 100644 index 0000000..dd78baa --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/setup_assistant.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/trajectory_execution.launch.xml b/Leaf_robot_ros/src/leaf_moveit_config/launch/trajectory_execution.launch.xml new file mode 100644 index 0000000..bca95b0 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/trajectory_execution.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/warehouse.launch b/Leaf_robot_ros/src/leaf_moveit_config/launch/warehouse.launch new file mode 100644 index 0000000..bc71e97 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/launch/warehouse_settings.launch.xml b/Leaf_robot_ros/src/leaf_moveit_config/launch/warehouse_settings.launch.xml new file mode 100644 index 0000000..e473b08 --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Leaf_robot_ros/src/leaf_moveit_config/package.xml b/Leaf_robot_ros/src/leaf_moveit_config/package.xml new file mode 100644 index 0000000..2f141de --- /dev/null +++ b/Leaf_robot_ros/src/leaf_moveit_config/package.xml @@ -0,0 +1,34 @@ + + + leaf_moveit_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the leaf with the MoveIt! Motion Planning Framework + + Wu Xueming + Wu Xueming + + 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 + + + leaf_description + leaf_description + + + diff --git a/Simple_MoveIt_Demo.zip b/Simple_MoveIt_Demo.zip new file mode 100644 index 0000000..6dcefe7 Binary files /dev/null and b/Simple_MoveIt_Demo.zip differ