diff --git a/LICENSE b/LICENSE new file mode 100755 index 00000000..a9c5e843 --- /dev/null +++ b/LICENSE @@ -0,0 +1,27 @@ +Copyright (c) 2018, UFACTORY Inc. + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/ReadMe.md b/ReadMe.md index 9df325fe..4d5d0cc1 100755 --- a/ReadMe.md +++ b/ReadMe.md @@ -1,11 +1,16 @@ # 1. Introduction -   This repository contains the 3D model of xArm and demo packages for ROS development and simulations. - Maintained by: Jimy (jimy.zhang@ufactory.cc) and Jason (jason@ufactory.cc) +   This repository contains the 3D model of xArm and demo packages for ROS development and simulations.Developing and testing environment: Ubuntu 16.04 + ROS Kinetic Kame. + Maintained by: Jason (jason@ufactory.cc) and Jimy (jimy.zhang@ufactory.cc) + ***Instructions below is based on xArm7, other model user can replace 'xarm7' with 'xarm6' or 'xarm5' where applicable.*** + For simplified Chinese instructions: [简体中文版](./ReadMe_cn.md) # 2. Update Summary - This is the initial version, tests, bug fixes and new functions are to be updated regularly in the future. - * Add xArm 7 description files, meshes and sample controller demos for ROS simulation and visualization ONLY. - * Direct control of real xArm is not yet available. + This package is still in early development, tests, bug fixes and new functions are to be updated regularly in the future. + * Add xArm 7 description files, meshes and sample controller demos for ROS simulation and visualization. + * Add Moveit! planner support to control Gazebo virtual model and real xArm, but the two can not launch together. + * Direct control of real xArm through Moveit GUI is still in beta version, please use it with special care. + * Add xArm hardware interface to use ROS position_controllers/JointTrajectoryController on real robot. + * Add initial xArm 6 simulation support. # 3. Preparations before using this package @@ -17,9 +22,10 @@ ROS Wiki: Gazebo Tutorial: Gazebo ROS Control: +Moveit tutorial: ## 3.3 Download the 'table' 3D model -  In Gazebo simulator, navigate through the model database for 'table' item, drag and place the 3D model inside the virtual environment. It will then be downloaded locally, as 'table' is needed running the demo. +  In Gazebo simulator, navigate through the model database for 'table' item, drag and place the 3D model inside the virtual environment. It will then be downloaded locally, as 'table' is needed for running the demo. # 4. Usage of ROS package 'xarm_ros' @@ -47,14 +53,18 @@ Skip above operation if you already have that inside your ~/.bashrc. Then do: ```bash $ source ~/.bashrc ``` +## 4.5 First try out in RViz: +```bash +$ roslaunch xarm_description xarm7_rviz_display.launch +``` -## 4.5 Run the demo in Gazebo simulator +## 4.6 Run the demo in Gazebo simulator ```bash - $ roslaunch xarm_gazebo xarm7_beside_table.launch + $ roslaunch xarm_gazebo xarm7_beside_table.launch [run_demo:=true] ``` -  Motion will start after clicking on "play" button. The command trajectory is written in xarm_controller\src\sample_motion.cpp. And the arm in this demo is controlled by pure position interface. +  Add the run_demo option if you wish to see a pre-programed loop motion in action. The command trajectory is written in xarm_controller\src\sample_motion.cpp. And the trajectory in this demo is controlled by pure position interface. -# 5. Package structure +# 5. Package description & Usage Guidance ## 5.1 xarm_description   xArm7 description files, mesh files and gazebo plugin configurations, etc. It's not recommended to change the xarm description file since other packages depend on it. @@ -63,30 +73,88 @@ $ source ~/.bashrc   Gazebo world description files and simulation launch files. User can add or build their own models in the simulation world file. ## 5.3 xarm_controller -   Controller configurations, robot command executable source, scripts and launch files. User can deploy their program inside this package or create their own. - -### 5.3.1 xarm_controller/config - Controller parameters to load into server, there are three basic types of controllers: - 1) joint_state_controller/JointStateController: controller that publshes joint status, for Rviz or feedback. - 2) effort_controllers/JointPositionController: position controller that has joint effort (torque) interface. - 3) effort_controllers/JointEffortController: pure joint effort controller. - 4) position_controllers/JointPositionController: pure position controller with only joint position interface. - User can add their self-defined controllers as well, refer to: http://wiki.ros.org/ros_control (controllers) - -### 5.3.2 xarm_controller/exec -   User can put their control scripts (shell, python, etc) here and can execute with 'rosrun' after setting them as executables. - -### 5.3.3 xarm_controller/src, xarm_controller/include -   User can apply ROS API to program in C++ or python to make a program to control the virtual robot to move or monitor its status. Source files can be put here, remember to edit CMakeLists.txt before compiling. Refer to: - - - -### 5.3.4 xarm_controller/launch -  For launch files that bring about all steps for a simulation in proper order. User can refer to the file provided, load and initiate controllers from yaml configuration file and then run user application program to command robot arm to move. - -## 5.4 xarm7_moveit_config -   (Note that there may be some potential incompatibility issue with the STL format of the model, we will fix this problem soon) -   Generated by moveit_setup_assistant, could use with Moveit Planner and Rviz visualization. If you have Moveit! installed, you can try the demo. +   Controller configurations, hardware_interface, robot command executable source, scripts and launch files. User can deploy their program inside this package or create their own. ***Note that*** effort controllers defined in xarm_controller/config are just examples for simulation purpose, when controlling the real arm, only 'position_controllers/JointTrajectoryController' interface is provided. User can add their self-defined controllers as well, refer to: http://wiki.ros.org/ros_control (controllers) + +## 5.4 xarm_bringup +  launch files to load xarm driver to enable direct control of real xArm hardware. + +## 5.5 xarm7_moveit_config +  Partially generated by moveit_setup_assistant, could use with Moveit Planner and Rviz visualization. If you have Moveit! installed, you can try the demo. ```bash $ roslaunch xarm7_moveit_config demo.launch ``` +#### To run Moveit! motion planner along with Gazebo simulator: + First run: + ```bash + $ roslaunch xarm_gazebo xarm7_beside_table.launch + ``` + Then in another terminal: + ```bash + $ roslaunch xarm7_moveit_config xarm7_moveit_gazebo.launch + ``` + If you have a satisfied motion planned in Moveit!, hit the "Execute" button and the virtual arm in Gazebo will execute the trajectory. + +#### To run Moveit! motion planner to control the real xArm: + First make sure the xArm and the controller box are powered on, then execute: + ```bash + $ roslaunch xarm7_moveit_config realMove_exec.launch robot_ip:= + ``` + Examine the terminal output and see if any error occured during the launch. If not, just play with the robot in Rviz and you can execute the sucessfully planned trajectory on real arm. But be sure it will not hit any surroundings before execution! + +## 5.6 xarm_planner: +  This implemented simple planner interface is based on move_group from Moveit! and provide ros service for users to do planning & execution based on the requested target, user can find detailed instructions on how to use it inside [***xarm_planner package***](./xarm_planner/). +#### To launch the xarm simple motion planner together with the real xArm: +```bash + $ roslaunch xarm_planner xarm_planner_realHW.launch robot_ip:= robot_dof:=<7/6/5> +``` +Argument 'robot_dof' specifies the number of joints of your xArm (default is 7). + +## 5.7 xarm_api/xarm_msgs: +  These two packages provide user with the ros services to control xArm without self-trajectory planning (through Moveit! or xarm_planner), the controller box computer will do the planning work. Note that these services ***does not*** use xarm hardware interface, which is for Moveit and 'JointTrajectoryController' interface. There are three types of motion command (service names) supported: +* move_joint: joint space point to point command, given target joint angles, max joint velocity and acceleration. +* move_line: straight-line motion to the specified Cartesian Tool Centre Point(TCP) target. +* move_lineb: a list of via points followed by target Cartesian point. Each segment is straight-line with Arc blending at the via points, to make velocity continuous. +Please ***keep in mind that*** before calling the three motion services above, first set robot mode to be 0, then set robot state to be 0, by calling relavent services. Meaning of the commands are consistent with the descriptions in product ***user manual***, other xarm API supported functions are also available as service call. Refer to [xarm_msgs package](./xarm_msgs/) for more details and usage guidance. + +#### Example of using API service calls: + +  First startup the service server for xarm7, ip address is just an example: +```bash +$ roslaunch xarm_bringup xarm7_server.launch robot_ip:=192.168.1.128 +``` +  Then make sure all the servo motors are enabled, refer to [SetAxis.srv](/xarm_msgs/srv/SetAxis.srv): +```bash +$ rosservice call /xarm/motion_ctrl 8 1 +``` +  Before any motion commands, set proper robot mode(0: POSE) and state(0: READY) ***in order***, refer to [SetInt16.srv](/xarm_msgs/srv/SetInt16.srv): +```bash +$ rosservice call /xarm/set_mode 0 + +$ rosservice call /xarm/set_state 0 +``` +  All motion commands use the same type of srv request: [Move.srv](./xarm_msgs/srv/Move.srv). For example, to call joint space motion with max speed 0.35 rad/s and acceleration 7 rad/s^2: +```bash +$ rosservice call /xarm/move_joint [0,0,0,0,0,0,0] 0.35 7 0 0 +``` +  To call Cartesian spece motion with max speed 200 mm/s and acceleration 2000 mm/s^2: +```bash +$ rosservice call /xarm/move_line [250,100,300,3.14,0,0] 200 2000 0 0 +``` +  To go back to home (all joints at 0 rad) position with max speed 0.35 rad/s and acceleration 7 rad/s^2: +```bash +$ rosservice call /xarm/go_home [] 0.35 7 0 0 +``` + +#### Getting status feedback: +  Having connected with a real xArm robot by running 'xarm7_server.launch', user can subscribe to the topic ***"/xarm_status"*** for feedback information about current robot states, including joint angles, TCP position, error/warning code, etc. Refer to [RobotMsg.msg](./xarm_msgs/msg/RobotMsg.msg) for content details. +  Another option is subscribing to ***"/joint_states"*** topic, which is reporting in [JointState.msg](http://docs.ros.org/jade/api/sensor_msgs/html/msg/JointState.html), however, currently ***only "position" field is valid***; "velocity" is non-filtered numerical differentiation based on 2 adjacent position data, so it is just for reference; and we do not provide "effort" feedback yet. +  In consideration of performance, current update rate of above two topics are set at ***10Hz***. + +#### Setting Tool Center Point Offset: +  The tool tip point offset values can be set by calling service "/xarm/set_tcp_offset". Refer to the figure below, please note this offset coordinate is expressed with respect to ***initial tool frame*** (Frame B), which is located at flange center, with roll, pitch, yaw rotations of (PI, 0, 0) from base frame (Frame A). +![xArmFrames](./doc/xArmFrames.png) +  For example: +```bash +$ rosservice call /xarm/set_tcp_offset 0 0 20 0 0 0 +``` +  This is to set tool frame position offset (x = 0 mm, y = 0 mm, z = 20 mm), and orientation (RPY) offset of ( 0, 0, 0 ) radians with respect to initial tool frame (Frame B in picture). ***Remember to set this offset each time the controller box is restarted !*** \ No newline at end of file diff --git a/ReadMe_cn.md b/ReadMe_cn.md new file mode 100755 index 00000000..7cb7f54b --- /dev/null +++ b/ReadMe_cn.md @@ -0,0 +1,160 @@ +# 1. 简介： +   此代码库包含XArm模型文件以及相关的控制、规划等示例开发包。开发及测试使用的环境为 Ubuntu 16.04 + ROS Kinetic Kame。 + 维护者: Jason (jason@ufactory.cc),Jimy (jimy.zhang@ufactory.cc) + ***以下的指令说明是基于xArm7, 其他型号用户可以在对应位置将'xarm7'替换成'xarm6'或'xarm5'*** + +# 2. 更新记录： + 此代码库仍然处在早期开发阶段，新的功能支持、示例代码，bug修复等等会保持更新。 + * 添加Xarm 7 描述文档，3D图形文件以及controller示例，用于进行ROS可视化仿真模拟。 + * 添加MoveIt!规划器支持，用于控制Gazebo/RViz模型或者XArm真机，但二者不可同时启动。 + * 由ROS直接控制XArm真机的相关支持目前还是Beta版本，用户使用时应尽量小心，我们会尽快完善。 + * 添加 xArm hardware interface 并在驱动真实机械臂时使用 ROS position_controllers/JointTrajectoryController。 + * 添加 xArm 6 初版仿真支持。 + +# 3. 准备工作 + +## 3.1 安装 gazebo_ros interface 模块 + gazebo_ros_pkgs: + ros_control: (记得选择您使用的 ROS 版本) + +## 3.2 完整学习相关的官方教程 +ROS Wiki: +Gazebo Tutorial: +Gazebo ROS Control: +Moveit tutorial: + +## 3.3 如果使用Gazebo: 请提前下载好 'table' 3D 模型 +  这个模型在Gazebo demo中会用到。在Gazebo仿真环境中, 在model database列表里寻找 'table', 并将此模型拖入旁边的3D环境中. 通过这个操作，桌子的模型就会自动下载到本地。 + +# 4. 'xarm_ros'的使用教程 + +## 4.1 生成catkin workspace. +   如果您已经有了自己的catkin工作区，请跳过此步往下进行。 + 按照[这里](http://wiki.ros.org/catkin/Tutorials/create_a_workspace)的教程生成catkin_ws。 + 请留意本文档已假设用户继续沿用 '~/catkin_ws' 作为默认的catkin工作区地址。 + +## 4.2 获取代码包 + ```bash + $ cd ~/catkin_ws/src + $ git clone https://github.com/xArm-Developer/xarm_ros.git + ``` + +## 4.3 编译代码 + ```bash + $ cd ~/catkin_ws + $ catkin_make + ``` +## 4.4 执行配置脚本 +```bash +$ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc +``` +如果在您的 ~/.bashrc中已经有了以上语句，直接运行: +```bash +$ source ~/.bashrc +``` +## 4.5 在RViz环境试用: +```bash +$ roslaunch xarm_description xarm7_rviz_display.launch +``` +## 4.6 如果已安装Gazebo,可以执行demo查看效果 + ```bash + $ roslaunch xarm_gazebo xarm7_beside_table.launch [run_demo:=true] + ``` +  指定'run_demo'为true时Gazebo环境启动后机械臂会自动执行一套编好的循环动作。 这套简单的command trajectory写在xarm_controller\src\sample_motion.cpp. 这个demo加载的控制器使用position interface（纯位置控制）。 + +# 5. 代码库介绍及使用说明 + +## 5.1 xarm_description +   包含xArm描述文档, mesh文件和gazebo plugin配置等等。 不推荐用户去修改urdf描述因为其他ros package对其都有依赖。 + +## 5.2 xarm_gazebo +   Gazebo world 描述文档以及仿真launch启动文档。用户可以在world中修改添加自己需要的模型与环境。 + +## 5.3 xarm_controller +   xarm使用的Controller配置, 硬件接口，轨迹指令源文件, 脚本以及launch文件。 用户可以基于这个包开发或者使用自己的package。***注意*** xarm_controller/config里面定义好的position/effort控制器仅用作仿真的示例, 当控制真实机械臂时只提供position_controllers/JointTrajectoryController接口。用户可以根据需要添加自己的controller, 参考: http://wiki.ros.org/ros_control (controllers) + +## 5.4 xarm_bringup +  内含加载xarm driver的启动文件，用来控制真实机械臂。 + +## 5.5 xarm7_moveit_config +   + 部分文档由moveit_setup_assistant自动生成, 用于Moveit Planner和Rviz可视化仿真。如果已安装MoveIt!,可以尝试跑demo： + ```bash + $ roslaunch xarm7_moveit_config demo.launch + ``` + +#### Moveit!图形控制界面 + Gazebo 仿真环境: + 首先执行: + ```bash + $ roslaunch xarm_gazebo xarm7_beside_table.launch + ``` + 然后在另一个终端运行: + ```bash + $ roslaunch xarm7_moveit_config xarm7_moveit_gazebo.launch + ``` + 如果您在Moveit界面中规划了一条满意的轨迹, 点按"Execute"会使Gazebo中的虚拟机械臂同步执行此轨迹。 + +#### Moveit!图形控制界面 + xArm 真实机械臂: + 首先, 检查并确认xArm电源和控制器已上电开启, 然后运行: + ```bash + $ roslaunch xarm7_moveit_config realMove_exec.launch robot_ip:=<控制盒的局域网IP地址> + ``` + 检查terminal中的输出看看有无错误信息。如果启动无误，您可以将RViz中通过Moveit规划好的轨迹通过'Execute'按钮下发给机械臂执行。***但一定确保它不会与周围环境发生碰撞！*** + + +## 5.6 xarm_planner: +这个简单包装实现的规划器接口是基于 Moveit!中的 move_group interface, 可以使用户通过service指定目标位置进行规划和执行。 这部分的详细使用方法请阅读[xarm_planner包](./xarm_planner)的文档。 +#### 启动 xarm simple motion planner 控制 xArm 真实机械臂: +```bash + $ roslaunch xarm_planner xarm_planner_realHW.launch robot_ip:=<控制盒的局域网IP地址> robot_dof:=<7/6/5> +``` +'robot_dof'参数指的是xArm的关节数目 (默认值为7)。 + +## 5.7 xarm_api/xarm_msgs: +  这两个package提供给用户不需要自己进行轨迹规划(通过Moveit!或xarm_planner)就可以控制真实xArm机械臂的ros服务, xarm自带的控制盒会进行轨迹规划。 请 ***注意*** 这些service的执行并不通过面向'JointTrajectoryController'的hardware interface。当前支持三种运动命令（ros service同名）: +* move_joint: 关节空间的点到点运动, 用户仅需要给定目标关节位置，运动过程最大关节速度/加速度即可。 +* move_line: 笛卡尔空间的直线轨迹运动，用户需要给定工具中心点（TCP）目标位置以及笛卡尔速度、加速度。 +* move_lineb: 圆弧交融的直线运动，给定一系列中间点以及目标位置。 每两个中间点间为直线轨迹，但在中间点处做一个圆弧过渡（需给定半径）来保证速度连续。 +另外需要 ***注意*** 的是，使用以上三种service之前，需要通过service依次将机械臂模式(mode)设置为0，然后状态(state)设置为0。这些运动指令的意义和详情可以参考产品使用指南。除此之外还提供了其他xarm编程API支持的service调用, 对于相关ros service的定义在 [xarm_msgs目录](./xarm_msgs/)中。 + +#### 使用 API service call 的示例: + +  首先启动xarm7 service server, 以下ip地址只是举例: +```bash +$ roslaunch xarm_bringup xarm7_server.launch robot_ip:=192.168.1.128 +``` +  然后确保每个关节的控制已经使能, 参考[SetAxis.srv](/xarm_msgs/srv/SetAxis.srv): +```bash +$ rosservice call /xarm/motion_ctrl 8 1 +``` +  在传递任何运动指令前，先***依次***设置正确的机械臂模式(0: POSE)和状态(0: READY), 参考[SetInt16.srv](/xarm_msgs/srv/SetInt16.srv): +```bash +$ rosservice call /xarm/set_mode 0 + +$ rosservice call /xarm/set_state 0 +``` +  以上三个运动命令使用同类型的srv request: [Move.srv](./xarm_msgs/srv/Move.srv)。 比如，调用关节运动命令，最大速度 0.35 rad/s，加速度 7 rad/s^2: +```bash +$ rosservice call /xarm/move_joint [0,0,0,0,0,0,0] 0.35 7 0 0 +``` +  调用笛卡尔空间指令，最大线速度 200 mm/s，加速度为 2000 mm/s^2: +```bash +$ rosservice call /xarm/move_line [250,100,300,3.14,0,0] 200 2000 0 0 +``` +  调用回原点服务 (各关节回到0角度)，最大角速度 0.35 rad/s，角加速度 7 rad/s^2: +```bash +$ rosservice call /xarm/go_home [] 0.35 7 0 0 +``` +#### 获得反馈状态信息: +  如果通过运行'xarm7_server.launch'连接了一台xArm机械臂，用户可以通过订阅 ***"/xarm_status"*** topic 获得机械臂当前的各种状态信息， 包括关节角度、工具坐标点的位置、错误、警告信息等等。具体的信息列表请参考[RobotMsg.msg](./xarm_msgs/msg/RobotMsg.msg). +  另一种选择是订阅 ***"/joint_states"*** topic, 它是以[JointState.msg](http://docs.ros.org/jade/api/sensor_msgs/html/msg/JointState.html)格式发布数据的, 但是当前 ***只有 "position" 是有效数据***; "velocity" 是没有经过任何滤波的基于相邻两组位置数据进行的数值微分, 因而只能作为参考，我们目前还不提供 "effort" 的反馈数据. +  基于运行时性能考虑，目前以上两个topic的数据更新率固定为 ***10Hz***. + +#### 关于设定末端工具偏移量: +  末端工具的偏移量可以也通过'/xarm/set_tcp_offset'服务来设定,参考下图，请注意这一坐标偏移量是基于 ***原始工具坐标系*** (坐标系B)描述的，它位于末端法兰中心，并且相对基坐标系(坐标系A)有（PI, 0, 0)的RPY旋转偏移。 +![xArmFrames](./doc/xArmFrames.png) +  例如： +```bash +$ rosservice call /xarm/set_tcp_offset 0 0 20 0 0 0 +``` +  这条命令设置了基于原始工具坐标系(x = 0 mm, y = 0 mm, z = 20 mm)的位置偏移量，还有（0 rad, 0 rad, 0 rad)的RPY姿态偏移量。***如果需要请在每次重新启动/上电控制盒时设定一次正确的偏移量，因为此设定会掉电丢失。*** \ No newline at end of file diff --git a/doc/xArmFrames.png b/doc/xArmFrames.png new file mode 100644 index 00000000..5165c8eb Binary files /dev/null and b/doc/xArmFrames.png differ diff --git a/xarm6_moveit_config/.setup_assistant b/xarm6_moveit_config/.setup_assistant new file mode 100755 index 00000000..a37e4a6c --- /dev/null +++ b/xarm6_moveit_config/.setup_assistant @@ -0,0 +1,11 @@ +moveit_setup_assistant_config: + URDF: + package: xarm_description + relative_path: urdf/xarm6_robot.urdf.xacro + xacro_args: "--inorder " + SRDF: + relative_path: config/xarm6.srdf + CONFIG: + author_name: Jason Peng + author_email: jason@ufactory.cc + generated_timestamp: 1545822419 \ No newline at end of file diff --git a/xarm6_moveit_config/CMakeLists.txt b/xarm6_moveit_config/CMakeLists.txt new file mode 100755 index 00000000..38cb35e3 --- /dev/null +++ b/xarm6_moveit_config/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 2.8.3) +project(xarm6_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/xarm6_moveit_config/config/chomp_planning.yaml b/xarm6_moveit_config/config/chomp_planning.yaml new file mode 100755 index 00000000..75258e53 --- /dev/null +++ b/xarm6_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/xarm6_moveit_config/config/controllers.yaml b/xarm6_moveit_config/config/controllers.yaml new file mode 100755 index 00000000..ad5f45bf --- /dev/null +++ b/xarm6_moveit_config/config/controllers.yaml @@ -0,0 +1,12 @@ +controller_list: + - name: "" + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + diff --git a/xarm6_moveit_config/config/fake_controllers.yaml b/xarm6_moveit_config/config/fake_controllers.yaml new file mode 100755 index 00000000..a3a18c9b --- /dev/null +++ b/xarm6_moveit_config/config/fake_controllers.yaml @@ -0,0 +1,9 @@ +controller_list: + - name: fake_xarm6_controller + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 \ No newline at end of file diff --git a/xarm6_moveit_config/config/joint_limits.yaml b/xarm6_moveit_config/config/joint_limits.yaml new file mode 100755 index 00000000..7e259958 --- /dev/null +++ b/xarm6_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: 2.14 + has_acceleration_limits: false + max_acceleration: 0 + joint2: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0 + joint3: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0 + joint4: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0 + joint5: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0 + joint6: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/xarm6_moveit_config/config/kinematics.yaml b/xarm6_moveit_config/config/kinematics.yaml new file mode 100755 index 00000000..35525621 --- /dev/null +++ b/xarm6_moveit_config/config/kinematics.yaml @@ -0,0 +1,5 @@ +xarm6: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 \ No newline at end of file diff --git a/xarm6_moveit_config/config/ompl_planning.yaml b/xarm6_moveit_config/config/ompl_planning.yaml new file mode 100755 index 00000000..eded49cd --- /dev/null +++ b/xarm6_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,148 @@ +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 +xarm6: + 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 \ No newline at end of file diff --git a/xarm6_moveit_config/config/ros_controllers.yaml b/xarm6_moveit_config/config/ros_controllers.yaml new file mode 100755 index 00000000..9974d93f --- /dev/null +++ b/xarm6_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,26 @@ +xarm6: +# 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/xarm6_moveit_config/config/sensors_3d.yaml b/xarm6_moveit_config/config/sensors_3d.yaml new file mode 100755 index 00000000..d2955dcd --- /dev/null +++ b/xarm6_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/xarm6_moveit_config/config/xarm6.srdf b/xarm6_moveit_config/config/xarm6.srdf new file mode 100755 index 00000000..ceddc708 --- /dev/null +++ b/xarm6_moveit_config/config/xarm6.srdf @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/xarm6_moveit_config/config/xarm6_params.yaml b/xarm6_moveit_config/config/xarm6_params.yaml new file mode 100755 index 00000000..38703c05 --- /dev/null +++ b/xarm6_moveit_config/config/xarm6_params.yaml @@ -0,0 +1,3 @@ +xarm: + DOF: 6 + joint_names: ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6'] \ No newline at end of file diff --git a/xarm6_moveit_config/launch/chomp_planning_pipeline.launch.xml b/xarm6_moveit_config/launch/chomp_planning_pipeline.launch.xml new file mode 100755 index 00000000..ce52f8a8 --- /dev/null +++ b/xarm6_moveit_config/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/xarm6_moveit_config/launch/default_warehouse_db.launch b/xarm6_moveit_config/launch/default_warehouse_db.launch new file mode 100755 index 00000000..b0b19921 --- /dev/null +++ b/xarm6_moveit_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/xarm6_moveit_config/launch/demo.launch b/xarm6_moveit_config/launch/demo.launch new file mode 100755 index 00000000..cca12ae0 --- /dev/null +++ b/xarm6_moveit_config/launch/demo.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/xarm6_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/xarm6_moveit_config/launch/fake_moveit_controller_manager.launch.xml new file mode 100755 index 00000000..3692f548 --- /dev/null +++ b/xarm6_moveit_config/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/xarm6_moveit_config/launch/joystick_control.launch b/xarm6_moveit_config/launch/joystick_control.launch new file mode 100755 index 00000000..9411f6e6 --- /dev/null +++ b/xarm6_moveit_config/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/xarm6_moveit_config/launch/move_group.launch b/xarm6_moveit_config/launch/move_group.launch new file mode 100755 index 00000000..bc53d3f4 --- /dev/null +++ b/xarm6_moveit_config/launch/move_group.launch @@ -0,0 +1,75 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/xarm6_moveit_config/launch/moveit.rviz b/xarm6_moveit_config/launch/moveit.rviz new file mode 100755 index 00000000..da6e43c7 --- /dev/null +++ b/xarm6_moveit_config/launch/moveit.rviz @@ -0,0 +1,309 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - /MotionPlanning1/Planned Path1 + - /RobotModel1 + Splitter Ratio: 0.742560029 + Tree Height: 405 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +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 + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.0799999982 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: xarm6 + 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 + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 1 + - 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 + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + 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: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 1.6667558 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.113567002 + Y: 0.105920002 + Z: 2.23518001e-07 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: -0.040202342 + Target Frame: world + Value: XYOrbit (rviz) + Yaw: 5.55494833 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1028 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002ad000003befc0200000007fb000000100044006900730070006c00610079007301000000280000022a000000d700fffffffb0000000800480065006c00700000000342000000bb0000007300fffffffb0000000a0056006900650077007300000003b0000000b0000000ad00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000002580000018e0000018300ffffff00000471000003be00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1828 + X: 92 + Y: 24 diff --git a/xarm6_moveit_config/launch/moveit_rviz.launch b/xarm6_moveit_config/launch/moveit_rviz.launch new file mode 100755 index 00000000..52aa19b7 --- /dev/null +++ b/xarm6_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + diff --git a/xarm6_moveit_config/launch/moveit_rviz_common.launch b/xarm6_moveit_config/launch/moveit_rviz_common.launch new file mode 100755 index 00000000..8db20e53 --- /dev/null +++ b/xarm6_moveit_config/launch/moveit_rviz_common.launch @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + $(arg jnt_stat_source) + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/xarm6_moveit_config/launch/ompl_planning_pipeline.launch.xml b/xarm6_moveit_config/launch/ompl_planning_pipeline.launch.xml new file mode 100755 index 00000000..d273a1c3 --- /dev/null +++ b/xarm6_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + diff --git a/xarm6_moveit_config/launch/planning_context.launch b/xarm6_moveit_config/launch/planning_context.launch new file mode 100755 index 00000000..fc1ccf47 --- /dev/null +++ b/xarm6_moveit_config/launch/planning_context.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/xarm6_moveit_config/launch/planning_pipeline.launch.xml b/xarm6_moveit_config/launch/planning_pipeline.launch.xml new file mode 100755 index 00000000..bf1e6a7b --- /dev/null +++ b/xarm6_moveit_config/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/xarm6_moveit_config/launch/realMove_exec.launch b/xarm6_moveit_config/launch/realMove_exec.launch new file mode 100755 index 00000000..772e1936 --- /dev/null +++ b/xarm6_moveit_config/launch/realMove_exec.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/xarm6_moveit_config/launch/ros_controllers.launch b/xarm6_moveit_config/launch/ros_controllers.launch new file mode 100755 index 00000000..583b4dd8 --- /dev/null +++ b/xarm6_moveit_config/launch/ros_controllers.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + diff --git a/xarm6_moveit_config/launch/run_benchmark_ompl.launch b/xarm6_moveit_config/launch/run_benchmark_ompl.launch new file mode 100755 index 00000000..4066c165 --- /dev/null +++ b/xarm6_moveit_config/launch/run_benchmark_ompl.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/xarm6_moveit_config/launch/sensor_manager.launch.xml b/xarm6_moveit_config/launch/sensor_manager.launch.xml new file mode 100755 index 00000000..4e937346 --- /dev/null +++ b/xarm6_moveit_config/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/xarm6_moveit_config/launch/setup_assistant.launch b/xarm6_moveit_config/launch/setup_assistant.launch new file mode 100755 index 00000000..06464e0b --- /dev/null +++ b/xarm6_moveit_config/launch/setup_assistant.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + diff --git a/xarm6_moveit_config/launch/trajectory_execution.launch.xml b/xarm6_moveit_config/launch/trajectory_execution.launch.xml new file mode 100755 index 00000000..54c98d32 --- /dev/null +++ b/xarm6_moveit_config/launch/trajectory_execution.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/xarm6_moveit_config/launch/warehouse.launch b/xarm6_moveit_config/launch/warehouse.launch new file mode 100755 index 00000000..79d8b226 --- /dev/null +++ b/xarm6_moveit_config/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/xarm6_moveit_config/launch/warehouse_settings.launch.xml b/xarm6_moveit_config/launch/warehouse_settings.launch.xml new file mode 100755 index 00000000..e473b083 --- /dev/null +++ b/xarm6_moveit_config/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/xarm6_moveit_config/launch/xarm6_moveit_controller_manager.launch.xml b/xarm6_moveit_config/launch/xarm6_moveit_controller_manager.launch.xml new file mode 100755 index 00000000..0da7f0a1 --- /dev/null +++ b/xarm6_moveit_config/launch/xarm6_moveit_controller_manager.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/xarm6_moveit_config/launch/xarm6_moveit_gazebo.launch b/xarm6_moveit_config/launch/xarm6_moveit_gazebo.launch new file mode 100755 index 00000000..fa8f1e6a --- /dev/null +++ b/xarm6_moveit_config/launch/xarm6_moveit_gazebo.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/xarm6_moveit_config/launch/xarm6_moveit_sensor_manager.launch.xml b/xarm6_moveit_config/launch/xarm6_moveit_sensor_manager.launch.xml new file mode 100755 index 00000000..5d02698d --- /dev/null +++ b/xarm6_moveit_config/launch/xarm6_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/xarm6_moveit_config/package.xml b/xarm6_moveit_config/package.xml new file mode 100755 index 00000000..31b83dbb --- /dev/null +++ b/xarm6_moveit_config/package.xml @@ -0,0 +1,34 @@ + + + xarm6_moveit_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the xarm6 with the MoveIt! Motion Planning Framework + + Jason Peng + Jason Peng + + 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 + + + xarm_description + xarm_description + + + diff --git a/xarm7_moveit_config/.setup_assistant b/xarm7_moveit_config/.setup_assistant old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/CMakeLists.txt b/xarm7_moveit_config/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/config/controllers.yaml b/xarm7_moveit_config/config/controllers.yaml new file mode 100755 index 00000000..ffadaadc --- /dev/null +++ b/xarm7_moveit_config/config/controllers.yaml @@ -0,0 +1,12 @@ +controller_list: + - name: "" + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 diff --git a/xarm7_moveit_config/config/fake_controllers.yaml b/xarm7_moveit_config/config/fake_controllers.yaml old mode 100644 new mode 100755 index 69943375..3f71a8cc --- a/xarm7_moveit_config/config/fake_controllers.yaml +++ b/xarm7_moveit_config/config/fake_controllers.yaml @@ -1,5 +1,6 @@ controller_list: - name: fake_xarm7_controller + type: interpolate joints: - joint1 - joint2 diff --git a/xarm7_moveit_config/config/joint_limits.yaml b/xarm7_moveit_config/config/joint_limits.yaml old mode 100644 new mode 100755 index cb40169b..7a7b97b3 --- a/xarm7_moveit_config/config/joint_limits.yaml +++ b/xarm7_moveit_config/config/joint_limits.yaml @@ -4,36 +4,36 @@ joint_limits: joint1: has_velocity_limits: true - max_velocity: 3.14 + max_velocity: 2.14 has_acceleration_limits: false max_acceleration: 0 joint2: has_velocity_limits: true - max_velocity: 3.14 + max_velocity: 2.14 has_acceleration_limits: false max_acceleration: 0 joint3: has_velocity_limits: true - max_velocity: 3.14 + max_velocity: 2.14 has_acceleration_limits: false max_acceleration: 0 joint4: has_velocity_limits: true - max_velocity: 3.14 + max_velocity: 2.14 has_acceleration_limits: false max_acceleration: 0 joint5: has_velocity_limits: true - max_velocity: 3.14 + max_velocity: 2.14 has_acceleration_limits: false max_acceleration: 0 joint6: has_velocity_limits: true - max_velocity: 3.14 + max_velocity: 2.14 has_acceleration_limits: false max_acceleration: 0 joint7: has_velocity_limits: true - max_velocity: 3.14 + max_velocity: 2.14 has_acceleration_limits: false max_acceleration: 0 \ No newline at end of file diff --git a/xarm7_moveit_config/config/kinematics.yaml b/xarm7_moveit_config/config/kinematics.yaml old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/config/ompl_planning.yaml b/xarm7_moveit_config/config/ompl_planning.yaml old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/config/xarm7.srdf b/xarm7_moveit_config/config/xarm7.srdf old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/config/xarm7_params.yaml b/xarm7_moveit_config/config/xarm7_params.yaml new file mode 100755 index 00000000..aa0b56a9 --- /dev/null +++ b/xarm7_moveit_config/config/xarm7_params.yaml @@ -0,0 +1,3 @@ +xarm: + DOF: 7 + joint_names: ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7'] \ No newline at end of file diff --git a/xarm7_moveit_config/launch/default_warehouse_db.launch b/xarm7_moveit_config/launch/default_warehouse_db.launch old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/launch/demo.launch b/xarm7_moveit_config/launch/demo.launch old mode 100644 new mode 100755 index 2310c49b..d9f24fed --- a/xarm7_moveit_config/launch/demo.launch +++ b/xarm7_moveit_config/launch/demo.launch @@ -1,57 +1,12 @@ - - - - - - - - - - - - - - - - - - - - - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - - - - - - - - - - - - - + + + + + + + diff --git a/xarm7_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/xarm7_moveit_config/launch/fake_moveit_controller_manager.launch.xml old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/launch/joystick_control.launch b/xarm7_moveit_config/launch/joystick_control.launch old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/launch/move_group.launch b/xarm7_moveit_config/launch/move_group.launch old mode 100644 new mode 100755 index 046fa5d1..84216682 --- a/xarm7_moveit_config/launch/move_group.launch +++ b/xarm7_moveit_config/launch/move_group.launch @@ -1,6 +1,9 @@ - - + @@ -46,13 +49,18 @@ - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + $(arg jnt_stat_source) + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/xarm7_moveit_config/launch/ompl_planning_pipeline.launch.xml b/xarm7_moveit_config/launch/ompl_planning_pipeline.launch.xml old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/launch/planning_context.launch b/xarm7_moveit_config/launch/planning_context.launch old mode 100644 new mode 100755 index afdab700..09956860 --- a/xarm7_moveit_config/launch/planning_context.launch +++ b/xarm7_moveit_config/launch/planning_context.launch @@ -1,12 +1,13 @@ + - + diff --git a/xarm7_moveit_config/launch/planning_pipeline.launch.xml b/xarm7_moveit_config/launch/planning_pipeline.launch.xml old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/launch/realMove_exec.launch b/xarm7_moveit_config/launch/realMove_exec.launch new file mode 100755 index 00000000..f2299ca3 --- /dev/null +++ b/xarm7_moveit_config/launch/realMove_exec.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/xarm7_moveit_config/launch/run_benchmark_ompl.launch b/xarm7_moveit_config/launch/run_benchmark_ompl.launch old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/launch/sensor_manager.launch.xml b/xarm7_moveit_config/launch/sensor_manager.launch.xml old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/launch/setup_assistant.launch b/xarm7_moveit_config/launch/setup_assistant.launch old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/launch/trajectory_execution.launch.xml b/xarm7_moveit_config/launch/trajectory_execution.launch.xml old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/launch/warehouse.launch b/xarm7_moveit_config/launch/warehouse.launch old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/launch/warehouse_settings.launch.xml b/xarm7_moveit_config/launch/warehouse_settings.launch.xml old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/launch/xarm7_moveit_controller_manager.launch.xml b/xarm7_moveit_config/launch/xarm7_moveit_controller_manager.launch.xml old mode 100644 new mode 100755 index 5d02698d..b3ca137d --- a/xarm7_moveit_config/launch/xarm7_moveit_controller_manager.launch.xml +++ b/xarm7_moveit_config/launch/xarm7_moveit_controller_manager.launch.xml @@ -1,3 +1,8 @@ - + + + + + + diff --git a/xarm7_moveit_config/launch/xarm7_moveit_gazebo.launch b/xarm7_moveit_config/launch/xarm7_moveit_gazebo.launch new file mode 100755 index 00000000..3f0c80e6 --- /dev/null +++ b/xarm7_moveit_config/launch/xarm7_moveit_gazebo.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/xarm7_moveit_config/launch/xarm7_moveit_sensor_manager.launch.xml b/xarm7_moveit_config/launch/xarm7_moveit_sensor_manager.launch.xml old mode 100644 new mode 100755 diff --git a/xarm7_moveit_config/package.xml b/xarm7_moveit_config/package.xml old mode 100644 new mode 100755 diff --git a/xarm_api/CMakeLists.txt b/xarm_api/CMakeLists.txt new file mode 100755 index 00000000..a3cbb6a0 --- /dev/null +++ b/xarm_api/CMakeLists.txt @@ -0,0 +1,246 @@ +cmake_minimum_required(VERSION 2.8.3) +project(xarm_api) + +## 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 + std_msgs + std_srvs + genmsg + xarm_msgs + sensor_msgs +) + +set(XARM_API_DIR src/xarm_core) + +## 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 xarm_api + CATKIN_DEPENDS roscpp 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} + ${XARM_API_DIR}/connect.cc + ${XARM_API_DIR}/report_data.cc + + ${XARM_API_DIR}/common/crc16.cc + ${XARM_API_DIR}/common/queue_memcpy.cc + + ${XARM_API_DIR}/debug/debug_print.cc + + ${XARM_API_DIR}/instruction/uxbus_cmd.cc + ${XARM_API_DIR}/instruction/uxbus_cmd_ser.cc + ${XARM_API_DIR}/instruction/uxbus_cmd_tcp.cc + + ${XARM_API_DIR}/linux/network.cc + ${XARM_API_DIR}/linux/thread.cc + + ${XARM_API_DIR}/port/serial.cc + ${XARM_API_DIR}/port/socket.cc + + ) + +add_library(xarm_ros_client + src/xarm_ros_client.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(example1_report_norm_node + test/example1_report_norm.cc +) + +add_executable(xarm_driver_node + src/xarm_driver_node.cpp + src/xarm_driver.cpp +) + +add_executable(move_test + test/move_test.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(example1_report_norm_node + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ) + + target_link_libraries(xarm_driver_node + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ) + +target_link_libraries(move_test + ${PROJECT_NAME} + ${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_xarm_api.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/xarm_api/include/xarm/common/crc16.h b/xarm_api/include/xarm/common/crc16.h new file mode 100755 index 00000000..289aedca --- /dev/null +++ b/xarm_api/include/xarm/common/crc16.h @@ -0,0 +1,14 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#ifndef COMMON_CRC16_H_ +#define COMMON_CRC16_H_ + +#include "xarm/common/data_type.h" + +u16 modbus_crc(u8 *data, u16 len); + +#endif diff --git a/xarm_api/include/xarm/common/data_type.h b/xarm_api/include/xarm/common/data_type.h new file mode 100755 index 00000000..0322c4d9 --- /dev/null +++ b/xarm_api/include/xarm/common/data_type.h @@ -0,0 +1,90 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#ifndef COMMON_DATA_TYPE_H_ +#define COMMON_DATA_TYPE_H_ + +#include + +typedef unsigned char u8; +typedef signed char s8; +typedef unsigned short u16; +typedef signed short s16; +typedef unsigned int u32; +typedef signed int s32; +typedef unsigned long long u64; +typedef signed long long s64; +typedef float fp32; +typedef double fp64; + +#define bin64_to_8(a, b) \ + { \ + (b)[0] = (u8)((u64)(a) >> 56); \ + (b)[1] = (u8)((u64)(a) >> 48); \ + (b)[2] = (u8)((u64)(a) >> 40); \ + (b)[3] = (u8)((u64)(a) >> 32); \ + (b)[4] = (u8)((u64)(a) >> 24); \ + (b)[5] = (u8)((u64)(a) >> 16); \ + (b)[6] = (u8)((u64)(a) >> 8); \ + (b)[7] = (u8)(a)); \ + } + +#define bin32_to_8(a, b) \ + { \ + (b)[0] = (u8)((u32)(a) >> 24); \ + (b)[1] = (u8)((u32)(a) >> 16); \ + (b)[2] = (u8)((u32)(a) >> 8); \ + (b)[3] = (u8)(a); \ + } + +#define bin16_to_8(a, b) \ + { \ + (b)[0] = (u8)((u16)(a) >> 8); \ + (b)[1] = (u8)(a); \ + } + +#define bin8_to_64(a) \ + ((((a)[0] << 56) | ((a)[1] << 48)) + (((a)[2] << 40) | ((a)[3] << 32)) + \ + (((a)[4] << 24) | ((a)[5] << 16)) + (((a)[6] << 8) | ((a)[7]))) + +#define bin8_to_32(a) \ + ((((a)[0] << 24) | ((a)[1] << 16)) + (((a)[2] << 8) | ((a)[3]))) + +#define bin8_to_16(a) (((a)[0] << 8) | ((a)[1])) + +inline void fp32_to_hex(fp64 dataf, u8 datahex[4]) { + union _fp32hex { + fp32 dataf; + u8 datahex[4]; + } fp32hex; + fp32hex.dataf = dataf; + datahex[0] = fp32hex.datahex[0]; + datahex[1] = fp32hex.datahex[1]; + datahex[2] = fp32hex.datahex[2]; + datahex[3] = fp32hex.datahex[3]; +} + +inline fp64 hex_to_fp32(u8 datahex[4]) { + union _fp32hex { + fp32 dataf; + u8 datahex[4]; + } fp32hex; + fp32hex.datahex[0] = datahex[0]; + fp32hex.datahex[1] = datahex[1]; + fp32hex.datahex[2] = datahex[2]; + fp32hex.datahex[3] = datahex[3]; + return (fp64)fp32hex.dataf; +} + +inline void hex_to_nfp32(u8 *datahex, fp32 *dataf, u8 n) { + for (u8 i = 0; i < n; ++i) dataf[i] = hex_to_fp32(&datahex[i * 4]); +} + +inline void nfp32_to_hex(fp32 *dataf, u8 *datahex, u8 n) { + for (u8 i = 0; i < n; ++i) fp32_to_hex(dataf[i], &datahex[i * 4]); +} + +#endif diff --git a/xarm_api/include/xarm/common/queue_memcpy.h b/xarm_api/include/xarm/common/queue_memcpy.h new file mode 100755 index 00000000..1744c5d6 --- /dev/null +++ b/xarm_api/include/xarm/common/queue_memcpy.h @@ -0,0 +1,36 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#ifndef COMMON_QUEUE_MEMCPY_H_ +#define COMMON_QUEUE_MEMCPY_H_ + +#include + +class QueueMemcpy { + public: + QueueMemcpy(long n, long n_size); + ~QueueMemcpy(void); + char flush(void); + char push(void *data); + char pop(void *data); + char get(void *data); + long size(void); + long node_size(void); + int is_full(void); + + protected: + private: + long total_; + long annode_size_; + + long cnt_; + long head_; + long tail_; + char *buf_; + pthread_mutex_t mutex_; +}; + +#endif diff --git a/xarm_api/include/xarm/connect.h b/xarm_api/include/xarm/connect.h new file mode 100755 index 00000000..649b9d78 --- /dev/null +++ b/xarm_api/include/xarm/connect.h @@ -0,0 +1,19 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#ifndef XARM_CONNECT_H_ +#define XARM_CONNECT_H_ + +#include "xarm/instruction/uxbus_cmd_ser.h" +#include "xarm/instruction/uxbus_cmd_tcp.h" + +UxbusCmdSer *connect_rs485_control(const char *com); +UxbusCmdTcp *connect_tcp_control(char *server_ip); +SocketPort *connect_tcp_report_norm(char *server_ip); +SocketPort *connect_tcp_report_rich(char *server_ip); +SocketPort *connect_tcp_report_real(char *server_ip); + +#endif diff --git a/xarm_api/include/xarm/debug/debug_print.h b/xarm_api/include/xarm/debug/debug_print.h new file mode 100755 index 00000000..cba4bd9a --- /dev/null +++ b/xarm_api/include/xarm/debug/debug_print.h @@ -0,0 +1,20 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#ifndef DEBUG_DEBUG_PRINT_H_ +#define DEBUG_DEBUG_PRINT_H_ + +#include "xarm/common/data_type.h" + +void print_log(const char *format, ...); +void print_nvect(const char *str, fp64 vect[], u8 n); +void print_nvect(const char *str, float *vect, u8 n); +void print_nvect(const char *str, u8 vect[], u8 n); +void print_nvect(const char *str, u16 vect[], u8 n); +void print_nvect(const char *str, u32 vect[], u8 n); +void print_hex(const char *str, u8 *hex, u8 len); + +#endif diff --git a/xarm_api/include/xarm/instruction/servo3_config.h b/xarm_api/include/xarm/instruction/servo3_config.h new file mode 100755 index 00000000..59bbbefb --- /dev/null +++ b/xarm_api/include/xarm/instruction/servo3_config.h @@ -0,0 +1,56 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#ifndef INSTRUCTION_SERVO3_CONFIG_H_ +#define INSTRUCTION_SERVO3_CONFIG_H_ +class SERVO3_RG { + public: + static const unsigned short CON_EN = 0x0100; + static const unsigned short CON_MODE = 0x0101; + static const unsigned short CON_DIR = 0x0102; + static const unsigned short SV3MOD_POS = 0; + static const unsigned short SV3MOD_SPD = 1; + static const unsigned short SV3MOD_FOS = 2; + static const unsigned short SV3_SAVE = 0x1000; + static const unsigned short BRAKE = 0x0104; + static const unsigned short GET_TEMP = 0x000E; + static const unsigned short OVER_TEMP = 0x0108; + static const unsigned short CURR_CURR = 0x0001; + static const unsigned short POS_KP = 0x0200; + static const unsigned short POS_FWDKP = 0x0201; + static const unsigned short POS_PWDTC = 0x0202; + static const unsigned short SPD_KP = 0x0203; + static const unsigned short SPD_KI = 0x0204; + static const unsigned short CURR_KP = 0x090C; + static const unsigned short CURR_KI = 0x090D; + static const unsigned short SPD_IFILT = 0x030C; + static const unsigned short SPD_OFILT = 0x030D; + static const unsigned short POS_CMDILT = 0x030E; + static const unsigned short CURR_IFILT = 0x0401; + static const unsigned short POS_KD = 0x0205; + static const unsigned short POS_ACCT = 0x0300; + static const unsigned short POS_DECT = 0x0301; + static const unsigned short POS_STHT = 0x0302; + static const unsigned short POS_SPD = 0x0303; + static const unsigned short MT_ID = 0x1600; + static const unsigned short BAUDRATE = 0x0601; + static const unsigned short TAGET_TOQ = 0x050a; + static const unsigned short CURR_TOQ = 0x050c; + static const unsigned short TOQ_SPD = 0x050e; + static const unsigned short TAGET_POS = 0x0700; + static const unsigned short CURR_POS = 0x0702; + static const unsigned short HARD_VER = 0x0800; + static const unsigned short SOFT_VER = 0x0801; + static const unsigned short MT_TYPE = 0x0802; + static const unsigned short MT_ZERO = 0x0817; + static const unsigned short RESET_PVL = 0x0813; + static const unsigned short CAL_ZERO = 0x080C; + static const unsigned short ERR_SWITCH = 0x0910; + static const unsigned short RESET_ERR = 0x0109; + static const unsigned short SV3_BRO_ID = 0xFF; +}; + +#endif diff --git a/xarm_api/include/xarm/instruction/uxbus_cmd.h b/xarm_api/include/xarm/instruction/uxbus_cmd.h new file mode 100755 index 00000000..72e49168 --- /dev/null +++ b/xarm_api/include/xarm/instruction/uxbus_cmd.h @@ -0,0 +1,80 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#ifndef INSTRUCTION_UXBUS_CMD_H_ +#define INSTRUCTION_UXBUS_CMD_H_ + +#include "xarm/common/data_type.h" + +class UxbusCmd { + public: + UxbusCmd(void); + ~UxbusCmd(void); + + int get_version(u8 rx_data[40]); + int motion_en(u8 id, u8 value); + int set_state(u8 value); + int get_state(u8 *rx_data); + int get_cmdnum(u16 *rx_data); + int get_errcode(u8 *rx_data); + int clean_err(void); + int clean_war(void); + int set_brake(u8 axis, u8 en); + int set_mode(u8 value); + int move_line(fp32 mvpose[6], fp32 mvvelo, fp32 mvacc, fp32 mvtime); + int move_lineb(fp32 mvpose[6], fp32 mvvelo, fp32 mvacc, fp32 mvtime, + fp32 mvradii); + int move_joint(fp32 mvjoint[7], fp32 mvvelo, fp32 mvacc, fp32 mvtime); + int move_gohome(fp32 mvvelo, fp32 mvacc, fp32 mvtime); + int move_servoj(fp32 mvjoint[7], fp32 mvvelo, fp32 mvacc, fp32 mvtime); + int sleep_instruction(fp32 sltime); + int set_tcp_jerk(fp32 jerk); + int set_tcp_maxacc(fp32 maxacc); + int set_joint_jerk(fp32 jerk); + int set_joint_maxacc(fp32 maxacc); + int set_tcp_offset(fp32 pose_offset[6]); + int clean_conf(void); + int save_conf(void); + int get_tcp_pose(fp32 pose[6]); + int get_joint_pose(fp32 angles[7]); + int get_ik(fp32 pose[6], fp32 angles[7]); + int get_fk(fp32 angles[7], fp32 pose[6]); + int is_joint_limit(fp32 joint[7], int *value); + int is_tcp_limit(fp32 pose[6], int *value); + int gripper_addr_w16(u8 id, u16 addr, fp32 value); + int gripper_addr_r16(u8 id, u16 addr, fp32 *value); + int gripper_addr_w32(u8 id, u16 addr, fp32 value); + int gripper_addr_r32(u8 id, u16 addr, fp32 *value); + int gripper_set_en(u16 value); + int gripper_set_mode(u16 value); + int gripper_set_zero(void); + int gripper_get_pos(fp32 *pulse); + int gripper_set_pos(fp32 pulse); + int gripper_set_posspd(fp32 speed); + int gripper_get_errcode(u8 rx_data[2]); + int gripper_clean_err(void); + int servo_set_zero(u8 id); + int servo_get_dbmsg(u8 rx_data[16]); + int servo_addr_w16(u8 id, u16 addr, fp32 value); + int servo_addr_r16(u8 id, u16 addr, fp32 *value); + int servo_addr_w32(u8 id, u16 addr, fp32 value); + int servo_addr_r32(u8 id, u16 addr, fp32 *value); + virtual void close(void); + + private: + virtual int check_xbus_prot(u8 *data, u8 funcode); + virtual int send_pend(u8 funcode, int num, int timeout, u8 *rx_data); + virtual int send_xbus(u8 funcode, u8 *txdata, int num); + int set_nu8(u8 funcode, u8 *datas, int num); + int get_nu8(u8 funcode, u8 num, u8 *rx_data); + int get_nu16(u8 funcode, u8 num, u16 *rx_data); + int set_nfp32(u8 funcode, fp32 *datas, u8 num); + int get_nfp32(u8 funcode, u8 num, fp32 *rx_data); + int swop_nfp32(u8 funcode, fp32 tx_datas[], u8 txn, fp32 *rx_data, u8 rxn); + int is_nfp32(u8 funcode, fp32 datas[], u8 txn, int *value); +}; + +#endif diff --git a/xarm_api/include/xarm/instruction/uxbus_cmd_config.h b/xarm_api/include/xarm/instruction/uxbus_cmd_config.h new file mode 100755 index 00000000..44d9ce93 --- /dev/null +++ b/xarm_api/include/xarm/instruction/uxbus_cmd_config.h @@ -0,0 +1,110 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#ifndef INSTRUCTION_UXBUS_CMD_CONFIG_H_ +#define INSTRUCTION_UXBUS_CMD_CONFIG_H_ +class UXBUS_RG { + public: + UXBUS_RG(void) {} + ~UXBUS_RG(void) {} + + static const unsigned char GET_VERSION = 1; + + static const unsigned char MOTION_EN = 11; + static const unsigned char SET_STATE = 12; + static const unsigned char GET_STATE = 13; + static const unsigned char GET_CMDNUM = 14; + static const unsigned char GET_ERROR = 15; + static const unsigned char CLEAN_ERR = 16; + static const unsigned char CLEAN_WAR = 17; + static const unsigned char SET_BRAKE = 18; + static const unsigned char SET_MODE = 19; + + static const unsigned char MOVE_LINE = 21; + static const unsigned char MOVE_LINEB = 22; + static const unsigned char MOVE_JOINT = 23; + static const unsigned char MOVE_HOME = 25; + static const unsigned char SLEEP_INSTT = 26; + static const unsigned char MOVE_SERVOJ = 29; + + static const unsigned char SET_TCP_JERK = 31; + static const unsigned char SET_TCP_MAXACC = 32; + static const unsigned char SET_JOINT_JERK = 33; + static const unsigned char SET_JOINT_MAXACC = 34; + static const unsigned char SET_TCP_OFFSET = 35; + static const unsigned char CLEAN_CONF = 39; + static const unsigned char SAVE_CONF = 40; + + static const unsigned char GET_TCP_POSE = 41; + static const unsigned char GET_JOINT_POS = 42; + static const unsigned char GET_IK = 43; + static const unsigned char GET_FK = 44; + static const unsigned char IS_JOINT_LIMIT = 45; + static const unsigned char IS_TCP_LIMIT = 46; + + static const unsigned char SERVO_W16B = 101; + static const unsigned char SERVO_R16B = 102; + static const unsigned char SERVO_W32B = 103; + static const unsigned char SERVO_R32B = 104; + static const unsigned char SERVO_ZERO = 105; + static const unsigned char SERVO_DBMSG = 106; + + static const unsigned char GPGET_ERR = 125; + static const unsigned char GRIPP_W16B = 127; + static const unsigned char GRIPP_R16B = 128; + static const unsigned char GRIPP_W32B = 129; + static const unsigned char GRIPP_R32B = 130; +}; + +class UXBUS_STATE { + public: + UXBUS_STATE(void) {} + ~UXBUS_STATE(void) {} + static const int ERR_CODE = 1; + static const int WAR_CODE = 2; + static const int ERR_TOUT = 3; + static const int ERR_LENG = 4; + static const int ERR_NUM = 5; + static const int ERR_PROT = 6; + static const int ERR_FUN = 7; + static const int ERR_NOTTCP = 8; + static const int ERR_OTHER = 11; +}; + +class UXBUS_CONF { + public: + UXBUS_CONF(void) {} + ~UXBUS_CONF(void) {} + + static const int SET_TIMEOUT = 1000; // ms + static const int GET_TIMEOUT = 1000; // ms + static const int GRIPPER_ID = 8; + static const int MASTER_ID = 0xAA; + static const int SLAVE_ID = 0x55; +}; + +class XARM_MODE { + public: + XARM_MODE(void) {} + ~XARM_MODE(void) {} + + static const int POSE = 0; + static const int SERVO = 1; + static const int TEACH_JOINT = 2; + static const int TEACH_CART = 3; +}; + +class XARM_STATE { + public: + XARM_STATE(void) {} + ~XARM_STATE(void) {} + + static const int START = 0; + static const int PAUSE = 3; + static const int STOP = 4; +}; + +#endif diff --git a/xarm_api/include/xarm/instruction/uxbus_cmd_ser.h b/xarm_api/include/xarm/instruction/uxbus_cmd_ser.h new file mode 100755 index 00000000..2c9fbf95 --- /dev/null +++ b/xarm_api/include/xarm/instruction/uxbus_cmd_ser.h @@ -0,0 +1,27 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#ifndef INSTRUCTION_UXBUS_CMD_SER_H_ +#define INSTRUCTION_UXBUS_CMD_SER_H_ + +#include "xarm/instruction/uxbus_cmd.h" +#include "xarm/port/serial.h" + +class UxbusCmdSer : public UxbusCmd { + public: + UxbusCmdSer(SerialPort *arm_port); + ~UxbusCmdSer(void); + + int check_xbus_prot(u8 *datas, u8 funcode); + int send_pend(u8 funcode, int num, int timeout, u8 *ret_data); + int send_xbus(u8 funcode, u8 *datas, int num); + void close(void); + + private: + SerialPort *arm_port_; +}; + +#endif diff --git a/xarm_api/include/xarm/instruction/uxbus_cmd_tcp.h b/xarm_api/include/xarm/instruction/uxbus_cmd_tcp.h new file mode 100755 index 00000000..e2a5a833 --- /dev/null +++ b/xarm_api/include/xarm/instruction/uxbus_cmd_tcp.h @@ -0,0 +1,33 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#ifndef INSTRUCTION_UXBUS_CMD_TCP_H_ +#define INSTRUCTION_UXBUS_CMD_TCP_H_ + +#include "xarm/instruction/uxbus_cmd.h" +#include "xarm/port/socket.h" + +class UxbusCmdTcp : public UxbusCmd { + public: + UxbusCmdTcp(SocketPort *arm_port); + ~UxbusCmdTcp(void); + + int check_xbus_prot(u8 *datas, u8 funcode); + int send_pend(u8 funcode, int num, int timeout, u8 *ret_data); + int send_xbus(u8 funcode, u8 *datas, int num); + void close(void); + + private: + SocketPort *arm_port_; + int bus_flag_; + int prot_flag_; + int TX2_PROT_CON_ = 2; // tcp cmd prot + int TX2_PROT_HEAT_ = 1; // tcp heat prot + int TX2_BUS_FLAG_MIN_ = 1; // cmd序号 起始值 + int TX2_BUS_FLAG_MAX_ = 5000; // cmd序号 最大值 +}; + +#endif diff --git a/xarm_api/include/xarm/linux/network.h b/xarm_api/include/xarm/linux/network.h new file mode 100755 index 00000000..2102e49f --- /dev/null +++ b/xarm_api/include/xarm/linux/network.h @@ -0,0 +1,16 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#ifndef LINUX_NETWORK_H_ +#define LINUX_NETWORK_H_ + +#include "xarm/common/data_type.h" + +int socket_init(char *local_ip, int port, u8 is_server); +s8 socket_send_data(int client_fp, u8 *data, u16 len); +s8 socket_connect_server(int *socket, char server_ip[], int server_port); + +#endif diff --git a/xarm_api/include/xarm/linux/thread.h b/xarm_api/include/xarm/linux/thread.h new file mode 100755 index 00000000..b856e06a --- /dev/null +++ b/xarm_api/include/xarm/linux/thread.h @@ -0,0 +1,17 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#ifndef LINUX_THREAD_H_ +#define LINUX_THREAD_H_ + +#include + +typedef void *(*fun_point_t)(void *); + +void thread_delete(pthread_t id); +pthread_t thread_init(fun_point_t fun_point, void *arg); + +#endif diff --git a/xarm_api/include/xarm/port/serial.h b/xarm_api/include/xarm/port/serial.h new file mode 100755 index 00000000..1ed6e330 --- /dev/null +++ b/xarm_api/include/xarm/port/serial.h @@ -0,0 +1,57 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#ifndef PORT_SERIAL_H_ +#define PORT_SERIAL_H_ + +#include + +#include "xarm/common/data_type.h" +#include "xarm/common/queue_memcpy.h" + +class SerialPort { + public: + SerialPort(const char *port, int baud, int que_num, int que_maxlen); + ~SerialPort(void); + int is_ok(void); + void flush(void); + void recv_proc(void); + int write_frame(u8 *data, int len); + int read_frame(u8 *data); + void close_port(void); + int que_maxlen_; + int que_num_; + + private: + int fp_; + int state_; + pthread_t thread_id_; + QueueMemcpy *rx_que_; + int init_serial(const char *port, int baud); + int read_char(u8 *ch); + int read_str(u8 *data, char eol, u8 len); + int write_char(u8 ch); + void parse_put(u8 *data, u16 len); + + typedef enum _UXBUS_RECV_STATE { + UXBUS_START_FROMID = 0, + UXBUS_START_TOOID = 1, + UXBUS_STATE_LENGTH = 2, + UXBUS_STATE_DATA = 3, + UXBUS_STATE_CRC1 = 4, + UXBUS_STATE_CRC2 = 5, + } UXBUS_RECV_STATE; + + u8 UXBUS_PROT_FROMID_; + u8 UXBUS_PROT_TOID_; + + u16 rx_data_idx_; + u8 rx_state_; + u8 rx_buf_[128]; + u16 rx_length_; +}; + +#endif diff --git a/xarm_api/include/xarm/port/socket.h b/xarm_api/include/xarm/port/socket.h new file mode 100755 index 00000000..d0b4b56b --- /dev/null +++ b/xarm_api/include/xarm/port/socket.h @@ -0,0 +1,35 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#ifndef PORT_SOCKET_H_ +#define PORT_SOCKET_H_ + +#include + +#include "xarm/common/data_type.h" +#include "xarm/common/queue_memcpy.h" + +class SocketPort { + public: + SocketPort(char *server_ip, int server_port, int que_num, int que_maxlen); + ~SocketPort(void); + int is_ok(void); + void flush(void); + void recv_proc(void); + int write_frame(u8 *data, int len); + int read_frame(u8 *data); + void close_port(void); + int que_maxlen_; + + private: + int fp_; + int state_; + int que_num_; + QueueMemcpy *rx_que_; + pthread_t thread_id_; +}; + +#endif diff --git a/xarm_api/include/xarm/report_data.h b/xarm_api/include/xarm/report_data.h new file mode 100755 index 00000000..e90cb0c9 --- /dev/null +++ b/xarm_api/include/xarm/report_data.h @@ -0,0 +1,81 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#ifndef XARM_REPORT_DATA_H_ +#define XARM_REPORT_DATA_H_ + +#include "xarm/common/data_type.h" + +class ReportDataNorm { + public: + ReportDataNorm(void); + ~ReportDataNorm(void); + + int flush_data(u8 *rx_data); + void print_data(void); + + public: + int runing_; + int mode_; + int mt_brake_; + int mt_able_; + int err_; + int war_; + int cmdnum_; + + float pose_[6]; + float tcp_offset_[6]; + int total_num_; + float angle_[7]; +}; + +class ReportDataRich { + public: + ReportDataRich(void); + ~ReportDataRich(void); + + int flush_data(u8 *rx_data); + void print_data(void); + + private: + int runing_; + int mode_; + int mt_brake_; + int mt_able_; + int err_; + int war_; + int cmdnum_; + float angle_[7]; + float pose_[6]; + float tcp_offset_[6]; + int total_num_; + + int arm_type_; + int axis_num_; + int master_id_; + int slave_id_; + int motor_tid_; + int motor_fid_; + u8 versions_[30]; + float trs_jerk_; + float trs_accmin_; + float trs_accmax_; + float trs_velomin_; + float trs_velomax_; + float p2p_jerk_; + float p2p_accmin_; + float p2p_accmax_; + float p2p_velomin_; + float p2p_velomax_; + float rot_jerk_; + float rot_accmax_; + int sv3msg_[16]; + float trs_msg_[5]; + float p2p_msg_[5]; + float rot_msg_[2]; +}; + +#endif diff --git a/xarm_api/include/xarm/xarm_config.h b/xarm_api/include/xarm/xarm_config.h new file mode 100755 index 00000000..3f41d418 --- /dev/null +++ b/xarm_api/include/xarm/xarm_config.h @@ -0,0 +1,19 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +class XARM_CONF { + public: + XARM_CONF() {} + ~XARM_CONF() {} + + static const int AXIS_NUM = 7; + static const int GRIPPER_ID = 8; + static const int SERIAL_BAUD = 921600; + static const int TCP_PORT_CONTROL = 502; + static const int TCP_PORT_REPORT_NORM = 30001; + static const int TCP_PORT_REPORT_RICH = 30002; + static const int TCP_PORT_REPORT_REAL = 30003; +}; diff --git a/xarm_api/include/xarm_driver.h b/xarm_api/include/xarm_driver.h new file mode 100755 index 00000000..27426690 --- /dev/null +++ b/xarm_api/include/xarm_driver.h @@ -0,0 +1,66 @@ +#ifndef __XARM_DRIVER_H +#define __XARM_DRIVER_H + +#include "ros/ros.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include "xarm/connect.h" +#include "xarm/report_data.h" + +namespace xarm_api +{ + class XARMDriver + { + public: + XARMDriver():spinner(4){spinner.start();}; + ~XARMDriver(); + void XARMDriverInit(ros::NodeHandle& root_nh, char *server_ip); + + bool MotionCtrlCB(xarm_msgs::SetAxis::Request &req, xarm_msgs::SetAxis::Response &res); + bool SetModeCB(xarm_msgs::SetInt16::Request& req, xarm_msgs::SetInt16::Response& res); + bool SetStateCB(xarm_msgs::SetInt16::Request &req, xarm_msgs::SetInt16::Response &res); + bool SetTCPOffsetCB(xarm_msgs::TCPOffset::Request &req, xarm_msgs::TCPOffset::Response &res); + bool GoHomeCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res); + bool MoveJointCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res); + bool MoveLinebCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res); + bool MoveLineCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res); + bool MoveServoJCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res); + + void pub_robot_msg(xarm_msgs::RobotMsg rm_msg); + void pub_joint_state(sensor_msgs::JointState js_msg); + + int get_frame(void); + int get_rich_data(ReportDataNorm &norm_data); + + private: + SocketPort *arm_report_; + ReportDataNorm norm_data_; + UxbusCmd *arm_cmd_; + unsigned char rx_data_[1280]; + std::string ip; + pthread_t thread_id_; + ros::AsyncSpinner spinner; + + ros::NodeHandle nh_; + ros::ServiceServer go_home_server_; + ros::ServiceServer move_joint_server_; + ros::ServiceServer motion_ctrl_server_; + ros::ServiceServer set_state_server_; + ros::ServiceServer set_mode_server_; + ros::ServiceServer move_lineb_server_; + ros::ServiceServer move_line_server_; + ros::ServiceServer move_servoj_server_; + ros::ServiceServer set_tcp_offset_server_; + + ros::Publisher joint_state_; + ros::Publisher robot_rt_state_; + }; +} + +#endif diff --git a/xarm_api/include/xarm_ros_client.h b/xarm_api/include/xarm_ros_client.h new file mode 100755 index 00000000..691b5e9a --- /dev/null +++ b/xarm_api/include/xarm_ros_client.h @@ -0,0 +1,48 @@ +#ifndef __XARM_ROS_CLIENT_H__ +#define __XARM_ROS_CLIENT_H__ + +#include "ros/ros.h" +#include + +namespace xarm_api{ + +class XArmROSClient +{ +public: + XArmROSClient(ros::NodeHandle& nh); + ~XArmROSClient(){}; + + int motionEnable(short en); + int setState(short state); + int setMode(short mode); + int setTCPOffset(const std::vector& tcp_offset); + int setServoJ(const std::vector& joint_cmd); + int goHome(float jnt_vel_rad, float jnt_acc_rad=15); + int moveJoint(const std::vector& joint_cmd, float jnt_vel_rad, float jnt_acc_rad=15); + int moveLine(const std::vector& cart_cmd, float cart_vel_mm, float cart_acc_mm=500); + int moveLineB(int num_of_pnts, const std::vector cart_cmds[], float cart_vel_mm, float cart_acc_mm=500, float radii=0); + +private: + ros::ServiceClient motion_ctrl_client_; + ros::ServiceClient set_mode_client_; + ros::ServiceClient set_state_client_; + ros::ServiceClient go_home_client_; + ros::ServiceClient move_lineb_client_; + ros::ServiceClient move_servoj_client_; + ros::ServiceClient move_line_client_; + ros::ServiceClient move_joint_client_; + ros::ServiceClient set_tcp_offset_client_; + + xarm_msgs::SetAxis set_axis_srv_; + xarm_msgs::SetInt16 set_int16_srv_; + xarm_msgs::TCPOffset offset_srv_; + xarm_msgs::Move move_srv_; + xarm_msgs::Move servoj_msg_; + + + ros::NodeHandle nh_; +}; + +} + +#endif diff --git a/xarm_api/package.xml b/xarm_api/package.xml new file mode 100755 index 00000000..5d2a3e5b --- /dev/null +++ b/xarm_api/package.xml @@ -0,0 +1,71 @@ + + + xarm_api + 0.0.0 + The xarm_api package + + + + + waylon + + + + + + BSD + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + xarm_msgs + sensor_msgs + + roscpp + std_msgs + xarm_msgs + sensor_msgs + + roscpp + std_msgs + xarm_msgs + sensor_msgs + + + + + + diff --git a/xarm_api/src/xarm_core/common/crc16.cc b/xarm_api/src/xarm_core/common/crc16.cc new file mode 100755 index 00000000..ac6b4fa4 --- /dev/null +++ b/xarm_api/src/xarm_core/common/crc16.cc @@ -0,0 +1,67 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#include "xarm/common/crc16.h" + +const u8 crc_tableh[256] = { + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40}; + +const u8 crc_tablel[256] = { + 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, + 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, + 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, + 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, + 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, + 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, + 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, + 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, + 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, + 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, + 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, + 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, + 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, + 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, + 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, + 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, + 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, + 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, + 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, + 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, + 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, + 0x41, 0x81, 0x80, 0x40}; + +u16 modbus_crc(u8 *data, u16 len) { + u8 init_crch = 0xFF; + u8 init_crcl = 0xFF; + u16 index; + while (len--) { + index = init_crcl ^ *data++; + init_crcl = init_crch ^ crc_tableh[index]; + init_crch = crc_tablel[index]; + } + return ((init_crch << 8) | init_crcl); +} diff --git a/xarm_api/src/xarm_core/common/queue_memcpy.cc b/xarm_api/src/xarm_core/common/queue_memcpy.cc new file mode 100755 index 00000000..31562df3 --- /dev/null +++ b/xarm_api/src/xarm_core/common/queue_memcpy.cc @@ -0,0 +1,83 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#include "xarm/common/queue_memcpy.h" + +#include + +QueueMemcpy::QueueMemcpy(long n, long n_size) { + total_ = n; + annode_size_ = n_size; + buf_ = new char[total_ * annode_size_]; + pthread_mutex_init(&mutex_, NULL); + flush(); +} + +QueueMemcpy::~QueueMemcpy(void) { delete[] buf_; } + +char QueueMemcpy::flush(void) { + cnt_ = 0; + head_ = 0; + tail_ = 0; + memset(buf_, 0, annode_size_ * total_); + + return 0; +} + +long QueueMemcpy::size(void) { return cnt_; } + +int QueueMemcpy::is_full(void) { + if (total_ <= cnt_) + return 1; + else + return 0; +} + +long QueueMemcpy::node_size(void) { return annode_size_; } + +char QueueMemcpy::pop(void *data) { + pthread_mutex_lock(&mutex_); + if (0 >= cnt_) { + pthread_mutex_unlock(&mutex_); + return -1; + } + if (total_ <= tail_) tail_ = 0; + + memcpy(data, &buf_[tail_ * annode_size_], annode_size_); + tail_++; + cnt_--; + pthread_mutex_unlock(&mutex_); + return 0; +} + +char QueueMemcpy::get(void *data) { + pthread_mutex_lock(&mutex_); + if (0 >= cnt_) { + pthread_mutex_unlock(&mutex_); + return -1; + } + if (total_ <= tail_) tail_ = 0; + + memcpy(data, &buf_[tail_ * annode_size_], annode_size_); + pthread_mutex_unlock(&mutex_); + + return 0; +} + +char QueueMemcpy::push(void *data) { + pthread_mutex_lock(&mutex_); + if (total_ <= cnt_) { + pthread_mutex_unlock(&mutex_); + return -1; + } + if (total_ <= head_) head_ = 0; + + memcpy(&buf_[head_ * annode_size_], data, annode_size_); + head_++; + cnt_++; + pthread_mutex_unlock(&mutex_); + return 0; +} diff --git a/xarm_api/src/xarm_core/connect.cc b/xarm_api/src/xarm_core/connect.cc new file mode 100755 index 00000000..ecdd1806 --- /dev/null +++ b/xarm_api/src/xarm_core/connect.cc @@ -0,0 +1,68 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#include "xarm/connect.h" + +#include "xarm/instruction/uxbus_cmd.h" +#include "xarm/instruction/uxbus_cmd_ser.h" +#include "xarm/instruction/uxbus_cmd_tcp.h" +#include "xarm/xarm_config.h" + +UxbusCmdSer *connect_rs485_control(const char *com) { + SerialPort *arm_port = new SerialPort(com, XARM_CONF::SERIAL_BAUD, 3, 128); + if (arm_port->is_ok() != 0) { + printf("Error: Serial RS485 connection failed\n"); + return NULL; + } + UxbusCmdSer *arm_cmd = new UxbusCmdSer(arm_port); + printf("Serial RS485 connection successful\n"); + return arm_cmd; +} + +UxbusCmdTcp *connect_tcp_control(char *server_ip) { + SocketPort *arm_port = + new SocketPort(server_ip, XARM_CONF::TCP_PORT_CONTROL, 3, 128); + if (arm_port->is_ok() != 0) { + printf("Error: Tcp Control connection failed\n"); + return NULL; + } + UxbusCmdTcp *arm_cmd = new UxbusCmdTcp(arm_port); + printf("Tcp Control connection successful\n"); + return arm_cmd; +} + +SocketPort *connect_tcp_report_norm(char *server_ip) { + SocketPort *arm_report = + new SocketPort(server_ip, XARM_CONF::TCP_PORT_REPORT_NORM, 3, 512); + if (arm_report->is_ok() != 0) { + printf("Error: Tcp Report Norm connection failed\n"); + return NULL; + } + printf("Tcp Report Norm connection successful\n"); + return arm_report; +} + +SocketPort *connect_tcp_report_rich(char *server_ip) { + SocketPort *arm_report = + new SocketPort(server_ip, XARM_CONF::TCP_PORT_REPORT_RICH, 3, 512); + if (arm_report->is_ok() != 0) { + printf("Error: Tcp Report Norm connection failed\n"); + return NULL; + } + printf("Tcp Report Norm connection successful\n"); + return arm_report; +} + +SocketPort *connect_tcp_report_real(char *server_ip) { + SocketPort *arm_report = + new SocketPort(server_ip, XARM_CONF::TCP_PORT_REPORT_REAL, 3, 512); + if (arm_report->is_ok() != 0) { + printf("Error: Tcp Report Norm connection failed\n"); + return NULL; + } + printf("Tcp Report Norm connection successful\n"); + return arm_report; +} diff --git a/xarm_api/src/xarm_core/debug/debug_print.cc b/xarm_api/src/xarm_core/debug/debug_print.cc new file mode 100755 index 00000000..8a1476f3 --- /dev/null +++ b/xarm_api/src/xarm_core/debug/debug_print.cc @@ -0,0 +1,67 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#include "xarm/debug/debug_print.h" + +#include + +#include "xarm/common/data_type.h" + +#define DB_FLG "[deprint ] " +#define PRINTF_NUM_MAX 128 +#define log_put printf + +void print_log(const char *format, ...) { + char buffer[PRINTF_NUM_MAX] = {0}; + va_list arg; + va_start(arg, format); + vsnprintf(buffer, PRINTF_NUM_MAX, format, arg); + printf("%s", buffer); + va_end(arg); +} + +void print_nvect(const char *str, fp64 vect[], u8 n) { + u8 i; + print_log("%s", str); + for (i = 0; i < n; ++i) print_log("%0.3f ", vect[i]); + print_log("\n"); +} + +void print_nvect(const char *str, float *vect, u8 n) { + u8 i; + print_log("%s", str); + for (i = 0; i < n; ++i) print_log("%0.3f ", vect[i]); + print_log("\n"); +} + +void print_nvect(const char *str, u8 vect[], u8 n) { + u8 i; + print_log("%s", str); + for (i = 0; i < n; ++i) print_log("%d ", vect[i]); + print_log("\n"); +} + +void print_nvect(const char *str, u16 vect[], u8 n) { + u8 i; + print_log("%s", str); + for (i = 0; i < n; ++i) print_log("%d ", vect[i]); + print_log("\n"); +} + +void print_nvect(const char *str, u32 vect[], u8 n) { + u8 i; + print_log("%s", str); + for (i = 0; i < n; ++i) print_log("%d ", vect[i]); + print_log("\n"); +} + +void print_hex(const char *str, u8 *hex, u8 len) { + char buf[len * 3 + 1] = {'\0'}; + long i; + for (i = 0; i < len; ++i) sprintf((char *)&buf[i * 3], "%02x ", hex[i]); + + printf("%s %s\n", str, buf); +} diff --git a/xarm_api/src/xarm_core/instruction/uxbus_cmd.cc b/xarm_api/src/xarm_core/instruction/uxbus_cmd.cc new file mode 100755 index 00000000..b5cd5ed8 --- /dev/null +++ b/xarm_api/src/xarm_core/instruction/uxbus_cmd.cc @@ -0,0 +1,425 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#include "xarm/instruction/uxbus_cmd.h" +#include "xarm/instruction/servo3_config.h" +#include "xarm/instruction/uxbus_cmd_config.h" + +UxbusCmd::UxbusCmd(void) {} + +UxbusCmd::~UxbusCmd(void) {} + +int UxbusCmd::check_xbus_prot(u8 *data, u8 funcode) { return -11; } + +int UxbusCmd::send_pend(u8 funcode, int num, int timeout, u8 *rx_data) { + return -11; +} + +int UxbusCmd::send_xbus(u8 funcode, u8 *txdata, int num) { return -11; } + +void UxbusCmd::close(void) {} + +int UxbusCmd::set_nu8(u8 funcode, u8 *datas, int num) { + int ret = send_xbus(funcode, datas, num); + if (ret != 0) return UXBUS_STATE::ERR_NOTTCP; + ret = send_pend(funcode, 0, UXBUS_CONF::SET_TIMEOUT, NULL); + + return ret; +} + +int UxbusCmd::get_nu8(u8 funcode, u8 num, u8 *rx_data) { + int ret = send_xbus(funcode, 0, 0); + if (ret != 0) return UXBUS_STATE::ERR_NOTTCP; + ret = send_pend(funcode, num, UXBUS_CONF::GET_TIMEOUT, rx_data); + + return ret; +} + +int UxbusCmd::get_nu16(u8 funcode, u8 num, u16 *rx_data) { + u8 datas[num * 2]; + int ret = send_xbus(funcode, 0, 0); + if (ret != 0) return UXBUS_STATE::ERR_NOTTCP; + + ret = send_pend(funcode, num * 2, UXBUS_CONF::GET_TIMEOUT, datas); + for (int i = 0; i < num; i++) rx_data[i] = bin8_to_16(&datas[i * 2]); + + return ret; +} + +int UxbusCmd::set_nfp32(u8 funcode, fp32 *datas, u8 num) { + u8 hexdata[num * 4] = {0}; + + nfp32_to_hex(datas, hexdata, num); + int ret = send_xbus(funcode, hexdata, num * 4); + if (0 != ret) return UXBUS_STATE::ERR_NOTTCP; + ret = send_pend(funcode, 0, UXBUS_CONF::SET_TIMEOUT, NULL); + + return ret; +} + +int UxbusCmd::get_nfp32(u8 funcode, u8 num, fp32 *rx_data) { + u8 datas[num * 4] = {0}; + + int ret = send_xbus(funcode, 0, 0); + if (0 != ret) return UXBUS_STATE::ERR_NOTTCP; + ret = send_pend(funcode, num * 4, UXBUS_CONF::GET_TIMEOUT, datas); + hex_to_nfp32(datas, rx_data, num); + + return ret; +} + +int UxbusCmd::swop_nfp32(u8 funcode, fp32 tx_datas[], u8 txn, fp32 *rx_data, + u8 rxn) { + u8 hexdata[128] = {0}; + + nfp32_to_hex(tx_datas, hexdata, txn); + int ret = send_xbus(funcode, hexdata, txn * 4); + if (0 != ret) return UXBUS_STATE::ERR_NOTTCP; + ret = send_pend(funcode, rxn * 4, UXBUS_CONF::GET_TIMEOUT, hexdata); + hex_to_nfp32(hexdata, rx_data, rxn); + + return ret; +} + +int UxbusCmd::is_nfp32(u8 funcode, fp32 datas[], u8 txn, int *value) { + u8 hexdata[txn * 4] = {0}; + + nfp32_to_hex(datas, hexdata, txn); + int ret = send_xbus(funcode, hexdata, txn * 4); + if (0 != ret) return UXBUS_STATE::ERR_NOTTCP; + ret = send_pend(funcode, 1, UXBUS_CONF::GET_TIMEOUT, hexdata); + *value = hexdata[0]; + + return ret; +} + +int UxbusCmd::get_version(u8 rx_data[40]) { + return get_nu8(UXBUS_RG::GET_VERSION, 40, rx_data); +} + +int UxbusCmd::motion_en(u8 id, u8 value) { + u8 txdata[2]; + txdata[0] = id; + txdata[1] = value; + int ret = set_nu8(UXBUS_RG::MOTION_EN, txdata, 2); + return ret; +} + +int UxbusCmd::set_state(u8 value) { + u8 txdata[1]; + txdata[0] = value; + int ret = set_nu8(UXBUS_RG::SET_STATE, txdata, 1); + return ret; +} + +int UxbusCmd::get_state(u8 *rx_data) { + int ret = get_nu8(UXBUS_RG::GET_STATE, 1, rx_data); + return ret; +} + +int UxbusCmd::get_cmdnum(u16 *rx_data) { + int ret = get_nu16(UXBUS_RG::GET_CMDNUM, 1, rx_data); + return ret; +} + +int UxbusCmd::get_errcode(u8 *rx_data) { + int ret = get_nu8(UXBUS_RG::GET_ERROR, 2, rx_data); + return ret; +} + +int UxbusCmd::clean_err(void) { + u8 txdata[1]; + txdata[0] = 0; + int ret = set_nu8(UXBUS_RG::CLEAN_ERR, txdata, 0); + return ret; +} + +int UxbusCmd::clean_war(void) { + u8 txdata[1]; + txdata[0] = 0; + int ret = set_nu8(UXBUS_RG::CLEAN_WAR, txdata, 0); + return ret; +} + +int UxbusCmd::set_brake(u8 axis, u8 en) { + u8 txdata[2] = {0}; + txdata[0] = axis; + txdata[1] = en; + int ret = set_nu8(UXBUS_RG::SET_BRAKE, txdata, 2); + return ret; +} + +int UxbusCmd::set_mode(u8 value) { + u8 txdata[1]; + txdata[0] = value; + int ret = set_nu8(UXBUS_RG::SET_MODE, txdata, 1); + return ret; +} + +int UxbusCmd::move_line(fp32 mvpose[6], fp32 mvvelo, fp32 mvacc, fp32 mvtime) { + int i; + fp32 txdata[9] = {0}; + for (i = 0; i < 6; i++) txdata[i] = mvpose[i]; + txdata[6] = mvvelo; + txdata[7] = mvacc; + txdata[8] = mvtime; + int ret = set_nfp32(UXBUS_RG::MOVE_LINE, txdata, 9); + return ret; +} + +int UxbusCmd::move_lineb(fp32 mvpose[6], fp32 mvvelo, fp32 mvacc, fp32 mvtime, + fp32 mvradii) { + int i; + fp32 txdata[10] = {0}; + for (i = 0; i < 6; i++) txdata[i] = mvpose[i]; + txdata[6] = mvvelo; + txdata[7] = mvacc; + txdata[8] = mvtime; + txdata[9] = mvradii; + + int ret = set_nfp32(UXBUS_RG::MOVE_LINEB, txdata, 10); + return ret; +} + +int UxbusCmd::move_joint(fp32 mvjoint[7], fp32 mvvelo, fp32 mvacc, + fp32 mvtime) { + int i; + fp32 txdata[10] = {0}; + for (i = 0; i < 7; i++) txdata[i] = mvjoint[i]; + txdata[7] = mvvelo; + txdata[8] = mvacc; + txdata[9] = mvtime; + int ret = set_nfp32(UXBUS_RG::MOVE_JOINT, txdata, 10); + return ret; +} + +int UxbusCmd::move_gohome(fp32 mvvelo, fp32 mvacc, fp32 mvtime) { + fp32 txdata[3] = {0}; + txdata[0] = mvvelo; + txdata[1] = mvacc; + txdata[2] = mvtime; + int ret = set_nfp32(UXBUS_RG::MOVE_HOME, txdata, 3); + return ret; +} + +int UxbusCmd::move_servoj(fp32 mvjoint[7], fp32 mvvelo, fp32 mvacc, + fp32 mvtime) { + int i; + fp32 txdata[10] = {0}; + for (i = 0; i < 7; i++) txdata[i] = mvjoint[i]; + txdata[7] = mvvelo; + txdata[8] = mvacc; + txdata[9] = mvtime; + int ret = set_nfp32(UXBUS_RG::MOVE_SERVOJ, txdata, 10); + return ret; +} + +int UxbusCmd::sleep_instruction(fp32 sltime) { + fp32 txdata[1] = {0}; + txdata[0] = sltime; + int ret = set_nfp32(UXBUS_RG::SLEEP_INSTT, txdata, 1); + return ret; +} + +int UxbusCmd::set_tcp_jerk(fp32 jerk) { + fp32 txdata[1] = {0}; + txdata[0] = jerk; + int ret = set_nfp32(UXBUS_RG::SET_TCP_JERK, txdata, 1); + return ret; +} + +int UxbusCmd::set_tcp_maxacc(fp32 maxacc) { + fp32 txdata[1] = {0}; + txdata[0] = maxacc; + int ret = set_nfp32(UXBUS_RG::SET_TCP_MAXACC, txdata, 1); + return ret; +} + +int UxbusCmd::set_joint_jerk(fp32 jerk) { + fp32 txdata[1] = {0}; + txdata[0] = jerk; + int ret = set_nfp32(UXBUS_RG::SET_JOINT_JERK, txdata, 1); + return ret; +} + +int UxbusCmd::set_joint_maxacc(fp32 maxacc) { + fp32 txdata[1] = {0}; + txdata[0] = maxacc; + int ret = set_nfp32(UXBUS_RG::SET_JOINT_MAXACC, txdata, 1); + return ret; +} + +int UxbusCmd::set_tcp_offset(fp32 pose_offset[6]) { + return set_nfp32(UXBUS_RG::SET_TCP_OFFSET, pose_offset, 6); +} + +int UxbusCmd::clean_conf() { return set_nu8(UXBUS_RG::CLEAN_CONF, 0, 0); } + +int UxbusCmd::save_conf() { return set_nu8(UXBUS_RG::SAVE_CONF, 0, 0); } + +int UxbusCmd::get_tcp_pose(fp32 pose[6]) { + return get_nfp32(UXBUS_RG::GET_TCP_POSE, 6, pose); +} + +int UxbusCmd::get_joint_pose(fp32 angles[7]) { + return get_nfp32(UXBUS_RG::GET_JOINT_POS, 7, angles); +} + +int UxbusCmd::get_ik(fp32 pose[6], fp32 angles[7]) { + return swop_nfp32(UXBUS_RG::GET_IK, pose, 6, angles, 7); +} + +int UxbusCmd::get_fk(fp32 angles[7], fp32 pose[6]) { + return swop_nfp32(UXBUS_RG::GET_FK, angles, 7, pose, 6); +} + +int UxbusCmd::is_joint_limit(fp32 joint[7], int *value) { + return is_nfp32(UXBUS_RG::IS_JOINT_LIMIT, joint, 7, value); +} + +int UxbusCmd::is_tcp_limit(fp32 pose[6], int *value) { + return is_nfp32(UXBUS_RG::IS_TCP_LIMIT, pose, 6, value); +} + +int UxbusCmd::gripper_addr_w16(u8 id, u16 addr, fp32 value) { + u8 txdata[7]; + txdata[0] = id; + bin16_to_8(addr, &txdata[1]); + fp32_to_hex(value, &txdata[3]); + int ret = send_xbus(UXBUS_RG::GRIPP_W16B, txdata, 7); + if (0 != ret) return UXBUS_STATE::ERR_NOTTCP; + + ret = send_pend(UXBUS_RG::GRIPP_W16B, 0, UXBUS_CONF::GET_TIMEOUT, NULL); + return ret; +} + +int UxbusCmd::gripper_addr_r16(u8 id, u16 addr, fp32 *value) { + u8 txdata[3], rx_data[4]; + txdata[0] = id; + bin16_to_8(addr, &txdata[1]); + int ret = send_xbus(UXBUS_RG::GRIPP_R16B, txdata, 3); + if (0 != ret) return UXBUS_STATE::ERR_NOTTCP; + + ret = send_pend(UXBUS_RG::GRIPP_R16B, 4, UXBUS_CONF::GET_TIMEOUT, rx_data); + *value = bin8_to_16(rx_data); + return ret; +} + +int UxbusCmd::gripper_addr_w32(u8 id, u16 addr, fp32 value) { + u8 txdata[7]; + txdata[0] = id; + bin16_to_8(addr, &txdata[1]); + fp32_to_hex(value, &txdata[3]); + int ret = send_xbus(UXBUS_RG::GRIPP_W32B, txdata, 7); + if (0 != ret) return UXBUS_STATE::ERR_NOTTCP; + + ret = send_pend(UXBUS_RG::GRIPP_W32B, 0, UXBUS_CONF::GET_TIMEOUT, NULL); + return ret; +} + +int UxbusCmd::gripper_addr_r32(u8 id, u16 addr, fp32 *value) { + u8 txdata[3], rx_data[4]; + txdata[0] = id; + bin16_to_8(addr, &txdata[1]); + int ret = send_xbus(UXBUS_RG::GRIPP_R32B, txdata, 3); + if (0 != ret) return UXBUS_STATE::ERR_NOTTCP; + + ret = send_pend(UXBUS_RG::GRIPP_R32B, 4, UXBUS_CONF::GET_TIMEOUT, rx_data); + *value = bin8_to_32(rx_data); + return ret; +} + +int UxbusCmd::gripper_set_en(u16 value) { + return gripper_addr_w16(UXBUS_CONF::GRIPPER_ID, SERVO3_RG::CON_EN, value); +} + +int UxbusCmd::gripper_set_mode(u16 value) { + return gripper_addr_w16(UXBUS_CONF::GRIPPER_ID, SERVO3_RG::CON_MODE, value); +} + +int UxbusCmd::gripper_set_zero() { + return gripper_addr_w16(UXBUS_CONF::GRIPPER_ID, SERVO3_RG::MT_ZERO, 1); +} + +int UxbusCmd::gripper_get_pos(fp32 *pulse) { + return gripper_addr_r32(UXBUS_CONF::GRIPPER_ID, SERVO3_RG::CURR_POS, pulse); +} + +int UxbusCmd::gripper_set_pos(fp32 pulse) { + return gripper_addr_w32(UXBUS_CONF::GRIPPER_ID, SERVO3_RG::TAGET_POS, pulse); +} + +int UxbusCmd::gripper_set_posspd(fp32 speed) { + return gripper_addr_w16(UXBUS_CONF::GRIPPER_ID, SERVO3_RG::POS_SPD, speed); +} + +int UxbusCmd::gripper_get_errcode(u8 rx_data[2]) { + return get_nu8(UXBUS_RG::GPGET_ERR, 2, rx_data); +} + +int UxbusCmd::gripper_clean_err() { + return gripper_addr_w16(UXBUS_CONF::GRIPPER_ID, SERVO3_RG::RESET_ERR, 1); +} + +int UxbusCmd::servo_set_zero(u8 id) { + u8 txdata[1]; + txdata[0] = id; + int ret = set_nu8(UXBUS_RG::SERVO_ZERO, txdata, 1); + return ret; +} + +int UxbusCmd::servo_get_dbmsg(u8 rx_data[16]) { + int ret = get_nu8(UXBUS_RG::SERVO_DBMSG, 16, rx_data); + return ret; +} + +int UxbusCmd::servo_addr_w16(u8 id, u16 addr, fp32 value) { + u8 txdata[7]; + txdata[0] = id; + bin16_to_8(addr, &txdata[1]); + fp32_to_hex(value, &txdata[3]); + int ret = send_xbus(UXBUS_RG::SERVO_W16B, txdata, 7); + if (0 != ret) return UXBUS_STATE::ERR_NOTTCP; + + ret = send_pend(UXBUS_RG::SERVO_W16B, 0, UXBUS_CONF::GET_TIMEOUT, NULL); + return ret; +} + +int UxbusCmd::servo_addr_r16(u8 id, u16 addr, fp32 *value) { + u8 txdata[3], rx_data[4]; + txdata[0] = id; + bin16_to_8(addr, &txdata[1]); + int ret = send_xbus(UXBUS_RG::SERVO_R16B, txdata, 3); + if (0 != ret) return UXBUS_STATE::ERR_NOTTCP; + + ret = send_pend(UXBUS_RG::SERVO_R16B, 4, UXBUS_CONF::GET_TIMEOUT, rx_data); + *value = bin8_to_16(rx_data); + return ret; +} + +int UxbusCmd::servo_addr_w32(u8 id, u16 addr, fp32 value) { + u8 txdata[7]; + txdata[0] = id; + bin16_to_8(addr, &txdata[1]); + fp32_to_hex(value, &txdata[3]); + int ret = send_xbus(UXBUS_RG::SERVO_W32B, txdata, 7); + if (0 != ret) return UXBUS_STATE::ERR_NOTTCP; + + ret = send_pend(UXBUS_RG::SERVO_W32B, 0, UXBUS_CONF::GET_TIMEOUT, NULL); + return ret; +} + +int UxbusCmd::servo_addr_r32(u8 id, u16 addr, fp32 *value) { + u8 txdata[3], rx_data[4]; + txdata[0] = id; + bin16_to_8(addr, &txdata[1]); + int ret = send_xbus(UXBUS_RG::SERVO_R32B, txdata, 3); + if (0 != ret) return UXBUS_STATE::ERR_NOTTCP; + + ret = send_pend(UXBUS_RG::SERVO_R32B, 4, UXBUS_CONF::GET_TIMEOUT, rx_data); + *value = bin8_to_32(rx_data); + return ret; +} diff --git a/xarm_api/src/xarm_core/instruction/uxbus_cmd_ser.cc b/xarm_api/src/xarm_core/instruction/uxbus_cmd_ser.cc new file mode 100755 index 00000000..43998bf8 --- /dev/null +++ b/xarm_api/src/xarm_core/instruction/uxbus_cmd_ser.cc @@ -0,0 +1,63 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#include "xarm/instruction/uxbus_cmd_ser.h" + +#include + +#include "xarm/common/crc16.h" +#include "xarm/debug/debug_print.h" +#include "xarm/instruction/uxbus_cmd_config.h" + +UxbusCmdSer::UxbusCmdSer(SerialPort *arm_port) { arm_port_ = arm_port; } +UxbusCmdSer::~UxbusCmdSer(void) {} + +int UxbusCmdSer::check_xbus_prot(u8 *datas, u8 funcode) { + if (datas[3] & 0x40) + return UXBUS_STATE::ERR_CODE; + else if (datas[3] & 0x20) + return UXBUS_STATE::WAR_CODE; + else + return 0; +} + +int UxbusCmdSer::send_pend(u8 funcode, int num, int timeout, u8 *ret_data) { + int ret; + u8 rx_data[arm_port_->que_maxlen_] = {0}; + int times = timeout; + while (times) { + times -= 1; + ret = arm_port_->read_frame(rx_data); + if (ret != -1) { + ret = check_xbus_prot(rx_data, funcode); + for (int i = 0; i < num; i++) ret_data[i] = rx_data[i + 4]; + return ret; + } + usleep(1000); + } + return UXBUS_STATE::ERR_TOUT; +} + +int UxbusCmdSer::send_xbus(u8 funcode, u8 *datas, int num) { + int i; + u8 send_data[num + 4]; + + send_data[0] = UXBUS_CONF::MASTER_ID; + send_data[1] = UXBUS_CONF::SLAVE_ID; + send_data[2] = num + 1; + send_data[3] = funcode; + for (i = 0; i < num; i++) send_data[4 + i] = datas[i]; + + u16 crc = modbus_crc(send_data, 4 + num); + send_data[4 + num] = (u8)(crc & 0xFF); + send_data[5 + num] = (u8)((crc >> 8) & 0xFF); + + arm_port_->flush(); + int ret = arm_port_->write_frame(send_data, num + 6); + return ret; +} + +void UxbusCmdSer::close(void) { arm_port_->close_port(); } diff --git a/xarm_api/src/xarm_core/instruction/uxbus_cmd_tcp.cc b/xarm_api/src/xarm_core/instruction/uxbus_cmd_tcp.cc new file mode 100755 index 00000000..3a923c5f --- /dev/null +++ b/xarm_api/src/xarm_core/instruction/uxbus_cmd_tcp.cc @@ -0,0 +1,88 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#include "xarm/instruction/uxbus_cmd_tcp.h" + +#include + +#include "xarm/debug/debug_print.h" +#include "xarm/instruction/uxbus_cmd_config.h" + +UxbusCmdTcp::UxbusCmdTcp(SocketPort *arm_port) { + arm_port_ = arm_port; + bus_flag_ = TX2_BUS_FLAG_MIN_; + prot_flag_ = TX2_PROT_CON_; +} + +UxbusCmdTcp::~UxbusCmdTcp(void) {} + +int UxbusCmdTcp::check_xbus_prot(u8 *datas, u8 funcode) { + u8 *data_fp = &datas[4]; + + int sizeof_data = bin8_to_32(datas); + if (sizeof_data < 8 || sizeof_data >= arm_port_->que_maxlen_) + return UXBUS_STATE::ERR_LENG; + + int num = bin8_to_16(&data_fp[0]); + int prot = bin8_to_16(&data_fp[2]); + int len = bin8_to_16(&data_fp[4]); + int fun = data_fp[6]; + int state = data_fp[7]; + + int bus_flag = bus_flag_; + if (bus_flag == TX2_BUS_FLAG_MIN_) + bus_flag = TX2_BUS_FLAG_MAX_; + else + bus_flag -= 1; + + if (num != bus_flag) return UXBUS_STATE::ERR_NUM; + if (prot != TX2_PROT_CON_) return UXBUS_STATE::ERR_PROT; + if (fun != funcode) return UXBUS_STATE::ERR_FUN; + if (state & 0x40) return UXBUS_STATE::ERR_CODE; + if (state & 0x20) return UXBUS_STATE::WAR_CODE; + if (sizeof_data != len + 6) return UXBUS_STATE::ERR_LENG; + return 0; +} + +int UxbusCmdTcp::send_pend(u8 funcode, int num, int timeout, u8 *ret_data) { + int i; + int ret; + u8 rx_data[arm_port_->que_maxlen_] = {0}; + int times = timeout; + while (times) { + times -= 1; + ret = arm_port_->read_frame(rx_data); + if (ret != -1) { + ret = check_xbus_prot(rx_data, funcode); + for (i = 0; i < num; i++) ret_data[i] = rx_data[i + 8 + 4]; + // print_hex(" 3", rx_data, num + 8 + 4); + return ret; + } + usleep(1000); + } + return UXBUS_STATE::ERR_TOUT; +} + +int UxbusCmdTcp::send_xbus(u8 funcode, u8 *datas, int num) { + int len = num + 7; + u8 send_data[len]; + bin16_to_8(bus_flag_, &send_data[0]); + bin16_to_8(prot_flag_, &send_data[2]); + bin16_to_8(num + 1, &send_data[4]); + send_data[6] = funcode; + + for (int i = 0; i < num; i++) send_data[7 + i] = datas[i]; + arm_port_->flush(); + int ret = arm_port_->write_frame(send_data, len); + if (ret != len) return -1; + + bus_flag_ += 1; + if (bus_flag_ > TX2_BUS_FLAG_MAX_) bus_flag_ = TX2_BUS_FLAG_MIN_; + + return 0; +} + +void UxbusCmdTcp::close(void) { arm_port_->close_port(); } diff --git a/xarm_api/src/xarm_core/linux/network.cc b/xarm_api/src/xarm_core/linux/network.cc new file mode 100755 index 00000000..5caf16e3 --- /dev/null +++ b/xarm_api/src/xarm_core/linux/network.cc @@ -0,0 +1,89 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#include "xarm/linux/network.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#define DB_FLG "[net work] " +#define PRINT_ERR printf + +#define PERRNO(ret, db_flg, str) \ + { \ + if (-1 == ret) { \ + PRINT_ERR("%s%s\n", db_flg, str); \ + return -1; \ + } \ + \ +} + +int socket_init(char *local_ip, int port, u8 is_server) { + int sockfd = socket(AF_INET, SOCK_STREAM, 0); + PERRNO(sockfd, DB_FLG, "error: socket"); + + int on = 1; + int keepAlive = 1; // Turn on keepalive attribute + int keepIdle = 1; // If there is no data in n seconds, probe + int keepInterval = 1; // Detection interval,5 seconds + int keepCount = 3; // 3 detection attempts + struct timeval timeout = {2, 0}; + + int ret = + setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, (void *)&on, sizeof(on)); + PERRNO(ret, DB_FLG, "error: setsockopt"); + ret = setsockopt(sockfd, SOL_SOCKET, SO_KEEPALIVE, (void *)&keepAlive, + sizeof(keepAlive)); + PERRNO(ret, DB_FLG, "error: setsockopt"); + ret = setsockopt(sockfd, SOL_TCP, TCP_KEEPIDLE, (void *)&keepIdle, + sizeof(keepIdle)); + PERRNO(ret, DB_FLG, "error: setsockopt"); + ret = setsockopt(sockfd, SOL_TCP, TCP_KEEPINTVL, (void *)&keepInterval, + sizeof(keepInterval)); + PERRNO(ret, DB_FLG, "error: setsockopt"); + ret = setsockopt(sockfd, SOL_TCP, TCP_KEEPCNT, (void *)&keepCount, + sizeof(keepCount)); + PERRNO(ret, DB_FLG, "error: setsockopt"); + ret = setsockopt(sockfd, SOL_SOCKET, SO_SNDTIMEO, (char *)&timeout, + sizeof(struct timeval)); + PERRNO(ret, DB_FLG, "error: setsockopt"); + + if (is_server) { + struct sockaddr_in local_addr; + local_addr.sin_family = AF_INET; + local_addr.sin_port = htons(port); + local_addr.sin_addr.s_addr = inet_addr(local_ip); + ret = bind(sockfd, (struct sockaddr *)&local_addr, sizeof(local_addr)); + PERRNO(ret, DB_FLG, "error: bind"); + + int ret = listen(sockfd, 10); + PERRNO(ret, DB_FLG, "error: listen"); + } + return sockfd; +} + +s8 socket_connect_server(int *socket, char server_ip[], int server_port) { + struct sockaddr_in server_addr; + server_addr.sin_family = AF_INET; + server_addr.sin_port = htons(server_port); + inet_aton(server_ip, &server_addr.sin_addr); + int ret = + connect(*socket, (struct sockaddr *)&server_addr, sizeof(server_addr)); + PERRNO(ret, DB_FLG, "error: connect"); + return 0; +} + +s8 socket_send_data(int client_fp, u8 *data, u16 len) { + s8 ret = send(client_fp, (void *)data, len, 0); + if (ret == -1) PRINT_ERR(DB_FLG "error: socket_send_data\n"); + return ret; +} diff --git a/xarm_api/src/xarm_core/linux/thread.cc b/xarm_api/src/xarm_core/linux/thread.cc new file mode 100755 index 00000000..ccdc827c --- /dev/null +++ b/xarm_api/src/xarm_core/linux/thread.cc @@ -0,0 +1,25 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#include "xarm/linux/thread.h" + +#include + +#define PRINT_ERR printf + +pthread_t thread_init(fun_point_t fun_point, void *arg) { + pthread_t id; + pthread_attr_t attr; + + pthread_attr_init(&attr); + pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED); + char ret = pthread_create(&id, &attr, fun_point, arg); + if (0 != ret) PRINT_ERR("error: pthread create failes\n"); + + return id; +} + +void thread_delete(pthread_t id) { pthread_cancel(id); } diff --git a/xarm_api/src/xarm_core/port/serial.cc b/xarm_api/src/xarm_core/port/serial.cc new file mode 100755 index 00000000..e6826c2e --- /dev/null +++ b/xarm_api/src/xarm_core/port/serial.cc @@ -0,0 +1,229 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#include "xarm/port/serial.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "xarm/common/crc16.h" +#include "xarm/linux/thread.h" + +void SerialPort::recv_proc(void) { + u8 ch; + int ret; + while (state_ == 0) { + ret = read_char(&ch); + + if (ret >= 0) { + parse_put(&ch, 1); + continue; + } + usleep(1000); + } +} + +static void *recv_proc_(void *arg) { + SerialPort *my_this = (SerialPort *)arg; + + my_this->recv_proc(); + + pthread_exit(0); +} + +SerialPort::SerialPort(const char *port, int baud, int que_num, + int que_maxlen) { + que_num_ = que_num; + que_maxlen_ = que_maxlen; + rx_que_ = new QueueMemcpy(que_num_, que_maxlen_); + + int ret = init_serial(port, baud); + if (ret == -1) + state_ = -1; + else + state_ = 0; + + if (state_ == -1) return; + + UXBUS_PROT_FROMID_ = 0x55; + UXBUS_PROT_TOID_ = 0xAA; + flush(); + thread_id_ = thread_init(recv_proc_, this); +} + +SerialPort::~SerialPort(void) { + state_ = -1; + delete rx_que_; +} + +int SerialPort::is_ok(void) { return state_; } + +void SerialPort::flush(void) { + rx_que_->flush(); + rx_data_idx_ = 0; + rx_state_ = UXBUS_START_FROMID; +} + +int SerialPort::read_char(u8 *ch) { return (read(fp_, ch, 1) == 1) ? 0 : -1; } + +int SerialPort::read_frame(u8 *data) { + if (state_ != 0) return -1; + + if (rx_que_->size() == 0) return -1; + + rx_que_->pop(data); + return 0; +} + +int SerialPort::write_char(u8 ch) { + return ((write(fp_, &ch, 1) == 1) ? 0 : -1); +} + +int SerialPort::write_frame(u8 *data, int len) { + if (write(fp_, data, len) != len) return -1; + return 0; +} + +void SerialPort::close_port(void) { + state_ = -1; + close(fp_); + delete rx_que_; +} + +void SerialPort::parse_put(u8 *data, u16 len) { + u8 ch; + + for (int i = 0; i < len; i++) { + ch = data[i]; + // printf("---state = %d, ch = %x\n", rx_state_, ch); + switch (rx_state_) { + case UXBUS_START_FROMID: + if (UXBUS_PROT_FROMID_ == ch) { + rx_buf_[0] = ch; + rx_state_ = UXBUS_START_TOOID; + } + break; + + case UXBUS_START_TOOID: + if (UXBUS_PROT_TOID_ == ch) { + rx_buf_[1] = ch; + rx_state_ = UXBUS_STATE_LENGTH; + } else { + rx_state_ = UXBUS_START_FROMID; + } + break; + + case UXBUS_STATE_LENGTH: + if (0 < ch && ch < (que_maxlen_ - 5)) { + rx_buf_[2] = ch; + rx_length_ = ch; + rx_data_idx_ = 3; + rx_state_ = UXBUS_STATE_DATA; + } else { + rx_state_ = UXBUS_START_FROMID; + } + break; + + case UXBUS_STATE_DATA: + if (rx_data_idx_ < rx_length_ + 3) { + rx_buf_[rx_data_idx_++] = ch; + if (rx_data_idx_ == rx_length_ + 3) { + rx_state_ = UXBUS_STATE_CRC1; + } + } else { + rx_state_ = UXBUS_START_FROMID; + } + break; + + case UXBUS_STATE_CRC1: + rx_buf_[rx_length_ + 3] = ch; + rx_state_ = UXBUS_STATE_CRC2; + break; + + case UXBUS_STATE_CRC2: + u16 crc, crc_r; + rx_buf_[rx_length_ + 4] = ch; + crc = modbus_crc(rx_buf_, rx_length_ + 3); + crc_r = (rx_buf_[rx_length_ + 4] << 8) + rx_buf_[rx_length_ + 3]; + if (crc == crc_r) { + rx_que_->push(rx_buf_); + } + rx_state_ = UXBUS_START_FROMID; + break; + + default: + rx_state_ = UXBUS_START_FROMID; + break; + } + } +} + +int SerialPort::init_serial(const char *port, int baud) { + speed_t speed; + struct termios options; + + fp_ = open((const char *)port, O_RDWR | O_NOCTTY); + if (-1 == fp_) return -1; + + fcntl(fp_, F_SETFL, FNDELAY); + tcgetattr(fp_, &options); + bzero(&options, sizeof(options)); + + switch (baud) { + case 110: + speed = B110; + break; + case 300: + speed = B300; + break; + case 600: + speed = B600; + break; + case 1200: + speed = B1200; + break; + case 2400: + speed = B2400; + break; + case 4800: + speed = B4800; + break; + case 9600: + speed = B9600; + break; + case 19200: + speed = B19200; + break; + case 38400: + speed = B38400; + break; + case 57600: + speed = B57600; + break; + case 115200: + speed = B115200; + break; + case 921600: + speed = B921600; + break; + } + + cfsetispeed(&options, speed); + cfsetospeed(&options, speed); + + options.c_oflag &= ~OPOST; + options.c_cc[VTIME] = 200; + options.c_cc[VMIN] = 10; + tcsetattr(fp_, TCSANOW, &options); + return 0; +} diff --git a/xarm_api/src/xarm_core/port/socket.cc b/xarm_api/src/xarm_core/port/socket.cc new file mode 100755 index 00000000..2a4f60bb --- /dev/null +++ b/xarm_api/src/xarm_core/port/socket.cc @@ -0,0 +1,84 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#include "xarm/port/socket.h" + +#include +#include +#include + +#include "xarm/linux/network.h" +#include "xarm/linux/thread.h" + +void SocketPort::recv_proc(void) { + int num; + char recv_data[que_maxlen_]; + while (state_ == 0) { + bzero(recv_data, que_maxlen_); + num = recv(fp_, (void *)&recv_data[4], que_maxlen_ - 1, 0); + if (num <= 0) { + close(fp_); + printf("SocketPort::recv_proc exit, %d\n", fp_); + pthread_exit(0); + } + bin32_to_8(num, &recv_data[0]); + rx_que_->push(recv_data); + } +} + +static void *recv_proc_(void *arg) { + SocketPort *my_this = (SocketPort *)arg; + + my_this->recv_proc(); + + pthread_exit(0); +} + +SocketPort::SocketPort(char *server_ip, int server_port, int que_num, + int que_maxlen) { + que_num_ = que_num; + que_maxlen_ = que_maxlen; + state_ = -1; + rx_que_ = new QueueMemcpy(que_num_, que_maxlen_); + fp_ = socket_init((char *)" ", 0, 0); + if (fp_ == -1) return; + + int ret = socket_connect_server(&fp_, server_ip, server_port); + if (ret == -1) return; + + state_ = 0; + flush(); + thread_id_ = thread_init(recv_proc_, this); +} + +SocketPort::~SocketPort(void) { + state_ = -1; + delete rx_que_; +} + +int SocketPort::is_ok(void) { return state_; } + +void SocketPort::flush(void) { rx_que_->flush(); } + +int SocketPort::read_frame(u8 *data) { + if (state_ != 0) return -1; + + if (rx_que_->size() == 0) return -1; + + rx_que_->pop(data); + return 0; +} + +int SocketPort::write_frame(u8 *data, int len) { + int ret = socket_send_data(fp_, data, len); + return ret; +} + +void SocketPort::close_port(void) { + close(fp_); + state_ = -1; + delete rx_que_; +} diff --git a/xarm_api/src/xarm_core/report_data.cc b/xarm_api/src/xarm_core/report_data.cc new file mode 100755 index 00000000..df28124b --- /dev/null +++ b/xarm_api/src/xarm_core/report_data.cc @@ -0,0 +1,167 @@ +/* Copyright 2017 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jimy Zhang + ============================================================================*/ +#include "xarm/report_data.h" + +#include +#include + +#include "xarm/debug/debug_print.h" + +ReportDataNorm::ReportDataNorm(void) { + runing_ = 0; + mode_ = 0; + mt_brake_ = 0; + mt_able_ = 0; + err_ = 0; + war_ = 0; + cmdnum_ = 0; + total_num_ = 0; + + memset(angle_, 0, sizeof(angle_)); + memset(pose_, 0, sizeof(pose_)); + memset(tcp_offset_, 0, sizeof(tcp_offset_)); +} + +ReportDataNorm::~ReportDataNorm(void) {} + +int ReportDataNorm::flush_data(u8 *rx_data) { + u8 *data_fp = &rx_data[4]; + int sizeof_data = bin8_to_32(rx_data); + if (sizeof_data < 87) + { + printf(" len = %d \n", sizeof_data); + return -1; + } + + total_num_ = bin8_to_32(data_fp); + if (total_num_ != 87) return -2; + + runing_ = data_fp[4] & 0x0F; + mode_ = data_fp[4] >> 4; + mt_brake_ = data_fp[5]; + mt_able_ = data_fp[6]; + err_ = data_fp[7]; + war_ = data_fp[8]; + + hex_to_nfp32(&data_fp[9], angle_, 7); + hex_to_nfp32(&data_fp[37], pose_, 6); + cmdnum_ = bin8_to_16(&data_fp[61]); + hex_to_nfp32(&data_fp[63], tcp_offset_, 6); + return 0; +} + +void ReportDataNorm::print_data(void) { + printf("total = %d\n", total_num_); + printf("runing = %d\n", runing_); + printf("mode = %d\n", mode_); + printf("mt_brake= %x\n", mt_brake_); + printf("mt_able = %x\n", mt_able_); + printf("err&war = %d %d\n", err_, war_); + printf("cmdnum = %d\n", cmdnum_); + print_nvect("angle = ", angle_, 7); + print_nvect("pose = ", pose_, 6); + print_nvect("offset = ", tcp_offset_, 6); +} + +ReportDataRich::ReportDataRich(void) { + runing_ = 0; + mode_ = 0; + mt_brake_ = 0; + mt_able_ = 0; + err_ = 0; + war_ = 0; + cmdnum_ = 0; + total_num_ = 0; + + memset(angle_, 0, sizeof(angle_)); + memset(pose_, 0, sizeof(pose_)); + memset(tcp_offset_, 0, sizeof(tcp_offset_)); +} + +ReportDataRich::~ReportDataRich(void) {} + +int ReportDataRich::flush_data(u8 *rx_data) { + u8 *data_fp = &rx_data[4]; + int sizeof_data = bin8_to_32(rx_data); + if (sizeof_data < 187) { + printf("sizeof_data = %d\n", sizeof_data); + return -1; + } + + total_num_ = bin8_to_32(data_fp); + if (total_num_ != 187) return -2; + + runing_ = data_fp[4] & 0x0F; + mode_ = data_fp[4] >> 4; + mt_brake_ = data_fp[5]; + mt_able_ = data_fp[6]; + err_ = data_fp[7]; + war_ = data_fp[8]; + + hex_to_nfp32(&data_fp[9], angle_, 7); + hex_to_nfp32(&data_fp[37], pose_, 6); + cmdnum_ = bin8_to_16(&data_fp[61]); + hex_to_nfp32(&data_fp[63], tcp_offset_, 6); + + arm_type_ = data_fp[87]; + axis_num_ = data_fp[88]; + master_id_ = data_fp[89]; + slave_id_ = data_fp[90]; + motor_tid_ = data_fp[91]; + motor_fid_ = data_fp[92]; + memcpy(versions_, &data_fp[93], 30); + + hex_to_nfp32(&data_fp[123], trs_msg_, 5); + trs_jerk_ = trs_msg_[0]; + trs_accmin_ = trs_msg_[1]; + trs_accmax_ = trs_msg_[2]; + trs_velomin_ = trs_msg_[3]; + trs_velomax_ = trs_msg_[4]; + + hex_to_nfp32(&data_fp[143], p2p_msg_, 5); + p2p_jerk_ = p2p_msg_[0]; + p2p_accmin_ = p2p_msg_[1]; + p2p_accmax_ = p2p_msg_[2]; + p2p_velomin_ = p2p_msg_[3]; + p2p_velomax_ = p2p_msg_[4]; + + hex_to_nfp32(&data_fp[163], rot_msg_, 2); + rot_jerk_ = rot_msg_[0]; + rot_accmax_ = rot_msg_[1]; + + for (int i = 0; i < 17; i++) sv3msg_[i] = data_fp[171 + i]; + + return 0; +} + +void ReportDataRich::print_data(void) { + printf("versions= %s\n", versions_); + printf("total = %d\n", total_num_); + printf("runing = %d\n", runing_); + printf("mode = %d\n", mode_); + printf("mt_brake= %x\n", mt_brake_); + printf("mt_able = %x\n", mt_able_); + printf("err&war = %d %d\n", err_, war_); + printf("cmdnum = %d\n", cmdnum_); + print_nvect("angle = ", angle_, 7); + print_nvect("pose = ", pose_, 6); + print_nvect("offset = ", tcp_offset_, 6); + + printf("xarm_type = %d(axis%d)\n", arm_type_, axis_num_); + printf("xarm_msid = 0x%X 0x%X\n", master_id_, slave_id_); + printf("motor_tfid = 0x%X 0x%X\n", motor_tid_, motor_fid_); + + print_nvect("trs_msg = ", trs_msg_, 5); + print_nvect("p2p_msg = ", p2p_msg_, 5); + print_nvect("ros_msg = ", rot_msg_, 2); + + printf("ID 执行状态 错误代码\n"); + for (int i = 0; i < 8; i++) { + printf("%d %d 0x%X\n", i + 1, sv3msg_[i * 2], + sv3msg_[i * 2 + 1]); + } +} diff --git a/xarm_api/src/xarm_driver.cpp b/xarm_api/src/xarm_driver.cpp new file mode 100755 index 00000000..948e08d5 --- /dev/null +++ b/xarm_api/src/xarm_driver.cpp @@ -0,0 +1,245 @@ +/* Copyright 2018 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: waylon + ============================================================================*/ +#include +#include "xarm/instruction/uxbus_cmd_config.h" + +namespace xarm_api +{ + XARMDriver::~XARMDriver() + { + arm_cmd_->set_mode(XARM_MODE::POSE); + arm_cmd_->close(); + spinner.stop(); + } + + void XARMDriver::XARMDriverInit(ros::NodeHandle& root_nh, char *server_ip) + { + nh_ = root_nh; + // api command services: + motion_ctrl_server_ = nh_.advertiseService("motion_ctrl", &XARMDriver::MotionCtrlCB, this); + set_mode_server_ = nh_.advertiseService("set_mode", &XARMDriver::SetModeCB, this); + set_state_server_ = nh_.advertiseService("set_state", &XARMDriver::SetStateCB, this); + set_tcp_offset_server_ = nh_.advertiseService("set_tcp_offset", &XARMDriver::SetTCPOffsetCB, this); + go_home_server_ = nh_.advertiseService("go_home", &XARMDriver::GoHomeCB, this); + move_joint_server_ = nh_.advertiseService("move_joint", &XARMDriver::MoveJointCB, this); + move_lineb_server_ = nh_.advertiseService("move_lineb", &XARMDriver::MoveLinebCB, this); + move_line_server_ = nh_.advertiseService("move_line", &XARMDriver::MoveLineCB, this); + move_servoj_server_ = nh_.advertiseService("move_servoj", &XARMDriver::MoveServoJCB, this); + + // state feedback topics: + joint_state_ = nh_.advertise("joint_states", 10, true); + robot_rt_state_ = nh_.advertise("xarm_states", 10, true); + + arm_report_ = connect_tcp_report_norm(server_ip); + // ReportDataNorm norm_data_; + arm_cmd_ = connect_tcp_control(server_ip); + if (arm_cmd_ == NULL) + ROS_ERROR("Xarm Connection Failed!"); + } + + bool XARMDriver::MotionCtrlCB(xarm_msgs::SetAxis::Request& req, xarm_msgs::SetAxis::Response& res) + { + res.ret = arm_cmd_->motion_en(req.id, req.data); + if(req.data == 1) + { + res.message = "motion enable, ret = " + std::to_string(res.ret); + } + else + { + res.message = "motion disable, ret = " + std::to_string(res.ret); + } + return true; + } + + bool XARMDriver::SetModeCB(xarm_msgs::SetInt16::Request& req, xarm_msgs::SetInt16::Response& res) + { + res.ret = arm_cmd_->set_mode(req.data); + switch(req.data) + { + case XARM_MODE::POSE: + { + res.message = "pose mode, ret = " + std::to_string(res.ret); + }break; + case XARM_MODE::SERVO: + { + res.message = "servo mode, ret = " + std::to_string(res.ret); + }break; + case XARM_MODE::TEACH_CART: + { + res.message = "cartesian teach, ret = " + std::to_string(res.ret); + } break; + case XARM_MODE::TEACH_JOINT: + { + res.message = "joint teach , ret = " + std::to_string(res.ret); + } break; + default: + { + res.message = "the failed mode, ret = " + std::to_string(res.ret); + } + } + + return true; + } + + bool XARMDriver::SetStateCB(xarm_msgs::SetInt16::Request& req, xarm_msgs::SetInt16::Response& res) + { + res.ret = arm_cmd_->set_state(req.data); + switch(req.data) + { + case XARM_STATE::START: + { + res.message = "start, ret = " + std::to_string(res.ret); + }break; + case XARM_STATE::PAUSE: + { + res.message = "pause, ret = " + std::to_string(res.ret); + }break; + case XARM_STATE::STOP: + { + res.message = "stop, ret = " + std::to_string(res.ret); + }break; + default: + { + res.message = "the failed state, ret = " + std::to_string(res.ret); + } + } + + return true; + } + + bool XARMDriver::SetTCPOffsetCB(xarm_msgs::TCPOffset::Request &req, xarm_msgs::TCPOffset::Response &res) + { + float offsets[6] = {req.x, req.y, req.z, req.roll, req.pitch, req.yaw}; + res.ret = arm_cmd_->set_tcp_offset(offsets); + res.message = "set tcp offset: ret = " + std::to_string(res.ret); + return true; + } + + bool XARMDriver::GoHomeCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res) + { + res.ret = arm_cmd_->move_gohome(req.mvvelo, req.mvacc, req.mvtime); + res.message = "go home, ret = " + std::to_string(res.ret); + return true; + } + + bool XARMDriver::MoveJointCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res) + { + float joint[1][7]; + int index = 0; + if(req.pose.size() != 7) + { + res.ret = req.pose.size(); + res.message = "pose parameters incorrect!"; + return true; + } + else + { + for(index = 0; index < 7; index++) + { + joint[0][index] = req.pose[index]; + } + } + + res.ret = arm_cmd_->move_joint(joint[0], req.mvvelo, req.mvacc, req.mvtime); + res.message = "move joint, ret = " + std::to_string(res.ret); + return true; + } + + bool XARMDriver::MoveLineCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res) + { + float pose[1][6]; + int index = 0; + if(req.pose.size() != 6) + { + res.ret = -1; + res.message = "parameters incorrect!"; + return true; + } + else + { + for(index = 0; index < 6; index++) + { + pose[0][index] = req.pose[index]; + } + } + + res.ret = arm_cmd_->move_line(pose[0], req.mvvelo, req.mvacc, req.mvtime); + res.message = "move line, ret = " + std::to_string(res.ret); + return true; + } + + bool XARMDriver::MoveLinebCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res) + { + float pose[1][6]; + int index = 0; + if(req.pose.size() != 6) + { + res.ret = -1; + res.message = "parameters incorrect!"; + return true; + } + else + { + for(index = 0; index < 6; index++) + { + pose[0][index] = req.pose[index]; + } + } + + res.ret = arm_cmd_->move_lineb(pose[0], req.mvvelo, req.mvacc, req.mvtime, req.mvradii); + res.message = "move lineb, ret = " + std::to_string(res.ret); + return true; + } + + bool XARMDriver::MoveServoJCB(xarm_msgs::Move::Request &req, xarm_msgs::Move::Response &res) + { + float pose[1][7]; + int index = 0; + if(req.pose.size() != 7) + { + res.ret = -1; + res.message = "parameters incorrect!"; + return true; + } + else + { + for(index = 0; index < 7; index++) + { + pose[0][index] = req.pose[index]; + } + } + + res.ret = arm_cmd_->move_servoj(pose[0], req.mvvelo, req.mvacc, req.mvtime); + res.message = "move servoj, ret = " + std::to_string(res.ret); + return true; + } + + void XARMDriver::pub_robot_msg(xarm_msgs::RobotMsg rm_msg) + { + robot_rt_state_.publish(rm_msg); + } + + void XARMDriver::pub_joint_state(sensor_msgs::JointState js_msg) + { + joint_state_.publish(js_msg); + } + + int XARMDriver::get_frame(void) + { + int ret; + ret = arm_report_->read_frame(rx_data_); + return ret; + } + + int XARMDriver::get_rich_data(ReportDataNorm &norm_data) + { + int ret; + ret = norm_data_.flush_data(rx_data_); + norm_data = norm_data_; + return ret; + } +} diff --git a/xarm_api/src/xarm_driver_node.cpp b/xarm_api/src/xarm_driver_node.cpp new file mode 100755 index 00000000..5b0cfe8a --- /dev/null +++ b/xarm_api/src/xarm_driver_node.cpp @@ -0,0 +1,165 @@ +/* Copyright 2018 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: waylon + ============================================================================*/ +#include +#include +#include "xarm/connect.h" +#include "xarm/report_data.h" + +class XarmRTConnection +{ + public: + XarmRTConnection(ros::NodeHandle& root_nh, char *server_ip, xarm_api::XARMDriver &drv) + { + root_nh.getParam("DOF", joint_num_); + root_nh.getParam("joint_names", joint_name_); + ip = server_ip; + xarm_driver = drv; + xarm_driver.XARMDriverInit(root_nh, server_ip); + thread_id = thread_init(thread_proc, (void *)this); + + } + + void thread_run(void) + { + int ret; + int err_num; + int rxcnt; + int i; + int first_cycle = 1; + double d, prev_angle[joint_num_]; + + ros::Rate r(REPORT_RATE_HZ); // 50Hz + + while(true) + { + // usleep(5000); + ret = xarm_driver.get_frame(); + if (ret != 0) continue; + + ret = xarm_driver.get_rich_data(norm_data); + if (ret == 0) + { + rxcnt++; + + now = ros::Time::now(); + js_msg.header.stamp = now; + js_msg.header.frame_id = "real-time data"; + js_msg.name.resize(joint_num_); + js_msg.position.resize(joint_num_); + js_msg.velocity.resize(joint_num_); + js_msg.effort.resize(joint_num_); + for(i = 0; i < joint_num_; i++) + { + d = (double)norm_data.angle_[i]; + js_msg.name[i] = joint_name_[i]; + js_msg.position[i] = d; + + if (first_cycle) + { + js_msg.velocity[i] = 0; + first_cycle = 0; + } + else + { + js_msg.velocity[i] = (js_msg.position[i] - prev_angle[i])*REPORT_RATE_HZ; + } + + js_msg.effort[i] = 0; + + prev_angle[i] = d; + } + + xarm_driver.pub_joint_state(js_msg); + + rm_msg.state = norm_data.runing_; + rm_msg.mode = norm_data.mode_; + rm_msg.cmdnum = norm_data.cmdnum_; + rm_msg.err = norm_data.err_; + rm_msg.warn = norm_data.war_; + rm_msg.mt_brake = norm_data.mt_brake_; + rm_msg.mt_able = norm_data.mt_able_; + rm_msg.angle.resize(joint_num_); + + for(i = 0; i < joint_num_; i++) + { + /* set the float precision*/ + double d = norm_data.angle_[i]; + double r; + char str[8]; + sprintf(str, "%0.3f", d); + sscanf(str, "%lf", &r); + rm_msg.angle[i] = r; + } + for(i = 0; i < 6; i++) + { + rm_msg.pose[i] = norm_data.pose_[i]; + rm_msg.offset[i] = norm_data.tcp_offset_[i]; + } + xarm_driver.pub_robot_msg(rm_msg); + } + else + { + printf("Error: real_data.flush_data failed, ret = %d\n", ret); + err_num++; + } + + r.sleep(); + } + } + + static void* thread_proc(void *arg) + { + XarmRTConnection* pThreadTest=(XarmRTConnection*)arg; + pThreadTest->thread_run(); + } + + public: + pthread_t thread_id; + char *ip; + ros::Time now; + SocketPort *arm_report; + ReportDataNorm norm_data; + sensor_msgs::JointState js_msg; + xarm_api::XARMDriver xarm_driver; + xarm_msgs::RobotMsg rm_msg; + + int joint_num_; + std::vector joint_name_; + constexpr static const double REPORT_RATE_HZ = 10; /* 10Hz, same with norm_report frequency */ +}; + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "xarm_driver_node"); + + ros::NodeHandle n; + + xarm_api::XARMDriver driver; + ROS_INFO("start xarm driver"); + + std::string robot_ip = "192.168.1.121"; + if (!n.hasParam("xarm_robot_ip")) + { + ROS_INFO("No param named 'xarm_robot_ip'"); + ROS_INFO("Use default robot ip = 192.168.1.121"); + } + else + { + n.getParam("xarm_robot_ip", robot_ip); + } + + char server_ip[20]; + strcpy(server_ip,robot_ip.c_str()); + XarmRTConnection rt_connect(n, server_ip, driver); + + // ros::spin(); + ros::waitForShutdown(); + + printf("end"); + + return 0; +} diff --git a/xarm_api/src/xarm_ros_client.cpp b/xarm_api/src/xarm_ros_client.cpp new file mode 100755 index 00000000..f8b6bb1d --- /dev/null +++ b/xarm_api/src/xarm_ros_client.cpp @@ -0,0 +1,200 @@ +/* Copyright 2018 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jason + ============================================================================*/ +#include + +namespace xarm_api{ + +XArmROSClient::XArmROSClient(ros::NodeHandle& nh) +{ + nh_ = nh; + motion_ctrl_client_ = nh_.serviceClient("motion_ctrl"); + set_mode_client_ = nh_.serviceClient("set_mode"); + set_state_client_ = nh_.serviceClient("set_state"); + set_tcp_offset_client_ = nh_.serviceClient("set_tcp_offset"); + go_home_client_ = nh_.serviceClient("go_home"); + move_lineb_client_ = nh_.serviceClient("move_lineb"); + move_line_client_ = nh_.serviceClient("move_line"); + move_joint_client_ = nh_.serviceClient("move_joint"); + move_servoj_client_ = nh_.serviceClient("move_servoj",true); // persistent connection for servoj + +} + +int XArmROSClient::motionEnable(short en) +{ + set_axis_srv_.request.id = 8; + set_axis_srv_.request.data = en; + if(motion_ctrl_client_.call(set_axis_srv_)) + { + ROS_INFO("%s\n", set_axis_srv_.response.message.c_str()); + return set_axis_srv_.response.ret; + } + else + { + ROS_ERROR("Failed to call service motion_ctrl"); + return 1; + } + +} + +int XArmROSClient::setState(short state) +{ + set_int16_srv_.request.data = state; + if(set_state_client_.call(set_int16_srv_)) + { + ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); + return set_int16_srv_.response.ret; + } + else + { + ROS_ERROR("Failed to call service set_state"); + return 1; + } +} + +int XArmROSClient::setMode(short mode) +{ + set_int16_srv_.request.data = mode; + if(set_mode_client_.call(set_int16_srv_)) + { + ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); + return set_int16_srv_.response.ret; + } + else + { + ROS_ERROR("Failed to call service set_mode"); + return 1; + } + +} + +int XArmROSClient::setServoJ(const std::vector& joint_cmd) +{ + servoj_msg_.request.mvvelo = 0; + servoj_msg_.request.mvacc = 0; + servoj_msg_.request.mvtime = 0; + servoj_msg_.request.pose = joint_cmd; + + if(move_servoj_client_.call(servoj_msg_)) + { + // ROS_INFO("%s\n", servoj_msg_.response.message.c_str()); + return servoj_msg_.response.ret; + } + else + { + ROS_ERROR("Failed to call service move_servoj"); + return 1; + } +} + +int XArmROSClient::setTCPOffset(const std::vector& tcp_offset) +{ + if(tcp_offset.size() != 6) + { + ROS_ERROR("Set tcp offset service parameter should be 6-element Cartesian offset!"); + return 1; + } + + offset_srv_.request.x = tcp_offset[0]; + offset_srv_.request.y = tcp_offset[1]; + offset_srv_.request.z = tcp_offset[2]; + offset_srv_.request.roll = tcp_offset[3]; + offset_srv_.request.pitch = tcp_offset[4]; + offset_srv_.request.yaw = tcp_offset[5]; + + if(set_tcp_offset_client_.call(offset_srv_)) + { + return offset_srv_.response.ret; + } + else + { + ROS_ERROR("Failed to call service set_tcp_offset"); + return 1; + } + +} + +int XArmROSClient::goHome(float jnt_vel_rad, float jnt_acc_rad) +{ + move_srv_.request.mvvelo = jnt_vel_rad; + move_srv_.request.mvacc = jnt_acc_rad; + move_srv_.request.mvtime = 0; + + if(go_home_client_.call(move_srv_)) + { + return move_srv_.response.ret; + } + else + { + ROS_ERROR("Failed to call service go_home"); + return 1; + } +} + +int XArmROSClient::moveJoint(const std::vector& joint_cmd, float jnt_vel_rad, float jnt_acc_rad) +{ + move_srv_.request.mvvelo = jnt_vel_rad; + move_srv_.request.mvacc = jnt_acc_rad; + move_srv_.request.mvtime = 0; + move_srv_.request.pose = joint_cmd; + + if(move_joint_client_.call(move_srv_)) + { + return move_srv_.response.ret; + } + else + { + ROS_ERROR("Failed to call service move_joint"); + return 1; + } +} + +int XArmROSClient::moveLine(const std::vector& cart_cmd, float cart_vel_mm, float cart_acc_mm) +{ + move_srv_.request.mvvelo = cart_vel_mm; + move_srv_.request.mvacc = cart_acc_mm; + move_srv_.request.mvtime = 0; + move_srv_.request.pose = cart_cmd; + + if(move_joint_client_.call(move_srv_)) + { + return move_srv_.response.ret; + } + else + { + ROS_ERROR("Failed to call service move_line"); + return 1; + } +} + +int XArmROSClient::moveLineB(int num_of_pnts, const std::vector cart_cmds[], float cart_vel_mm, float cart_acc_mm, float radii) +{ + move_srv_.request.mvvelo = cart_vel_mm; + move_srv_.request.mvacc = cart_acc_mm; + move_srv_.request.mvtime = 0; + move_srv_.request.mvradii = radii; + + for(int i=0; i + ============================================================================*/ +#include + +#include "xarm/connect.h" +#include "xarm/report_data.h" + +int main(int argc, char **argv) { + if (argc < 2) { + printf("Please enter IP address\n"); + return 0; + } + char *server_ip = argv[1]; + SocketPort *arm_report = connect_tcp_report_norm(server_ip); + if (arm_report == NULL) return 0; + + int rxcnt = 0; + ReportDataNorm *norm_data = new ReportDataNorm(); + + u8 rx_data[1280]; + int ret; + int err_num = 0; + while (1) { + usleep(1000); + + ret = arm_report->read_frame(rx_data); + if (ret != 0) continue; + ret = norm_data->flush_data(rx_data); + + if (ret == 0) { + rxcnt++; + printf("\n【normal report】: len = %d, rxcnt = %d, err_num = %d\n", + bin8_to_32(rx_data), rxcnt, err_num); + norm_data->print_data(); + } else { + printf("Error: norm_data.flush_data failed, ret = %d\n", ret); + err_num++; + } + } +} diff --git a/xarm_api/test/move_test.cpp b/xarm_api/test/move_test.cpp new file mode 100755 index 00000000..22653bd1 --- /dev/null +++ b/xarm_api/test/move_test.cpp @@ -0,0 +1,168 @@ +/* Copyright 2018 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: waylon + ============================================================================*/ +#include "ros/ros.h" +#include + +int go_home_test(xarm_msgs::Move srv, ros::ServiceClient client) +{ + srv.request.mvvelo = 20.0 / 57.0; + srv.request.mvacc = 1000; + srv.request.mvtime = 0; + if(client.call(srv)) + { + ROS_INFO("%s\n", srv.response.message.c_str()); + } + else + { + ROS_ERROR("Failed to call service go home"); + return 1; + } + return 0; +} + +int servoj_test(xarm_msgs::Move srv, ros::ServiceClient client) +{ + std::vector joint[2] = {{0, 0, 0, 0, 0, 0, 0}, + {0, -0.3, 0, 0, 0, 0, 0}}; + + srv.request.mvvelo = 50; + srv.request.mvacc = 100; + srv.request.mvtime = 0; + for (int i = 0; i < 2; i++) + { + srv.request.pose = joint[i]; + if(client.call(srv)) + { + ROS_INFO("%s\n", srv.response.message.c_str()); + } + else + { + ROS_ERROR("Failed to call service move_lineb"); + return 1; + } + sleep(2); + } + return 0; +} + +int move_lineb_test(xarm_msgs::Move srv, ros::ServiceClient client) +{ + std::vector pose[5] = { {300, 0, 100, -3.14, 0, 0}, + {300, 100, 100, -3.14, 0, 0}, + {400, 100, 100, -3.14, 0, 0}, + {400, -100, 100, -3.14, 0, 0}, + {300, 0, 100, -3.14, 0, 0}}; + + srv.request.mvvelo = 50; + srv.request.mvacc = 1000; + srv.request.mvtime = 0; + srv.request.mvradii = 0; + for (int i = 0; i < 5; i++) + { + srv.request.pose = pose[i]; + if(client.call(srv)) + { + ROS_INFO("%s\n", srv.response.message.c_str()); + } + else + { + ROS_ERROR("Failed to call service move_lineb"); + return 1; + } + } + return 0; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "xarm_move_test"); + ros::NodeHandle nh; + ros::ServiceClient motion_ctrl_client_ = nh.serviceClient("motion_ctrl"); + ros::ServiceClient set_mode_client_ = nh.serviceClient("set_mode"); + ros::ServiceClient set_state_client_ = nh.serviceClient("set_state"); + ros::ServiceClient go_home_client_ = nh.serviceClient("go_home"); + ros::ServiceClient move_lineb_client_ = nh.serviceClient("move_lineb"); + ros::ServiceClient move_servoj_client_ = nh.serviceClient("move_servoj"); + + xarm_msgs::SetAxis set_axis_srv_; + xarm_msgs::SetInt16 set_int16_srv_; + xarm_msgs::Move move_srv_; + + set_axis_srv_.request.id = 1; + set_axis_srv_.request.data = 1; + if(motion_ctrl_client_.call(set_axis_srv_)) + { + ROS_INFO("%s\n", set_axis_srv_.response.message.c_str()); + } + else + { + ROS_ERROR("Failed to call service motion_ctrl"); + return 1; + } + + set_int16_srv_.request.data = 0; + if(set_mode_client_.call(set_int16_srv_)) + { + ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); + } + else + { + ROS_ERROR("Failed to call service set_mode"); + return 1; + } + + set_int16_srv_.request.data = 0; + if(set_state_client_.call(set_int16_srv_)) + { + ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); + } + else + { + ROS_ERROR("Failed to call service set_state"); + return 1; + } + + if(go_home_test(move_srv_, go_home_client_) == 1) + return 1; + + if(move_lineb_test(move_srv_, move_lineb_client_) == 1) + return 1; + + /*wait for execution to complete*/ + sleep(15); + + if(go_home_test(move_srv_, go_home_client_) == 1) + return 1; + + /*wait for going to home*/ + sleep(3); + + set_int16_srv_.request.data = 1; + if(set_mode_client_.call(set_int16_srv_)) + { + ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); + } + else + { + ROS_ERROR("Failed to call service set_mode"); + return 1; + } + + set_int16_srv_.request.data = 0; + if(set_state_client_.call(set_int16_srv_)) + { + ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); + } + else + { + ROS_ERROR("Failed to call service set_state"); + return 1; + } + + if(servoj_test(move_srv_, move_servoj_client_) == 1) + return 1; +} \ No newline at end of file diff --git a/xarm_bringup/CMakeLists.txt b/xarm_bringup/CMakeLists.txt new file mode 100755 index 00000000..7de4551a --- /dev/null +++ b/xarm_bringup/CMakeLists.txt @@ -0,0 +1,195 @@ +cmake_minimum_required(VERSION 2.8.3) +project(xarm_bringup) + +## 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) + +## 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 # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES xarm_bringup +# CATKIN_DEPENDS other_catkin_pkg +# 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}/xarm_bringup.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/xarm_bringup_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_xarm_bringup.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/xarm_bringup/launch/xarm6_server.launch b/xarm_bringup/launch/xarm6_server.launch new file mode 100644 index 00000000..59745f78 --- /dev/null +++ b/xarm_bringup/launch/xarm6_server.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/xarm_bringup/launch/xarm7_server.launch b/xarm_bringup/launch/xarm7_server.launch new file mode 100644 index 00000000..4aace018 --- /dev/null +++ b/xarm_bringup/launch/xarm7_server.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/xarm_bringup/launch/xarm_driver_common.launch b/xarm_bringup/launch/xarm_driver_common.launch new file mode 100755 index 00000000..3f78ec74 --- /dev/null +++ b/xarm_bringup/launch/xarm_driver_common.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/xarm_bringup/launch/xarm_robot.launch b/xarm_bringup/launch/xarm_robot.launch new file mode 100755 index 00000000..bf7ee458 --- /dev/null +++ b/xarm_bringup/launch/xarm_robot.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/xarm_bringup/package.xml b/xarm_bringup/package.xml new file mode 100755 index 00000000..96147b3f --- /dev/null +++ b/xarm_bringup/package.xml @@ -0,0 +1,16 @@ + + + xarm_bringup + 0.0.0 + The xarm_bringup package + + waylon + BSD + + catkin + + xarm_api + + xarm_api + + diff --git a/xarm_controller/CMakeLists.txt b/xarm_controller/CMakeLists.txt index 063e430d..a76420b1 100755 --- a/xarm_controller/CMakeLists.txt +++ b/xarm_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3) project(xarm_controller) ## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +add_compile_options(-std=c++11) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -10,7 +10,20 @@ project(xarm_controller) find_package(catkin REQUIRED COMPONENTS controller_manager joint_state_controller - robot_state_publisher + # robot_state_publisher + # cmake_modules + control_msgs + control_toolbox + xarm_api + xarm_msgs + hardware_interface + # joint_limits_interface + # roscpp + sensor_msgs + std_msgs + # trajectory_msgs + # transmission_interface + # urdf ) ## System dependencies are found with CMake's conventions @@ -103,7 +116,7 @@ find_package(catkin REQUIRED COMPONENTS ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include +# INCLUDE_DIRS include # LIBRARIES xarm7_control # CATKIN_DEPENDS controller_manager joint_state_controller robot_state_publisher # DEPENDS system_lib @@ -201,4 +214,6 @@ include_directories( add_executable(sample_motion src/sample_motion.cpp) target_link_libraries(sample_motion ${catkin_LIBRARIES}) +add_executable(xarm_traj_controller src/xarm_hardware_interface.cpp) +target_link_libraries(xarm_traj_controller xarm_ros_client ${catkin_LIBRARIES}) diff --git a/xarm_controller/config/xarm6_position_controllers.yaml b/xarm_controller/config/xarm6_position_controllers.yaml new file mode 100755 index 00000000..a08f5d8e --- /dev/null +++ b/xarm_controller/config/xarm6_position_controllers.yaml @@ -0,0 +1,44 @@ +xarm: + # Publish all joint states ----------------------------------- + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + + # Position Controllers --------------------------------------- + joint1_position_controller: + type: position_controllers/JointPositionController + joint: joint1 + pid: {p: 1200.0, i: 5.0, d: 10.0} + joint2_position_controller: + type: position_controllers/JointPositionController + joint: joint2 + pid: {p: 1400.0, i: 5.0, d: 10.0} + joint3_position_controller: + type: position_controllers/JointPositionController + joint: joint3 + pid: {p: 1200.0, i: 5.0, d: 5.0} + joint4_position_controller: + type: position_controllers/JointPositionController + joint: joint4 + pid: {p: 850.0, i: 3.0, d: 5.0} + joint5_position_controller: + type: position_controllers/JointPositionController + joint: joint5 + pid: {p: 500.0, i: 3.0, d: 1.0} + joint6_position_controller: + type: position_controllers/JointPositionController + joint: joint6 + pid: {p: 500.0, i: 1.0, d: 1.0} + + + # No Pid gains specified error fix + gazebo_ros_control: + pid_gains: + joint1: {p: 1200.0, i: 5.0, d: 10.0} + joint2: {p: 1400.0, i: 5.0, d: 10.0} + joint3: {p: 1200.0, i: 5.0, d: 5.0} + joint4: {p: 850.0, i: 3.0, d: 5.0} + joint5: {p: 500.0, i: 3.0, d: 1.0} + joint6: {p: 500.0, i: 1.0, d: 1.0} + + diff --git a/xarm_controller/config/xarm6_traj_controller.yaml b/xarm_controller/config/xarm6_traj_controller.yaml new file mode 100755 index 00000000..9f8e6d45 --- /dev/null +++ b/xarm_controller/config/xarm6_traj_controller.yaml @@ -0,0 +1,37 @@ +xarm: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + + xarm6_traj_controller: + type: position_controllers/JointTrajectoryController + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + constraints: + goal_time: 0.5 + stopped_velocity_tolerance: 0.05 + joint1: {trajectory: 1, goal: 0.1} + joint2: {trajectory: 1, goal: 0.1} + joint3: {trajectory: 1, goal: 0.1} + joint4: {trajectory: 1, goal: 0.1} + joint5: {trajectory: 1, goal: 0.1} + joint6: {trajectory: 1, goal: 0.1} + stop_trajectory_duration: 0.2 + state_publish_rate: 25 + action_monitor_rate: 10 + + # No Pid gains specified error fix + gazebo_ros_control: + pid_gains: + joint1: {p: 1200.0, i: 5.0, d: 10.0} + joint2: {p: 1400.0, i: 5.0, d: 10.0} + joint3: {p: 1200.0, i: 5.0, d: 5.0} + joint4: {p: 850.0, i: 3.0, d: 5.0} + joint5: {p: 500.0, i: 3.0, d: 1.0} + joint6: {p: 500.0, i: 1.0, d: 1.0} + diff --git a/xarm_controller/config/xarm7_traj_controller.yaml b/xarm_controller/config/xarm7_traj_controller.yaml new file mode 100755 index 00000000..2c24ff6f --- /dev/null +++ b/xarm_controller/config/xarm7_traj_controller.yaml @@ -0,0 +1,40 @@ +xarm: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + + xarm7_traj_controller: + type: position_controllers/JointTrajectoryController + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 + constraints: + goal_time: 0.5 + stopped_velocity_tolerance: 0.05 + joint1: {trajectory: 1, goal: 0.1} + joint2: {trajectory: 1, goal: 0.1} + joint3: {trajectory: 1, goal: 0.1} + joint4: {trajectory: 1, goal: 0.1} + joint5: {trajectory: 1, goal: 0.1} + joint6: {trajectory: 1, goal: 0.1} + joint7: {trajectory: 1, goal: 0.1} + stop_trajectory_duration: 0.2 + state_publish_rate: 25 + action_monitor_rate: 10 + + # No Pid gains specified error fix + gazebo_ros_control: + pid_gains: + joint1: {p: 1200.0, i: 5.0, d: 10.0} + joint2: {p: 1400.0, i: 5.0, d: 10.0} + joint3: {p: 1200.0, i: 5.0, d: 5.0} + joint4: {p: 850.0, i: 3.0, d: 5.0} + joint5: {p: 500.0, i: 3.0, d: 1.0} + joint6: {p: 500.0, i: 1.0, d: 1.0} + joint7: {p: 300.0, i: 0.05, d: 1.0} + diff --git a/xarm_controller/exec/kill_ros_gazebo b/xarm_controller/exec/kill_ros_gazebo deleted file mode 100755 index a53118f7..00000000 --- a/xarm_controller/exec/kill_ros_gazebo +++ /dev/null @@ -1,8 +0,0 @@ -#!/bin/bash -# Kill all gazebo_related process with one shot -ps -ef | awk '/ros/ && !/awk/ {print $2}' | xargs -r kill -9 -ps -ef | awk '/gzserver/ && !/awk/ {print $2}' | xargs -r kill -9 -ps -ef | awk '/gzclient/ && !/awk/ {print $2}' | xargs -r kill -9 -ps -ef | awk '/controller_manager/ && !/awk/ {print $2}' | xargs -r kill -9 - -echo "Finished" diff --git a/xarm_controller/launch/xarm6_control.launch b/xarm_controller/launch/xarm6_control.launch new file mode 100755 index 00000000..d8aedce3 --- /dev/null +++ b/xarm_controller/launch/xarm6_control.launch @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/xarm_controller/launch/xarm7_control.launch b/xarm_controller/launch/xarm7_control.launch index 727ebcd9..2174c4e6 100755 --- a/xarm_controller/launch/xarm7_control.launch +++ b/xarm_controller/launch/xarm7_control.launch @@ -1,17 +1,17 @@ - + - - - - + - - + + + + + + xarm_controller 0.0.0 The xarm_controller package - - - - jason + BSD - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin controller_manager joint_state_controller robot_state_publisher + hardware_interface + sensor_msgs controller_manager joint_state_controller robot_state_publisher controller_manager joint_state_controller robot_state_publisher + hardware_interface + sensor_msgs - - - - - - diff --git a/xarm_controller/src/sample_motion.cpp b/xarm_controller/src/sample_motion.cpp index bc6c887a..b6c78906 100755 --- a/xarm_controller/src/sample_motion.cpp +++ b/xarm_controller/src/sample_motion.cpp @@ -21,19 +21,19 @@ int main(int argc, char **argv) ros::NodeHandle n; - ros::Publisher angle1_pub = n.advertise("/xarm/joint1_position_controller/command", 500); + ros::Publisher angle1_pub = n.advertise("/xarm/joint1_position_controller/command", 100); - ros::Publisher angle2_pub = n.advertise("/xarm/joint2_position_controller/command", 500); + ros::Publisher angle2_pub = n.advertise("/xarm/joint2_position_controller/command", 100); - ros::Publisher angle3_pub = n.advertise("/xarm/joint3_position_controller/command", 500); + ros::Publisher angle3_pub = n.advertise("/xarm/joint3_position_controller/command", 100); - ros::Publisher angle4_pub = n.advertise("/xarm/joint4_position_controller/command", 500); + ros::Publisher angle4_pub = n.advertise("/xarm/joint4_position_controller/command", 100); - ros::Publisher angle5_pub = n.advertise("/xarm/joint5_position_controller/command", 500); + ros::Publisher angle5_pub = n.advertise("/xarm/joint5_position_controller/command", 100); - ros::Publisher angle6_pub = n.advertise("/xarm/joint6_position_controller/command", 500); + ros::Publisher angle6_pub = n.advertise("/xarm/joint6_position_controller/command", 100); - ros::Publisher angle7_pub = n.advertise("/xarm/joint7_position_controller/command", 500); + ros::Publisher angle7_pub = n.advertise("/xarm/joint7_position_controller/command", 100); ros::Rate loop_rate(PUB_RATE); @@ -51,11 +51,17 @@ int main(int argc, char **argv) double acc_t = 0.0; double cmd_before_dec = 0.0; + if(!n.hasParam("DOF")) + { + ROS_ERROR("No DOF parameter specified! Could not give the right command"); + exit(-1); + } + + int robot_dof; + n.getParam("DOF",robot_dof); + while (ros::ok()) { - /** - * This is a message object. You stuff it with data, and then publish it. - */ if (i <= ACC_TIME * PUB_RATE /*&& dir == -1*/) { @@ -91,26 +97,62 @@ int main(int argc, char **argv) msg.data = angle_cmd; - // Give J2 and J4 same angle_cmd from calculation - angle4_pub.publish(msg); + switch(robot_dof) + { + case 7: + { + // Give J2 and J4 same angle_cmd from calculation + angle4_pub.publish(msg); + + angle2_pub.publish(msg); + + // Give J1 and J5 twice as angle_cmd from calculation + msg.data = msg.data * 2; + + angle1_pub.publish(msg); + + angle5_pub.publish(msg); + + // Other joint command fixed to zero + msg.data = 0.0; + + angle3_pub.publish(msg); + + angle6_pub.publish(msg); + + angle7_pub.publish(msg); + + break; + } + case 6: + { + // Give J2 and J3 same angle_cmd from calculation + angle3_pub.publish(msg); - angle2_pub.publish(msg); + angle2_pub.publish(msg); - // Give J1 and J5 twice as angle_cmd from calculation - msg.data = msg.data * 2; + // Give J1 and J4 twice as angle_cmd from calculation + msg.data = msg.data * 2; - angle1_pub.publish(msg); + angle1_pub.publish(msg); - angle5_pub.publish(msg); + angle4_pub.publish(msg); - // Other joint command fixed to zero - msg.data = 0.0; + // Other joint command fixed to zero + msg.data = 0.0; - angle3_pub.publish(msg); + angle5_pub.publish(msg); - angle6_pub.publish(msg); + angle6_pub.publish(msg); - angle7_pub.publish(msg); + break; + } + default: + { + ROS_ERROR("DOF parameter not correct, please check!"); + exit(-1); + } + } ros::spinOnce(); diff --git a/xarm_controller/src/xarm_hardware_interface.cpp b/xarm_controller/src/xarm_hardware_interface.cpp new file mode 100755 index 00000000..aec01722 --- /dev/null +++ b/xarm_controller/src/xarm_hardware_interface.cpp @@ -0,0 +1,199 @@ +/* Copyright 2018 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jason Peng + ============================================================================*/ +#ifndef __XARM_HARDWARE_INTERFACE_H__ +#define __XARM_HARDWARE_INTERFACE_H__ + +// ros_control +#include +#include +#include +#include +#include +// ROS +#include +#include +#include +#include +// for mutex +#include +// xarm +#include "xarm/instruction/uxbus_cmd_config.h" +#include "xarm_ros_client.h" + +namespace xarm_control +{ + +const std::string jnt_state_topic = "joint_states"; + +class XArmHWInterface : public hardware_interface::RobotHW +{ +public: + XArmHWInterface(unsigned int dof, std::vector& jnt_names, const std::string& robot_ip, ros::NodeHandle &root_nh):dof_(dof),xarm(root_nh) + { + jnt_names_ = jnt_names; + clientInit(robot_ip, root_nh); + }; + ~XArmHWInterface(); + void read(); + void write(); + + /* TODO: + virtual bool prepareSwitch(const std::list& start_list, + const std::list& stop_list) { return true; } + virtual void doSwitch(const std::list& , + const std::list& ) {}*/ + +private: + unsigned int dof_; + std::vector jnt_names_; + std::vector position_cmd_; + std::vector position_cmd_float_; + std::vector velocity_cmd_; + std::vector effort_cmd_; + + std::vector position_fdb_; + std::vector velocity_fdb_; + std::vector effort_fdb_; + + xarm_api::XArmROSClient xarm; + + hardware_interface::JointStateInterface js_interface_; + hardware_interface::EffortJointInterface ej_interface_; + hardware_interface::PositionJointInterface pj_interface_; + hardware_interface::VelocityJointInterface vj_interface_; + + ros::Subscriber pos_sub_, vel_sub_, effort_sub_; + + void clientInit(const std::string& robot_ip, ros::NodeHandle &root_nh); + void pos_fb_cb(const sensor_msgs::JointState::ConstPtr& data); + +}; + +void XArmHWInterface::clientInit(const std::string& robot_ip, ros::NodeHandle &root_nh) +{ + position_cmd_.resize(dof_); + position_cmd_float_.resize(7); // command vector must have 7 dimention! + position_fdb_.resize(dof_); + velocity_cmd_.resize(dof_); + velocity_fdb_.resize(dof_); + effort_cmd_.resize(dof_); + effort_fdb_.resize(dof_); + + pos_sub_ = root_nh.subscribe(jnt_state_topic, 100, &XArmHWInterface::pos_fb_cb, this); + + for(unsigned int j=0; j < dof_; j++) + { + // Create joint state interface for all joints + js_interface_.registerHandle(hardware_interface::JointStateHandle(jnt_names_[j], &position_fdb_[j], &velocity_fdb_[j], &effort_fdb_[j])); + + hardware_interface::JointHandle joint_handle; + joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(jnt_names_[j]),&position_cmd_[j]); + pj_interface_.registerHandle(joint_handle); + } + + registerInterface(&js_interface_); + registerInterface(&pj_interface_); + + int ret1 = xarm.motionEnable(1); + int ret2 = xarm.setMode(XARM_MODE::SERVO); + int ret3 = xarm.setState(XARM_STATE::START); + + if(ret1 || ret2 || ret3) + { + ROS_ERROR("The Xarm may not be properly connected or hardware error exists, PLEASE CHECK or RESTART HARDWARE!!!"); + ROS_ERROR(" "); + ROS_ERROR("Did you specify the correct ros param xarm_robot_ip ? Exitting..."); + exit(1); + } + +} + +XArmHWInterface::~XArmHWInterface() +{ + xarm.setMode(XARM_MODE::POSE); +} + +void XArmHWInterface::pos_fb_cb(const sensor_msgs::JointState::ConstPtr& data) +{ + + for(int j=0; jposition[j]; + velocity_fdb_[j] = data->velocity[j]; + effort_fdb_[j] = data->effort[j]; + } +} + +void XArmHWInterface::read() +{ + +} + +void XArmHWInterface::write() +{ + for(int k=0; k jnt_names; + int xarm_dof = 0; + if(!nh.hasParam("DOF")) + { + ROS_ERROR("ROS Parameter xarm_dof not specified!"); + } + if(!nh.hasParam("xarm_robot_ip")) + { + ROS_ERROR("ROS Parameter xarm_robot_ip not specified!"); + } + + nh.getParam("DOF", xarm_dof); + nh.getParam("xarm_robot_ip", ip); + nh.getParam("joint_names", jnt_names); + + ros::service::waitForService("motion_ctrl"); + ros::service::waitForService("set_state"); + ros::service::waitForService("set_mode"); + ros::service::waitForService("move_servoj"); + ros::Duration(1.0).sleep(); + xarm_control::XArmHWInterface xarm_hw(xarm_dof, jnt_names, ip, nh); + controller_manager::ControllerManager cm(&xarm_hw, nh); + + ros::AsyncSpinner spinner(4); + spinner.start(); + + // IMPORTANT: DO NOT REMOVE THIS DELAY !!! + /* Wait for correct initial position to be updated to ros_controller */ + ros::Duration(2.0).sleep(); + + ros::Time ts = ros::Time::now(); + while (ros::ok()) + { + ros::Duration elapsed = ros::Time::now() - ts; + ts = ros::Time::now(); + // xarm_hw.read(); + cm.update(ts, elapsed); + xarm_hw.write(); + r.sleep(); + } + spinner.stop(); + return 0; +} + +#endif diff --git a/xarm_description/cad/base.stl b/xarm_description/cad/base.stl deleted file mode 100755 index f4443ef6..00000000 Binary files a/xarm_description/cad/base.stl and /dev/null differ diff --git a/xarm_description/cad/link1.stl b/xarm_description/cad/link1.stl deleted file mode 100755 index 7077b402..00000000 Binary files a/xarm_description/cad/link1.stl and /dev/null differ diff --git a/xarm_description/cad/link2.stl b/xarm_description/cad/link2.stl deleted file mode 100755 index 4be94158..00000000 Binary files a/xarm_description/cad/link2.stl and /dev/null differ diff --git a/xarm_description/cad/link3.stl b/xarm_description/cad/link3.stl deleted file mode 100755 index 604527e0..00000000 Binary files a/xarm_description/cad/link3.stl and /dev/null differ diff --git a/xarm_description/cad/link4.stl b/xarm_description/cad/link4.stl deleted file mode 100755 index b187e7bd..00000000 Binary files a/xarm_description/cad/link4.stl and /dev/null differ diff --git a/xarm_description/cad/link5.stl b/xarm_description/cad/link5.stl deleted file mode 100755 index bad72d67..00000000 Binary files a/xarm_description/cad/link5.stl and /dev/null differ diff --git a/xarm_description/cad/link6.stl b/xarm_description/cad/link6.stl deleted file mode 100755 index ee4be449..00000000 Binary files a/xarm_description/cad/link6.stl and /dev/null differ diff --git a/xarm_description/cad/link7.stl b/xarm_description/cad/link7.stl deleted file mode 100755 index a9f46825..00000000 Binary files a/xarm_description/cad/link7.stl and /dev/null differ diff --git a/xarm_description/launch/display.rviz b/xarm_description/launch/display.rviz new file mode 100755 index 00000000..dac75f15 --- /dev/null +++ b/xarm_description/launch/display.rviz @@ -0,0 +1,174 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 565 + - 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: "" +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 + - 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 + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + 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: world + 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: 3.12220931 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.62539804 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.18357229 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002c4000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 550 + Y: 198 diff --git a/xarm_description/launch/xarm6_rviz_display.launch b/xarm_description/launch/xarm6_rviz_display.launch new file mode 100755 index 00000000..2ee3d4cb --- /dev/null +++ b/xarm_description/launch/xarm6_rviz_display.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/xarm_description/launch/xarm6_upload.launch b/xarm_description/launch/xarm6_upload.launch new file mode 100755 index 00000000..be5be248 --- /dev/null +++ b/xarm_description/launch/xarm6_upload.launch @@ -0,0 +1,11 @@ + + + + + + + + diff --git a/xarm_description/launch/xarm7_rviz_display.launch b/xarm_description/launch/xarm7_rviz_display.launch index 982ed1f2..533b33b0 100755 --- a/xarm_description/launch/xarm7_rviz_display.launch +++ b/xarm_description/launch/xarm7_rviz_display.launch @@ -12,6 +12,10 @@ - + + + + + diff --git a/xarm_description/meshes/xarm6/visual/base.stl b/xarm_description/meshes/xarm6/visual/base.stl new file mode 100755 index 00000000..8267395b Binary files /dev/null and b/xarm_description/meshes/xarm6/visual/base.stl differ diff --git a/xarm_description/meshes/xarm6/visual/link1.stl b/xarm_description/meshes/xarm6/visual/link1.stl new file mode 100755 index 00000000..285bff79 Binary files /dev/null and b/xarm_description/meshes/xarm6/visual/link1.stl differ diff --git a/xarm_description/meshes/xarm6/visual/link2.stl b/xarm_description/meshes/xarm6/visual/link2.stl new file mode 100755 index 00000000..2b249a75 Binary files /dev/null and b/xarm_description/meshes/xarm6/visual/link2.stl differ diff --git a/xarm_description/meshes/xarm6/visual/link3.stl b/xarm_description/meshes/xarm6/visual/link3.stl new file mode 100755 index 00000000..9a719025 Binary files /dev/null and b/xarm_description/meshes/xarm6/visual/link3.stl differ diff --git a/xarm_description/meshes/xarm6/visual/link4.stl b/xarm_description/meshes/xarm6/visual/link4.stl new file mode 100755 index 00000000..1fa1d6cc Binary files /dev/null and b/xarm_description/meshes/xarm6/visual/link4.stl differ diff --git a/xarm_description/meshes/xarm6/visual/link5.stl b/xarm_description/meshes/xarm6/visual/link5.stl new file mode 100755 index 00000000..7f6db90c Binary files /dev/null and b/xarm_description/meshes/xarm6/visual/link5.stl differ diff --git a/xarm_description/meshes/xarm6/visual/link6.stl b/xarm_description/meshes/xarm6/visual/link6.stl new file mode 100755 index 00000000..12ea005f Binary files /dev/null and b/xarm_description/meshes/xarm6/visual/link6.stl differ diff --git a/xarm_description/package.xml b/xarm_description/package.xml index a9606361..a7cc74b6 100755 --- a/xarm_description/package.xml +++ b/xarm_description/package.xml @@ -6,7 +6,7 @@ Jimy Zhang Jimy Zhang - + Jason Peng BSD diff --git a/xarm_description/urdf/xarm6.gazebo.xacro b/xarm_description/urdf/xarm6.gazebo.xacro new file mode 100755 index 00000000..99ed6c0d --- /dev/null +++ b/xarm_description/urdf/xarm6.gazebo.xacro @@ -0,0 +1,36 @@ + + + + + + + true + + + + true + + + + true + + + + true + + + + true + + + + true + + + + true + + + + + diff --git a/xarm_description/urdf/xarm6.transmission.xacro b/xarm_description/urdf/xarm6.transmission.xacro new file mode 100755 index 00000000..fc695d9e --- /dev/null +++ b/xarm_description/urdf/xarm6.transmission.xacro @@ -0,0 +1,75 @@ + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + reduction + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + reduction + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + reduction + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + reduction + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + reduction + + + + + + diff --git a/xarm_description/urdf/xarm6.urdf.xacro b/xarm_description/urdf/xarm6.urdf.xacro new file mode 100755 index 00000000..78b974ed --- /dev/null +++ b/xarm_description/urdf/xarm6.urdf.xacro @@ -0,0 +1,297 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/xarm_description/urdf/xarm6_robot.urdf.xacro b/xarm_description/urdf/xarm6_robot.urdf.xacro new file mode 100755 index 00000000..fc9c881a --- /dev/null +++ b/xarm_description/urdf/xarm6_robot.urdf.xacro @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/xarm_description/urdf/xarm7.urdf.xacro b/xarm_description/urdf/xarm7.urdf.xacro index ba5114b1..a6d9791b 100755 --- a/xarm_description/urdf/xarm7.urdf.xacro +++ b/xarm_description/urdf/xarm7.urdf.xacro @@ -245,7 +245,7 @@ - + - + - + \ No newline at end of file diff --git a/xarm_description/urdf/xarm7_robot.urdf.xacro b/xarm_description/urdf/xarm7_robot.urdf.xacro index 136a2e0e..14b4a5e3 100755 --- a/xarm_description/urdf/xarm7_robot.urdf.xacro +++ b/xarm_description/urdf/xarm7_robot.urdf.xacro @@ -21,11 +21,11 @@ diff --git a/xarm_gazebo/CMakeLists.txt b/xarm_gazebo/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/xarm_gazebo/launch/xarm6_beside_table.launch b/xarm_gazebo/launch/xarm6_beside_table.launch new file mode 100755 index 00000000..2ab755e3 --- /dev/null +++ b/xarm_gazebo/launch/xarm6_beside_table.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/xarm_gazebo/launch/xarm7_beside_table.launch b/xarm_gazebo/launch/xarm7_beside_table.launch index 6a683389..e4ef19a8 100755 --- a/xarm_gazebo/launch/xarm7_beside_table.launch +++ b/xarm_gazebo/launch/xarm7_beside_table.launch @@ -1,10 +1,11 @@ - - + + - + + @@ -12,17 +13,18 @@ - + + effort_control:=$(arg effort_control)" /> - + + diff --git a/xarm_gazebo/package.xml b/xarm_gazebo/package.xml old mode 100644 new mode 100755 index f91588e1..0002511f --- a/xarm_gazebo/package.xml +++ b/xarm_gazebo/package.xml @@ -13,7 +13,7 @@ - TODO + BSD diff --git a/xarm_msgs/CMakeLists.txt b/xarm_msgs/CMakeLists.txt new file mode 100755 index 00000000..0c908544 --- /dev/null +++ b/xarm_msgs/CMakeLists.txt @@ -0,0 +1,200 @@ +cmake_minimum_required(VERSION 2.8.3) +project(xarm_msgs) + +## 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 + message_generation + message_runtime + 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 + RobotMsg.msg + ) + +## Generate services in the 'srv' folder + add_service_files( + FILES + Move.srv + SetAxis.srv + SetInt16.srv + TCPOffset.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 xarm_msgs + CATKIN_DEPENDS std_msgs message_generation message_runtime +# 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}/xarm_msgs.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/xarm_msgs_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_xarm_msgs.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/xarm_msgs/ReadMe.md b/xarm_msgs/ReadMe.md new file mode 100755 index 00000000..46cdde3f --- /dev/null +++ b/xarm_msgs/ReadMe.md @@ -0,0 +1,37 @@ +# Service Introduction +  The services and messages defined in this package is based on the programming API. To offer a way for users to call them by ROS interface. Services provided are: + +## motion_ctrl +  To enable or disable the servo control of any joint.(message type: ***xarm_msgs::SetAxis***) +## set_mode +  To set operation mode. (message type: ***xarm_msgs::SetInt16***) +  * 0 for POSE mode, the robot will be position controlled. Trajectory will be planned by XArm Controller. +  * 1 for SERVOJ mode, the robot will be commanded by servo_j function, fast & immediate execution like a step response, use this if user can generate properly interpolated trajectory. +  * 2 for TEACH_JOINT mode, Gravity compensated mode, no position control. + +## set_state +  To set robot state. (message type: ***xarm_msgs::SetInt16***) +  * 0 for READY/START state, robot must be in this state to perform any motion. +  * 3 for PAUSE state, robot motion will be suspended. +  * 4 for STOP state, if error occurs or configuration changes, robot will switch to this state. + +## go_home +  Robot will go to home position with specified velocity and acceleration.(message type: ***xarm_msgs::Move***) + +## move_line +  Robot TCP will move to Caetesian target point with a straight line trajectory. Under specified Cartesian velocity and acceleartion. (message type: ***xarm_msgs::Move***) + +## move_lineb +  Given a set of targets, robot will move to final target through middle points, at each middle point, 2 straight-line trajectory will be blended with specified radius. (message type: ***xarm_msgs::Move***) + +## move_joint +  Given all desired joint positions, and max joint angular velocity/acceleration, robot will move to joint space target. (message type: ***xarm_msgs::Move***) + +## move_servoj: +  Used in SERVOJ mode, and if user have trajectory planned, call this service with high enough rate. Each trajectory point will be executed fast and immediately. (message type: ***xarm_msgs::Move***) + +***Please check the inside [srv](./srv/) files for detailed data information.*** + +# Feedback Status Message + +Refer to [RobotMsg](./msg/RobotMsg.msg) for robot feedback information contents published through topic "/xarm_states". \ No newline at end of file diff --git a/xarm_msgs/msg/RobotMsg.msg b/xarm_msgs/msg/RobotMsg.msg new file mode 100755 index 00000000..e76f2dd3 --- /dev/null +++ b/xarm_msgs/msg/RobotMsg.msg @@ -0,0 +1,38 @@ +# feedback information of the controlled robot + +# state of robot: +# 1 – RUNNING, executing motion command. +# 2 – SLEEPING, not in execution, but ready to move. +# 3 – PAUSED, paused in the middle of unfinished motion. +# 4 – STOPPED, not ready for any motion commands. +int16 state + +# mode of robot: +# 0 for POSITION mode.(position control by xarm controller box, execute api standard commands) +# 1 for SERVOJ mode. (Immediate execution towards received joint space target, like a step response) +# 2 for TEACHING_JOINT mode. (Gravity compensated mode, easy for teaching) +int16 mode + +# cmdnum: number of commands waiting in the buffer. +int16 cmdnum + +# mt_brake: if translated to binary digits, each bit represent one axis, 1 for brake enabled, 0 for brake disabled +int16 mt_brake + +# mt_able: if translated to binary digits, each bit represent one axis, 1 for servo control enabled, 0 for servo disabled +int16 mt_able + +# error code (if non-zero) +int16 err + +# warning code (if non-zero) +int16 warn + +# current joint angles expressed in radian. +float32[] angle + +# current TCP Cartesian position expressed in mm(position), radian(orientation) +float32[6] pose + +# TCP offset from center of flange, with respect to tool frame. +float32[6] offset \ No newline at end of file diff --git a/xarm_msgs/package.xml b/xarm_msgs/package.xml new file mode 100755 index 00000000..c455eb48 --- /dev/null +++ b/xarm_msgs/package.xml @@ -0,0 +1,19 @@ + + + xarm_msgs + 0.0.0 + The xarm_msgs package + waylon + BSD + catkin + std_msgs + message_generation + message_runtime + + message_generation + message_runtime + + std_msgs + std_msgs + + diff --git a/xarm_msgs/srv/Move.srv b/xarm_msgs/srv/Move.srv new file mode 100755 index 00000000..14a84bc6 --- /dev/null +++ b/xarm_msgs/srv/Move.srv @@ -0,0 +1,28 @@ +# request: command specification for motion executions. +# Units: +# joint space/angles: radian, radian/s and radian/s^2. +# Cartesian space: mm, mm/s, and mm/s^2. +# time: sec + +# pose： target coordinate. +# For Joint Space target，pose dimention is the number of joints. element as each target joint position. +# For Cartesian target: pose dimention is 6 for (x, y, z, roll, pitch, yaw) +float32[] pose + +# mvvelo: specified maximum velocity during execution. linear or angular velocity +float32 mvvelo +# mvacc: specified maximum acceleration during execution. linear or angular acceleration. +float32 mvacc +# mvtime: currently do not have any special meaning, please just give it 0. +float32 mvtime +# mvradii: this is special for move_ineb service, meaning the blending radius between 2 straight path trajectories, 0 for no blend. +float32 mvradii + +--- + +# response: +# ret is 0 for successful execution and others for errors or warnings occured +# message is a string returned by function, indicating execution status. + +int16 ret +string message \ No newline at end of file diff --git a/xarm_msgs/srv/SetAxis.srv b/xarm_msgs/srv/SetAxis.srv new file mode 100755 index 00000000..06732f25 --- /dev/null +++ b/xarm_msgs/srv/SetAxis.srv @@ -0,0 +1,15 @@ +# request: for enabling / disabling motion control of one(or all) joint. +# id: 1-7 for target joint number, or 8 for all joints +# data: 0 for disabling servo control, 1 for enabling servo control. + +int16 id +int16 data + +--- + +# response: +# ret is 0 for successful execution and others for errors or warnings occured +# message is a string returned by function, indicating execution status. + +int16 ret +string message \ No newline at end of file diff --git a/xarm_msgs/srv/SetInt16.srv b/xarm_msgs/srv/SetInt16.srv new file mode 100755 index 00000000..8982ee7a --- /dev/null +++ b/xarm_msgs/srv/SetInt16.srv @@ -0,0 +1,13 @@ +# request: set robot state or mode. +# data: int value indicating command robot state or execution mode. + +int16 data + +----- + +# response: +# ret is 0 for successful execution and others for errors or warnings occured +# message is a string returned by function, indicating execution status. + +int16 ret +string message diff --git a/xarm_msgs/srv/TCPOffset.srv b/xarm_msgs/srv/TCPOffset.srv new file mode 100755 index 00000000..e1a63dfb --- /dev/null +++ b/xarm_msgs/srv/TCPOffset.srv @@ -0,0 +1,14 @@ +# to give robot floating point number Cartesian TCP offset, based on initial Tool Frame located at flange center. + +float32 x +float32 y +float32 z + +float32 roll +float32 pitch +float32 yaw + +----- + +int16 ret +string message diff --git a/xarm_planner/CMakeLists.txt b/xarm_planner/CMakeLists.txt new file mode 100755 index 00000000..430e0526 --- /dev/null +++ b/xarm_planner/CMakeLists.txt @@ -0,0 +1,217 @@ +cmake_minimum_required(VERSION 2.8.3) +project(xarm_planner) + +## 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_core + moveit_visual_tools + moveit_ros_planning + moveit_ros_planning_interface + pluginlib + geometric_shapes + message_generation + geometry_msgs + 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 + pose_plan.srv + joint_plan.srv + exec_plan.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 + geometry_msgs + std_msgs + # Or other packages containing msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES xarm_planner + CATKIN_DEPENDS message_runtime geometry_msgs 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}/xarm_planner.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/xarm_planner_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_xarm_planner.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(xarm_simple_planner src/xarm_simple_planner.cpp) +add_dependencies(xarm_simple_planner ${${PROJECT_NAME}_EXPORTED_TARGETS}) +target_link_libraries(xarm_simple_planner ${catkin_LIBRARIES}) + +add_executable(xarm_simple_planner_test src/xarm_simple_planner_test.cpp) +add_dependencies(xarm_simple_planner_test ${${PROJECT_NAME}_EXPORTED_TARGETS}) +target_link_libraries(xarm_simple_planner_test ${catkin_LIBRARIES}) \ No newline at end of file diff --git a/xarm_planner/ReadMe.md b/xarm_planner/ReadMe.md new file mode 100755 index 00000000..94c71acb --- /dev/null +++ b/xarm_planner/ReadMe.md @@ -0,0 +1,54 @@ +# Package Introduction +  This package is intended to provide users a demo programming interface to use moveit!, instead of just using the GUI. To use the API better, users are encouraged to go through [Moveit tutorial](http://docs.ros.org/kinetic/api/moveit_tutorials/html/). +  Inside the package, 'xarm_simple_planner' is just a basic implementation of the Move_group interface, if higher level configurations (constraints, switch kinematic solver or planners, etc) are needed, user can fully explore Moveit abilities and implement a sofisticated version. +  For simplified Chinese instructions: [简体中文版](./ReadMe_cn.md) + +# Usage +## Launch the simple planner node: +If you want to try it in simulation, run: +```bash + $ roslaunch xarm_planner xarm_planner_rviz_sim.launch +``` +Or, if you would work with real xArm, run: +```bash + $ roslaunch xarm_planner xarm_planner_realHW.launch robot_ip:= robot_dof:=<7/6/5> +``` +Argument 'robot_dof' specifies the number of joints of your xArm (default is 7).This node can provide services for planning request in Cartesian target and joint space target. Service definition can be found in srv folder. User can call the services to let planner solve the path to specified target point, and retrieve the boolean result as successful or not. Once the node is launched, user can try in command-line first, something like: + +## For joint-space planning request: +```bash + $ rosservice call xarm_joint_plan 'target: [1.0, -0.5, 0.0, -0.3, 0.0, 0.0, 0.5]' +``` +The target elements in this case correspond to each joint target angle in radians. + +## For Cartesian-space planning request: +```bash + $ rosservice call xarm_pose_plan 'target: [[0.28, -0.2, 0.5], [0.0, 0.0, 0.0, 1.0]]' +``` +The separated fields for Cartesian target correspond to tool frame position (x, y, z) in meters and orientation Quaternion (x, y, z, w). +After calling the two services, a boolean result named 'success' will be returned. + +## For Execution of planned trajectory: + +***Notice: Use Cartesian planning with special care, since trajectory from Moveit (OMPL) planner can be highly random and not necessarily the optimal (closest) solution, Do check it in Rviz befroe confirm to move!*** + +If solution exists and user want to execute it on the robot, it can be done by service call (**recommended**) or topic message. + +### Execute by service call (Blocking) +Call the 'xarm_exec_plan' service with a request data of 'true', the latest planned path will be executed, the service will return after finishing the execution: +```bash + $ rosservice call xarm_exec_plan 'true' +``` + +### Execute by topic (Non-blocking) +Just publish a message (type: std_msgs/Bool) to the topic "/xarm_planner_exec", the boolean data should be 'true' to launch the execution, it returns immediately and does not wait for finish: +```bash + $ rostopic pub -1 /xarm_planner_exec std_msgs/Bool 'true' +``` + +Alternative way of calling services or publish messages, is to do it programatically. User can refer to ROS [tutorial1](http://wiki.ros.org/ROS/Tutorials/WritingServiceClient%28c%2B%2B%29) and [tutorial2](http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29) to find out how to do it, or refer to the 'xarm_simple_planner_test.cpp' in the src folder as an example. +To run the test program ( ***for xArm7 only***, user can modify the command list for other models), after launching the simple planner: +```bash + $ rosrun xarm_planner xarm_simple_planner_test +``` +The program will execute three hard-coded joint space target, ***MAKE SURE THERE ARE PLENTY SURROUNDING SPACES BEFORE EXECUTING THIS!*** diff --git a/xarm_planner/ReadMe_cn.md b/xarm_planner/ReadMe_cn.md new file mode 100755 index 00000000..b80e1bda --- /dev/null +++ b/xarm_planner/ReadMe_cn.md @@ -0,0 +1,55 @@ +# Package 描述 +  这个package是用于提供给用户一个编程接口去使用moveit!, 而不仅仅限于图形界面。为了达到更好的使用效果，用户需要提前学习[Moveit的官方教程](http://docs.ros.org/kinetic/api/moveit_tutorials/html/)。 +  'xarm_simple_planner' 其实只是基于Move_group interface的简单包装， 如果用户对于高级的配置 (constraints, 切换不同的kinematic solver或者planners, etc)有需求, 可以自己充分利用Moveit的功能实现更复杂的结构。 + +# xArm simple planner使用指南 +## 启动simple planner: +如果希望在仿真环境中使用，运行: +```bash + $ roslaunch xarm_planner xarm_planner_rviz_sim.launch +``` +或者如果希望直接控制真机，运行: +```bash + $ roslaunch xarm_planner xarm_planner_realHW.launch robot_ip:= robot_dof:=<7/6/5> +``` +'robot_dof'参数指的是xArm的关节数目 (默认值为7)，这个节点提供针对笛卡尔或者关节坐标进行轨迹规划的service，Service的定义可以在srv文件夹寻找。 用户可以调用相关service去尝试进行轨迹规划求解, 并会收到成功与否的布尔值。 按以上步骤启动节点之后，可以先尝试命令行方法使用： + +## 关节空间目标规划: +```bash + $ rosservice call xarm_joint_plan 'target: [1.0, -0.5, 0.0, -0.3, 0.0, 0.0, 0.5]' +``` +这种情况下列表中的元素代表每个关节的目标角度(单位是radian)。 + +## 笛卡尔目标规划: +```bash + $ rosservice call xarm_pose_plan 'target: [[0.28, -0.2, 0.5], [0.0, 0.0, 0.0, 1.0]]' +``` +目标列表中的域分别指代工具坐标系原点位置(x, y, z)，单位：米；以及四元数方位(x, y, z, w)。 +调用以上服务之后, 会返回名为'success'的布尔结果。 + +## 执行规划好的轨迹: + +***请注意: 使用笛卡尔规划时, Moveit (OMPL) 每次生成的路径可能非常不同且比较随机，并不一定是移动距离最小的解, 所以在执行前一定在仿真环境中确认轨迹!*** + +如果存在满意的解, 用户可以通过service(**推荐**)或topic下达执行命令。 + +### 通过service call执行 (阻塞式) +调用'xarm_exec_plan' service 并将request的data设为'true', 则上一次成功解算的轨迹会被执行, service call会在执行完毕后返回，命令行运行: +```bash + $ rosservice call xarm_exec_plan 'true' +``` + +### 通过topic执行 (非阻塞式) +只需要通过话题"/xarm_planner_exec"发布一条消息 (类型: std_msgs/Bool), 内容设为'true'即可命令机械臂执行轨迹, 但是这种方法会直接返回而不是等待执行完成: +```bash + $ rostopic pub -1 /xarm_planner_exec std_msgs/Bool 'true' +``` + +除了命令行，另一种调用service或发布topic的方法是通过编程的方法。 用户可以参考[ROS教程1](http://wiki.ros.org/ROS/Tutorials/WritingServiceClient%28c%2B%2B%29) 以及 [ROS教程2](http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29) 去了解怎样实现, 亦或者参考 src 目录下的'xarm_simple_planner_test.cpp'。 +若想执行此测试程序( ***此测试程序仅限xArm7运行***，不过用户可以更改源代码里的指令以适用于6关节或5关节), 在启动simple planner节点后，运行: +```bash + $ rosrun xarm_planner xarm_simple_planner_test +``` +这个测试程序会运行一些设计好的轨迹, ***执行前请确保机械臂周围有足够的空间!*** + + diff --git a/xarm_planner/launch/planner.rviz b/xarm_planner/launch/planner.rviz new file mode 100755 index 00000000..22a2df5c --- /dev/null +++ b/xarm_planner/launch/planner.rviz @@ -0,0 +1,326 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /Status1 + - /MotionPlanning1 + - /MotionPlanning1/Scene Robot1 + - /MotionPlanning1/Planning Request1 + - /MotionPlanning1/Planned Path1 + Splitter Ratio: 0.742560029 + Tree Height: 809 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +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 + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.0799999982 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: xarm7 + Query Goal State: false + 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 + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 1 + - 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 + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + 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: world + 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: 2.00718117 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.113567002 + Y: 0.105920002 + Z: 2.23518001e-07 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: -0.330201834 + Target Frame: world + Value: XYOrbit (rviz) + Yaw: 5.87495947 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1028 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002ad000003befc0200000007fb000000100044006900730070006c0061007900730100000028000003be000000d700fffffffb0000000800480065006c00700000000342000000bb0000007300fffffffb0000000a0056006900650077007300000003b0000000b0000000ad00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002580000018e0000018300ffffff00000471000003be00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1828 + X: 1997 + Y: 26 diff --git a/xarm_planner/launch/xarm_planner_realHW.launch b/xarm_planner/launch/xarm_planner_realHW.launch new file mode 100755 index 00000000..aa7c7ab1 --- /dev/null +++ b/xarm_planner/launch/xarm_planner_realHW.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/xarm_planner/launch/xarm_planner_rviz_sim.launch b/xarm_planner/launch/xarm_planner_rviz_sim.launch new file mode 100755 index 00000000..2a11df53 --- /dev/null +++ b/xarm_planner/launch/xarm_planner_rviz_sim.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/xarm_planner/package.xml b/xarm_planner/package.xml new file mode 100755 index 00000000..39f61b5a --- /dev/null +++ b/xarm_planner/package.xml @@ -0,0 +1,34 @@ + + + xarm_planner + 0.0.0 + The xarm_planner package + + jason + + BSD + + catkin + + pluginlib + moveit_core + moveit_ros_planning_interface + moveit_ros_perception + interactive_markers + geometric_shapes + moveit_visual_tools + message_generation + geometry_msgs + std_msgs + + pluginlib + moveit_core + moveit_fake_controller_manager + moveit_ros_planning_interface + moveit_ros_perception + interactive_markers + moveit_visual_tools + message_runtime + geometry_msgs + std_msgs + diff --git a/xarm_planner/src/xarm_simple_planner.cpp b/xarm_planner/src/xarm_simple_planner.cpp new file mode 100755 index 00000000..800c025d --- /dev/null +++ b/xarm_planner/src/xarm_simple_planner.cpp @@ -0,0 +1,181 @@ +/* Copyright 2018 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jason Peng + ============================================================================*/ +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#define SPINNER_THREAD_NUM 2 + + +class XArmSimplePlanner +{ + public: + XArmSimplePlanner(const std::string plan_group_name):spinner(SPINNER_THREAD_NUM), group(plan_group_name){init();}; + XArmSimplePlanner():spinner(SPINNER_THREAD_NUM),group(PLANNING_GROUP){init();}; + ~XArmSimplePlanner(){}; + void start(); + void stop(); + + static std::string PLANNING_GROUP; // declaration of static class member + + private: + ros::NodeHandle node_handle; + ros::AsyncSpinner spinner; + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + std::vector joint_names; + moveit::planning_interface::MoveGroupInterface group; + moveit::planning_interface::MoveGroupInterface::Plan my_xarm_plan; + + ros::Publisher display_path; + ros::ServiceServer plan_pose_srv; + ros::ServiceServer plan_joint_srv; + ros::Subscriber exec_plan_sub; /* non-blocking*/ + ros::ServiceServer exec_plan_srv; /* blocking with result feedback */ + + void init(); + bool do_pose_plan(xarm_planner::pose_plan::Request &req, xarm_planner::pose_plan::Response &res); + bool do_joint_plan(xarm_planner::joint_plan::Request &req, xarm_planner::joint_plan::Response &res); + bool exec_plan_cb(xarm_planner::exec_plan::Request &req, xarm_planner::exec_plan::Response &res); + void execute_plan_topic(const std_msgs::Bool::ConstPtr& exec); +}; + +void XArmSimplePlanner::init() +{ + joint_names = group.getJointNames(); + + display_path = node_handle.advertise("/move_group/display_planned_path", 1, true); /*necessary?*/ + + ROS_INFO_NAMED("move_group_planner", "Reference frame: %s", group.getPlanningFrame().c_str()); + + ROS_INFO_NAMED("move_group_planner", "End effector link: %s", group.getEndEffectorLink().c_str()); + + /* Notice: the correct way to specify member function as callbacks */ + plan_pose_srv = node_handle.advertiseService("xarm_pose_plan", &XArmSimplePlanner::do_pose_plan, this); + plan_joint_srv = node_handle.advertiseService("xarm_joint_plan", &XArmSimplePlanner::do_joint_plan, this); + + exec_plan_sub = node_handle.subscribe("xarm_planner_exec", 10, &XArmSimplePlanner::execute_plan_topic, this); + exec_plan_srv = node_handle.advertiseService("xarm_exec_plan", &XArmSimplePlanner::exec_plan_cb, this); + +} + +void XArmSimplePlanner::start() +{ + ROS_INFO("Spinning"); + spinner.start(); +} + +void XArmSimplePlanner::stop() +{ + spinner.stop(); +} + +bool XArmSimplePlanner::do_pose_plan(xarm_planner::pose_plan::Request &req, xarm_planner::pose_plan::Response &res) +{ + group.setPoseTarget(req.target); + + ROS_INFO("xarm_planner received new target: [ position: (%f, %f, %f), orientation: (%f, %f, %f, %f)", \ + req.target.position.x, req.target.position.y, req.target.position.z, req.target.orientation.x, \ + req.target.orientation.y, req.target.orientation.z, req.target.orientation.w); + + bool success = (group.plan(my_xarm_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + res.success = success; + ROS_INFO_NAMED("move_group_planner", "This plan (pose goal) %s", success ? "SUCCEEDED" : "FAILED"); + + return success; +} + +bool XArmSimplePlanner::do_joint_plan(xarm_planner::joint_plan::Request &req, xarm_planner::joint_plan::Response &res) +{ + ROS_INFO("move_group_planner received new plan Request"); + group.setJointValueTarget(req.target); + + bool success = (group.plan(my_xarm_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + res.success = success; + ROS_INFO_NAMED("move_group_planner", "This plan (joint goal) %s", success ? "SUCCEEDED" : "FAILED"); + + return success; +} + +bool XArmSimplePlanner::exec_plan_cb(xarm_planner::exec_plan::Request &req, xarm_planner::exec_plan::Response &res) +{ + if(req.exec) + { + ROS_INFO("Received Execution Service Request"); + bool finish_ok = (group.execute(my_xarm_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); /* return after execution finish */ + res.success = finish_ok; + return finish_ok; + } + + res.success = false; + return false; +} + +/* execution subscriber call-back function */ +void XArmSimplePlanner::execute_plan_topic(const std_msgs::Bool::ConstPtr& exec) +{ + if(exec->data) + { + ROS_INFO("Received Execution Command !!!!!"); + group.asyncExecute(my_xarm_plan); /* return without waiting for finish */ + } +} + + +std::string XArmSimplePlanner::PLANNING_GROUP; // Definition of static class member + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "xarm_move_group_planner"); + ros::NodeHandle nh; + int jnt_num; + + nh.getParam("DOF", jnt_num); + switch(jnt_num) + { + case 7: + { + XArmSimplePlanner::PLANNING_GROUP = "xarm7"; + break; + } + case 6: + { + XArmSimplePlanner::PLANNING_GROUP = "xarm6"; + break; + } + case 5: + { + XArmSimplePlanner::PLANNING_GROUP = "xarm5"; + break; + } + default: + { + ROS_ERROR("ROS parameter DOF not correct, please CHECK!!"); + } + } + + XArmSimplePlanner planner; + + planner.start(); + + ROS_INFO("Waiting for \'pose_plan\' or \'joint_plan\' service Request ..."); + + /* necessary: because AsyncSpinner is not operating in the same thread */ + ros::waitForShutdown(); + return 0; +} + diff --git a/xarm_planner/src/xarm_simple_planner_test.cpp b/xarm_planner/src/xarm_simple_planner_test.cpp new file mode 100755 index 00000000..566bfe4a --- /dev/null +++ b/xarm_planner/src/xarm_simple_planner_test.cpp @@ -0,0 +1,112 @@ +/* Copyright 2018 UFACTORY Inc. All Rights Reserved. + * + * Software License Agreement (BSD License) + * + * Author: Jason Peng + ============================================================================*/ +#include "ros/ros.h" +#include +#include +#include +#include +#include +#include + +bool request_plan(ros::ServiceClient& client, xarm_planner::joint_plan& srv) +{ + if(client.call(srv)) + { + return srv.response.success; + } + else + { + ROS_ERROR("Failed to call service joint_plan"); + return false; + } +} + +bool request_exec(ros::ServiceClient& client, xarm_planner::exec_plan& srv) +{ + if(client.call(srv)) + { + return srv.response.success; + } + else + { + ROS_ERROR("Failed to call service exec_plan"); + return false; + } +} + + +int main(int argc, char** argv) +{ + std::vector tar_joint1 = {-1.0, -0.75, 0.0, -0.5, 0.0, 0.3, 0.0}; + std::vector tar_joint2 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + std::vector tar_joint3 = {1.0, -0.75, 0.0, -0.5, 0.0, -0.3, 0.0}; + + ros::init(argc, argv, "xarm_simple_planner_client"); + ros::NodeHandle nh; + ros::ServiceClient client = nh.serviceClient("xarm_joint_plan"); + ros::ServiceClient client_exec = nh.serviceClient("xarm_exec_plan"); + + ros::Publisher exec_pub = nh.advertise("xarm_planner_exec", 10); + std_msgs::Bool msg; + xarm_planner::joint_plan srv; + xarm_planner::exec_plan srv_exec; + + + srv.request.target = tar_joint1; + if(request_plan(client, srv)) + { + ROS_INFO("Plan SUCCESS! Executing... "); + // msg.data = true; + // ros::Duration(1.0).sleep(); + // exec_pub.publish(msg); + srv_exec.request.exec = true; + request_exec(client_exec, srv_exec); + } + + // ros::Duration(4.0).sleep(); // Wait for last execution to finish + + srv.request.target = tar_joint2; + if(request_plan(client, srv)) + { + ROS_INFO("Plan SUCCESS! Executing... "); + // msg.data = true; + // ros::Duration(1.0).sleep(); + // exec_pub.publish(msg); + srv_exec.request.exec = true; + request_exec(client_exec, srv_exec); + } + + // ros::Duration(4.0).sleep(); // Wait for last execution to finish + + srv.request.target = tar_joint3; + if(request_plan(client, srv)) + { + ROS_INFO("Plan SUCCESS! Executing... "); + // msg.data = true; + // ros::Duration(1.0).sleep(); + // exec_pub.publish(msg); + srv_exec.request.exec = true; + request_exec(client_exec, srv_exec); + } + + // ros::Duration(4.0).sleep(); // Wait for last execution to finish + + srv.request.target = tar_joint2; + if(request_plan(client, srv)) + { + ROS_INFO("Plan SUCCESS! Executing... "); + // msg.data = true; + // ros::Duration(1.0).sleep(); + // exec_pub.publish(msg); + srv_exec.request.exec = true; + request_exec(client_exec, srv_exec); + } + + return 0; + +} + diff --git a/xarm_planner/srv/exec_plan.srv b/xarm_planner/srv/exec_plan.srv new file mode 100755 index 00000000..a3223618 --- /dev/null +++ b/xarm_planner/srv/exec_plan.srv @@ -0,0 +1,3 @@ +bool exec +--- +bool success \ No newline at end of file diff --git a/xarm_planner/srv/joint_plan.srv b/xarm_planner/srv/joint_plan.srv new file mode 100755 index 00000000..897241fc --- /dev/null +++ b/xarm_planner/srv/joint_plan.srv @@ -0,0 +1,6 @@ +# list of target joint positions in radian. +float64[] target + +--- + +bool success \ No newline at end of file diff --git a/xarm_planner/srv/pose_plan.srv b/xarm_planner/srv/pose_plan.srv new file mode 100755 index 00000000..c6fc41ea --- /dev/null +++ b/xarm_planner/srv/pose_plan.srv @@ -0,0 +1,5 @@ +geometry_msgs/Pose target + +--- + +bool success \ No newline at end of file