├── denso_cobotta_driver ├── srv │ ├── ClearError.srv │ ├── ClearSafeState.srv │ ├── ExecCalset.srv │ ├── ClearRobotError.srv │ ├── GetMotorState.srv │ ├── SetMotorState.srv │ ├── GetBrakeState.srv │ ├── SetBrakeState.srv │ └── SetLEDState.srv ├── msg │ ├── SafeState.msg │ └── RobotState.msg ├── config │ └── parameters.yaml ├── launch │ └── denso_cobotta_driver.launch ├── package.xml ├── CMakeLists.txt └── include │ └── denso_cobotta_driver │ └── denso_cobotta_driver.h ├── denso_cobotta_moveit_config ├── launch │ ├── cobotta_moveit_sensor_manager.launch.xml │ ├── planning_pipeline.launch.xml │ ├── fake_moveit_controller_manager.launch.xml │ ├── warehouse.launch │ ├── setup_assistant.launch │ ├── joystick_control.launch │ ├── sensor_manager.launch.xml │ ├── moveit_rviz.launch │ ├── warehouse_settings.launch.xml │ ├── default_warehouse_db.launch │ ├── run_benchmark_ompl.launch │ ├── ompl_planning_pipeline.launch.xml │ ├── cobotta_moveit_controller_manager.launch.xml │ ├── planning_context.launch │ ├── trajectory_execution.launch.xml │ ├── demo.launch │ └── move_group.launch ├── config │ ├── kinematics.yaml │ ├── fake_controllers.yaml │ ├── controllers.yaml │ ├── controllers_with_parallel.yaml │ ├── joint_limits.yaml │ ├── vacuum.srdf.xacro │ ├── parallel.srdf.xacro │ └── cobotta.srdf.xacro ├── CMakeLists.txt ├── .setup_assistant └── package.xml ├── denso_cobotta_ros ├── CMakeLists.txt └── package.xml ├── denso_cobotta_gripper ├── action │ ├── VacuumMove.action │ └── GripperMove.action ├── config │ ├── gripper_parallel.yaml │ └── gripper_vacuum.yaml ├── launch │ └── denso_cobotta_gripper.launch ├── include │ └── denso_cobotta_gripper │ │ ├── denso_cobotta_gripper.h │ │ ├── gripper_vacuum.h │ │ ├── gripper_parallel.h │ │ └── gripper_base.h ├── CMakeLists.txt ├── package.xml └── src │ ├── denso_cobotta_gripper.cpp │ └── gripper_vacuum.cpp ├── denso_cobotta_bringup ├── CMakeLists.txt ├── package.xml └── launch │ └── denso_cobotta_bringup.launch ├── denso_cobotta_descriptions ├── CMakeLists.txt ├── cobotta_description │ ├── denso_cobotta_control.yaml │ ├── gazebo_cobotta_control.yaml │ ├── vacuum.urdf.xacro │ ├── parallel.urdf.xacro │ └── J6.dae └── package.xml ├── denso_cobotta_gazebo ├── worlds │ └── cobotta.world ├── package.xml ├── launch │ └── denso_cobotta_gazebo.launch └── CMakeLists.txt ├── denso_cobotta_lib ├── package.xml ├── include │ └── denso_cobotta_lib │ │ ├── buttons.h │ │ ├── gripper.h │ │ ├── cobotta_exception.h │ │ ├── mini_io.h │ │ ├── message.h │ │ ├── publish_info.h │ │ ├── motor.h │ │ ├── brake.h │ │ ├── cobotta.h │ │ ├── cobotta_common.h │ │ ├── led.h │ │ ├── safety_mcu.h │ │ └── driver.h ├── src │ ├── cobotta_exception.cpp │ ├── buttons.cpp │ ├── gripper.cpp │ ├── publish_info.cpp │ ├── cobotta.cpp │ ├── mini_io.cpp │ └── motor.cpp └── CMakeLists.txt ├── .gitignore ├── denso_cobotta_control ├── launch │ └── denso_cobotta_control.launch ├── CMakeLists.txt ├── package.xml ├── src │ ├── denso_cobotta_control.cpp │ └── denso_cobotta_hw.cpp └── include │ └── denso_cobotta_control │ └── denso_cobotta_hw.h ├── README.md ├── .clang-format ├── CHANGELOG.md └── scripts └── packing_pose.py /denso_cobotta_driver/srv/ClearError.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool success 3 | -------------------------------------------------------------------------------- /denso_cobotta_driver/srv/ClearSafeState.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool success 3 | -------------------------------------------------------------------------------- /denso_cobotta_driver/srv/ExecCalset.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool success 3 | -------------------------------------------------------------------------------- /denso_cobotta_driver/srv/ClearRobotError.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool success 3 | -------------------------------------------------------------------------------- /denso_cobotta_driver/srv/GetMotorState.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool state 3 | bool success 4 | -------------------------------------------------------------------------------- /denso_cobotta_driver/srv/SetMotorState.srv: -------------------------------------------------------------------------------- 1 | bool state 2 | --- 3 | bool success 4 | -------------------------------------------------------------------------------- /denso_cobotta_driver/msg/SafeState.msg: -------------------------------------------------------------------------------- 1 | uint32 state_code 2 | uint32 state_subcode 3 | -------------------------------------------------------------------------------- /denso_cobotta_driver/srv/GetBrakeState.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool[] state 3 | bool success 4 | -------------------------------------------------------------------------------- /denso_cobotta_driver/srv/SetBrakeState.srv: -------------------------------------------------------------------------------- 1 | bool[] state 2 | --- 3 | bool success 4 | -------------------------------------------------------------------------------- /denso_cobotta_driver/msg/RobotState.msg: -------------------------------------------------------------------------------- 1 | uint32 arm_no 2 | uint32 state_code 3 | uint32 state_subcode 4 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/cobotta_moveit_sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /denso_cobotta_driver/srv/SetLEDState.srv: -------------------------------------------------------------------------------- 1 | uint8 red 2 | uint8 green 3 | uint8 blue 4 | uint8 blink_rate 5 | --- 6 | bool success 7 | -------------------------------------------------------------------------------- /denso_cobotta_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(denso_cobotta_ros) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /denso_cobotta_driver/config/parameters.yaml: -------------------------------------------------------------------------------- 1 | # Rang value 2 | # Write joint value read from QR code. 3 | rang_value: 4 | - 0 5 | - 0 6 | - 0 7 | - 0 8 | - 0 9 | - 0 -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout: 0.005 5 | kinematics_solver_attempts: 3 -------------------------------------------------------------------------------- /denso_cobotta_gripper/action/VacuumMove.action: -------------------------------------------------------------------------------- 1 | int32 direction 2 | float64 power_percentage 3 | --- 4 | bool success 5 | --- 6 | float64 pressure # Not implemented. 7 | bool stalled # True iff the gripper is exerting max effort and not moving 8 | -------------------------------------------------------------------------------- /denso_cobotta_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(denso_cobotta_bringup) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package() 6 | 7 | ########### 8 | ## Build ## 9 | ########### 10 | 11 | include_directories() -------------------------------------------------------------------------------- /denso_cobotta_descriptions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(denso_cobotta_descriptions) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | ########### 9 | ## Build ## 10 | ########### 11 | include_directories() 12 | -------------------------------------------------------------------------------- /denso_cobotta_gazebo/worlds/cobotta.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | model://sun 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /denso_cobotta_gripper/config/gripper_parallel.yaml: -------------------------------------------------------------------------------- 1 | max_soft_limit: 0.015 2 | min_soft_limit: 0.0 3 | max_velocity: 0.08 4 | max_acceleration: 0.8 5 | max_speed_percentage: 100.0 6 | min_speed_percentage: 1.0 7 | max_effort: 20.0 8 | min_effort: 6.0 9 | coeff_outpos_to_pulse: 2481230.769 10 | coeff_effort_to_torque: 0.0305 11 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: fake_arm_controller 3 | joints: 4 | - joint_1 5 | - joint_2 6 | - joint_3 7 | - joint_4 8 | - joint_5 9 | - joint_6 10 | - name: fake_gripper_controller 11 | joints: 12 | - joint_gripper -------------------------------------------------------------------------------- /denso_cobotta_gripper/config/gripper_vacuum.yaml: -------------------------------------------------------------------------------- 1 | max_soft_limit: 1.79769e+308 2 | min_soft_limit: -1.79769e+308 3 | max_velocity: 349.06585 4 | max_acceleration: 698.13172 5 | max_speed_percentage: 100.0 6 | min_speed_percentage: 40.0 7 | max_effort: 0.0 8 | min_effort: 0.0 9 | coeff_outpos_to_pulse: 244.461992589 10 | coeff_effort_to_torque: 0.0305 11 | -------------------------------------------------------------------------------- /denso_cobotta_gripper/action/GripperMove.action: -------------------------------------------------------------------------------- 1 | float64 target_position 2 | float32 speed 3 | float32 effort 4 | --- 5 | bool success 6 | --- 7 | float64 position # The current gripper position (in meters) 8 | bool stalled # True iff the gripper is exerting max effort and not moving 9 | bool reached_goal # True iff the gripper position has reached the commanded setpoint -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_manager_ns: controller_manager 2 | controller_list: 3 | - name: cobotta/arm_controller 4 | action_ns: follow_joint_trajectory 5 | type: FollowJointTrajectory 6 | default: true 7 | joints: 8 | - joint_1 9 | - joint_2 10 | - joint_3 11 | - joint_4 12 | - joint_5 13 | - joint_6 14 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(denso_cobotta_moveit_config) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 9 | PATTERN "setup_assistant.launch" EXCLUDE) 10 | install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 11 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | URDF: 3 | package: denso_cobotta_descriptions 4 | relative_path: cobotta_description/cobotta.urdf 5 | xacro_args: "--inorder " 6 | SRDF: 7 | relative_path: config/cobotta.srdf 8 | CONFIG: 9 | author_name: ToshitakaSuzuki 10 | author_email: toshitaka.suzuki@denso-wave.co.jp 11 | generated_timestamp: 1533907720 -------------------------------------------------------------------------------- /denso_cobotta_descriptions/cobotta_description/denso_cobotta_control.yaml: -------------------------------------------------------------------------------- 1 | cobotta: 2 | joint_state_controller: 3 | type: "joint_state_controller/JointStateController" 4 | publish_rate: 125 5 | 6 | arm_controller: 7 | type: "position_controllers/JointTrajectoryController" 8 | joints: 9 | - joint_1 10 | - joint_2 11 | - joint_3 12 | - joint_4 13 | - joint_5 14 | - joint_6 -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/fake_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /denso_cobotta_descriptions/cobotta_description/gazebo_cobotta_control.yaml: -------------------------------------------------------------------------------- 1 | gazebo_ros_control: 2 | pid_gains: 3 | joint_1: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0} 4 | joint_2: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0} 5 | joint_3: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0} 6 | joint_4: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0} 7 | joint_5: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0} 8 | joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0} 9 | # joint_gripper: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0} -------------------------------------------------------------------------------- /denso_cobotta_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | denso_cobotta_bringup 4 | 1.2.2 5 | The denso_cobotta_bringup package 6 | DENSO WAVE INCORPORATED 7 | GPL 8 | DENSO WAVE INCORPORATED 9 | catkin 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/config/controllers_with_parallel.yaml: -------------------------------------------------------------------------------- 1 | controller_manager_ns: controller_manager 2 | controller_list: 3 | - name: cobotta/arm_controller 4 | action_ns: follow_joint_trajectory 5 | type: FollowJointTrajectory 6 | default: true 7 | joints: 8 | - joint_1 9 | - joint_2 10 | - joint_3 11 | - joint_4 12 | - joint_5 13 | - joint_6 14 | 15 | - name: cobotta 16 | action_ns: gripper_action 17 | type: GripperCommand 18 | default: true 19 | joints: 20 | - joint_gripper 21 | -------------------------------------------------------------------------------- /denso_cobotta_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | denso_cobotta_ros 4 | 1.2.2 5 | Package for COBOTTA OSS 6 | DENSO WAVE INCORPORATED 7 | GPL 8 | DENSO WAVE INCORPORATED 9 | http://wiki.ros.org/denso_cobotta_ros 10 | catkin 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /denso_cobotta_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | denso_cobotta_gazebo 4 | 1.2.2 5 | The denso_cobotta_gazebo package 6 | DENSO WAVE INCORPORATED 7 | GPL 8 | http://wiki.ros.org/denso_cobotta_ros 9 | DENSO WAVE INCORPORATED 10 | catkin 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /denso_cobotta_descriptions/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | denso_cobotta_descriptions 4 | 1.2.2 5 | The denso_cobotta_descriptions package 6 | DENSO WAVE INCORPORATED 7 | GPL 8 | http://wiki.ros.org/denso_cobotta_ros 9 | DENSO WAVE INCORPORATED 10 | catkin 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /denso_cobotta_gripper/launch/denso_cobotta_gripper.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/warehouse.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/setup_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /denso_cobotta_lib/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | denso_cobotta_lib 4 | 1.2.2 5 | The denso_cobotta_lib package 6 | GPL 7 | 8 | DENSO WAVE INCORPORATED 9 | DENSO WAVE INCORPORATED 10 | http://wiki.ros.org/denso_cobotta_ros 11 | 12 | catkin 13 | roscpp 14 | roscpp 15 | roscpp 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/joystick_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /denso_cobotta_driver/launch/denso_cobotta_driver.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | $(arg enable_calset) 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/warehouse_settings.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | 53 | # VSCode 54 | .vscode -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/default_warehouse_db.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /denso_cobotta_control/launch/denso_cobotta_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/run_benchmark_ompl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/ompl_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /denso_cobotta_gripper/include/denso_cobotta_gripper/denso_cobotta_gripper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #ifndef DENSO_COBOTTA_GRIPPER_H 20 | #define DENSO_COBOTTA_GRIPPER_H 21 | 22 | #endif // DENSO_COBOTTA_GRIPPER_H 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # denso_cobotta_ros 2 | 3 | In order to operate cobotta, execute following command. 4 | 5 | ```sh 6 | roslaunch denso_cobotta_bringup denso_cobotta_bringup.launch 7 | roslaunch denso_cobotta_driver denso_cobotta_driver.launch 8 | ``` 9 | 10 | After executing the above command, you can use the following services and topics. 11 | 12 | - service 13 | - /cobotta/clear_error 14 | - /cobotta/clear_robot_error 15 | - /cobotta/clear_safe_state 16 | - /cobotta/get_brake_state 17 | - /cobotta/get_motor_state 18 | - /cobotta/set_LED_state 19 | - /cobotta/set_brake_state 20 | - /cobotta/set_motor_state 21 | - topic 22 | - /cobotta/close_button 23 | - /cobotta/gripper_move 24 | - /cobotta/gripper_action 25 | - /cobotta/miniIO_input 26 | - /cobotta/miniIO_output 27 | - /cobotta/open_button 28 | - /cobotta/position_button 29 | - /cobotta/robot_state 30 | - /cobotta/safe_state 31 | 32 | ## Installation and requirements 33 | 34 | Please see the [Getting started](https://densorobot.github.io/docs/denso_cobotta_ros/getting_started.html) page. 35 | -------------------------------------------------------------------------------- /denso_cobotta_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | denso_cobotta_driver 4 | 1.2.2 5 | The denso_cobotta_driver package 6 | DENSO WAVE INCORPORATED 7 | GPL 8 | http://wiki.ros.org/denso_cobotta_ros 9 | DENSO WAVE INCORPORATED 10 | catkin 11 | message_generation 12 | roscpp 13 | realtime_tools 14 | std_msgs 15 | denso_cobotta_lib 16 | roscpp 17 | std_msgs 18 | roscpp 19 | std_msgs 20 | message_runtime 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | denso_cobotta_moveit_config 3 | 1.2.2 4 | 5 | An automatically generated package with all the configuration and launch files for using the cobotta with the MoveIt! Motion Planning Framework 6 | 7 | http://moveit.ros.org/ 8 | https://github.com/ros-planning/moveit/issues 9 | https://github.com/ros-planning/moveit 10 | DENSO WAVE INCORPORATED 11 | GPL 12 | catkin 13 | moveit_ros_move_group 14 | moveit_fake_controller_manager 15 | moveit_kinematics 16 | moveit_planners_ompl 17 | moveit_ros_visualization 18 | joint_state_publisher 19 | robot_state_publisher 20 | xacro 21 | denso_cobotta_descriptions 22 | denso_cobotta_descriptions 23 | 24 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/cobotta_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /denso_cobotta_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(denso_cobotta_control) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | controller_manager 8 | hardware_interface 9 | joint_limits_interface 10 | roscpp 11 | std_msgs 12 | transmission_interface 13 | denso_cobotta_lib 14 | denso_cobotta_driver 15 | ) 16 | 17 | catkin_package( 18 | INCLUDE_DIRS include 19 | LIBRARIES denso_cobotta_control 20 | CATKIN_DEPENDS controller_manager hardware_interface joint_limits_interface roscpp std_msgs transmission_interface denso_cobotta_lib 21 | ) 22 | 23 | ########### 24 | ## Build ## 25 | ########### 26 | 27 | include_directories( 28 | include 29 | ${catkin_INCLUDE_DIRS} 30 | ) 31 | 32 | add_executable(${PROJECT_NAME} 33 | src/denso_cobotta_control.cpp 34 | src/denso_cobotta_hw.cpp 35 | ) 36 | 37 | add_dependencies(${PROJECT_NAME} denso_cobotta_driver_generate_messages_cpp) 38 | 39 | target_link_libraries(${PROJECT_NAME} 40 | ${catkin_LIBRARIES} 41 | yaml-cpp 42 | ) 43 | 44 | ############# 45 | ## Install ## 46 | ############# 47 | 48 | install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME} 49 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 50 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 51 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 52 | ) 53 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/planning_context.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /denso_cobotta_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(denso_cobotta_driver) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | set(CMAKE_CXX_STANDARD 14) 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | message_generation 10 | roscpp 11 | realtime_tools 12 | std_msgs 13 | denso_cobotta_lib 14 | ) 15 | 16 | add_service_files( 17 | FILES 18 | SetMotorState.srv 19 | GetMotorState.srv 20 | ClearError.srv 21 | ClearRobotError.srv 22 | ClearSafeState.srv 23 | SetBrakeState.srv 24 | GetBrakeState.srv 25 | SetLEDState.srv 26 | ExecCalset.srv 27 | ) 28 | 29 | add_message_files( 30 | FILES 31 | RobotState.msg 32 | SafeState.msg 33 | ) 34 | 35 | generate_messages( 36 | DEPENDENCIES 37 | std_msgs 38 | ) 39 | 40 | catkin_package( 41 | INCLUDE_DIRS include 42 | CATKIN_DEPENDS message_runtime roscpp std_msgs 43 | LIBRARIES denso_cobotta_lib 44 | ) 45 | 46 | ########### 47 | ## Build ## 48 | ########### 49 | 50 | include_directories( 51 | include 52 | # include/${PROJECT_NAME}/cobotta 53 | ${catkin_INCLUDE_DIRS} 54 | ) 55 | 56 | add_executable(${PROJECT_NAME} 57 | src/denso_cobotta_driver.cpp 58 | ) 59 | 60 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) 61 | 62 | target_link_libraries(${PROJECT_NAME} 63 | ${catkin_LIBRARIES} 64 | yaml-cpp 65 | ) 66 | 67 | ############# 68 | ## Install ## 69 | ############# 70 | 71 | install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME} 72 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 73 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 74 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 75 | ) 76 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 3 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 4 | 5 | # FYI: It might be incorrect. 6 | joint_limits: 7 | joint_1: 8 | has_velocity_limits: true 9 | max_velocity: 0.4031710572 10 | has_acceleration_limits: true 11 | max_acceleration: 1.46607 12 | joint_2: 13 | has_velocity_limits: true 14 | max_velocity: 0.3921754829 15 | has_acceleration_limits: true 16 | max_acceleration: 1.46607 17 | joint_3: 18 | has_velocity_limits: true 19 | max_velocity: 0.725707903 20 | has_acceleration_limits: true 21 | max_acceleration: 2.44346 22 | joint_4: 23 | has_velocity_limits: true 24 | max_velocity: 0.7391469382 25 | has_acceleration_limits: true 26 | max_acceleration: 3.29867 27 | joint_5: 28 | has_velocity_limits: true 29 | max_velocity: 0.7391469382 30 | has_acceleration_limits: true 31 | max_acceleration: 3.29867 32 | joint_6: 33 | has_velocity_limits: true 34 | max_velocity: 1.1093312726 35 | has_acceleration_limits: true 36 | max_acceleration: 4.88692 37 | joint_gripper: 38 | has_velocity_limits: true 39 | max_velocity: 0.04 40 | has_acceleration_limits: false 41 | max_acceleration: 0 42 | joint_gripper_mimic: 43 | has_velocity_limits: true 44 | max_velocity: 0.04 45 | has_acceleration_limits: false 46 | max_acceleration: 0 47 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/trajectory_execution.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /denso_cobotta_gripper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(denso_cobotta_gripper) 3 | 4 | add_compile_options(-std=c++14) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | controller_manager 8 | hardware_interface 9 | joint_limits_interface 10 | message_generation 11 | roscpp 12 | std_msgs 13 | transmission_interface 14 | actionlib 15 | actionlib_msgs 16 | denso_cobotta_lib 17 | denso_cobotta_driver 18 | ) 19 | add_action_files( 20 | FILES 21 | GripperMove.action 22 | VacuumMove.action 23 | ) 24 | 25 | generate_messages( 26 | DEPENDENCIES 27 | actionlib_msgs 28 | std_msgs 29 | ) 30 | 31 | catkin_package( 32 | INCLUDE_DIRS include 33 | LIBRARIES denso_cobotta_gripper 34 | CATKIN_DEPENDS controller_manager hardware_interface joint_limits_interface message_runtime roscpp std_msgs transmission_interface actionlib actionlib_msgs denso_cobotta_lib 35 | ) 36 | 37 | ########### 38 | ## Build ## 39 | ########### 40 | 41 | include_directories( 42 | include 43 | ${catkin_INCLUDE_DIRS} 44 | ) 45 | 46 | add_executable(${PROJECT_NAME} 47 | src/denso_cobotta_gripper.cpp 48 | src/gripper_base.cpp 49 | src/gripper_parallel.cpp 50 | src/gripper_vacuum.cpp 51 | ) 52 | 53 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) 54 | 55 | target_link_libraries(${PROJECT_NAME} 56 | ${catkin_LIBRARIES} 57 | ) 58 | 59 | ############# 60 | ## Install ## 61 | ############# 62 | 63 | install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME} 64 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 65 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 66 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 67 | ) 68 | 69 | install(FILES 70 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 71 | ) 72 | -------------------------------------------------------------------------------- /denso_cobotta_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | denso_cobotta_control 4 | 1.2.2 5 | The denso_cobotta_control package 6 | DENSO WAVE INCORPORATED 7 | GPL 8 | http://wiki.ros.org/denso_cobotta_ros 9 | DENSO WAVE INCORPORATED 10 | catkin 11 | controller_manager 12 | hardware_interface 13 | joint_limits_interface 14 | roscpp 15 | std_msgs 16 | transmission_interface 17 | denso_cobotta_lib 18 | denso_cobotta_driver 19 | controller_manager 20 | hardware_interface 21 | joint_limits_interface 22 | roscpp 23 | std_msgs 24 | transmission_interface 25 | controller_manager 26 | hardware_interface 27 | joint_limits_interface 28 | roscpp 29 | std_msgs 30 | transmission_interface 31 | denso_cobotta_lib 32 | denso_cobotta_driver 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /denso_cobotta_lib/include/denso_cobotta_lib/buttons.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #ifndef BUTTONS_H 20 | #define BUTTONS_H 21 | 22 | #include 23 | 24 | #include "denso_cobotta_lib/cobotta.h" 25 | 26 | namespace denso_cobotta_lib 27 | { 28 | namespace cobotta 29 | { 30 | class Cobotta; 31 | 32 | class Buttons 33 | { 34 | public: 35 | static constexpr const char* TAG = "Buttons"; 36 | 37 | Buttons(); 38 | Buttons(bool function_button, bool plus_button, bool minus_button, bool ip_reset_button); 39 | virtual ~Buttons() = default; 40 | 41 | bool update(bool function_button, bool plus_button, bool minus_button); 42 | 43 | bool isMinusButtonOn() const; 44 | bool isFunctionButtonOn() const; 45 | bool isIpResetButtonOn() const; 46 | bool isPlusButtonOn() const; 47 | 48 | private: 49 | bool function_button_; 50 | bool plus_button_; 51 | bool minus_button_; 52 | }; 53 | 54 | } /* namespace cobotta */ 55 | } /* namespace denso_cobotta_lib */ 56 | 57 | #endif /* BUTTONS_H */ 58 | -------------------------------------------------------------------------------- /denso_cobotta_lib/include/denso_cobotta_lib/gripper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | #ifndef GRIPPER_H 19 | #define GRIPPER_H 20 | 21 | #include "denso_cobotta_lib/cobotta.h" 22 | 23 | namespace denso_cobotta_lib 24 | { 25 | namespace cobotta 26 | { 27 | 28 | enum class GripperType 29 | { 30 | Undefined = -1, 31 | None = 0, 32 | Parallel = 1, 33 | Vacuum = 2, 34 | }; 35 | 36 | class Gripper 37 | { 38 | public: 39 | static constexpr const char* TAG = "Gripper"; 40 | 41 | Gripper(const std::shared_ptr& parent); 42 | virtual ~Gripper() = default; 43 | 44 | bool update(bool gripper_state); 45 | 46 | static GripperType convertGripperType(const std::string& gripper_type); 47 | static long readHwGripperState(int fd) throw(CobottaException, std::runtime_error); 48 | 49 | private: 50 | std::shared_ptr getParent() const; 51 | std::shared_ptr parent_; 52 | bool gripper_state_; 53 | }; 54 | 55 | } /* namespace cobotta */ 56 | } /* namespace denso_cobotta_lib */ 57 | #endif /* GRIPPER_H */ 58 | -------------------------------------------------------------------------------- /denso_cobotta_lib/include/denso_cobotta_lib/cobotta_exception.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #ifndef COBOTTA_EXCEPTION_H 20 | #define COBOTTA_EXCEPTION_H 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include "denso_cobotta_lib/message.h" 27 | 28 | namespace denso_cobotta_lib 29 | { 30 | namespace cobotta 31 | { 32 | 33 | /** 34 | * CobottaException 35 | */ 36 | class CobottaException : public std::exception 37 | { 38 | public: 39 | static constexpr const char* TAG = "CobottaException"; 40 | 41 | CobottaException(const uint32_t code); 42 | virtual ~CobottaException() = default; 43 | 44 | virtual const char* what() const throw(); 45 | 46 | int getErrorLevel() const; 47 | uint32_t getCode() const; 48 | const std::string getMessage() const; 49 | const std::string& getWhatMessage() const; 50 | 51 | private: 52 | uint32_t code_; 53 | int error_level_; 54 | std::string message_; 55 | std::string what_message_; 56 | }; 57 | 58 | } /* namespace cobotta */ 59 | } /* namespace denso_cobotta_lib */ 60 | 61 | #endif /* COBOTTA_EXCEPTION_H */ 62 | -------------------------------------------------------------------------------- /denso_cobotta_gripper/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | denso_cobotta_gripper 4 | 1.2.2 5 | The denso_cobotta_gripper package 6 | DENSO WAVE INCORPORATED 7 | GPL 8 | http://wiki.ros.org/denso_cobotta_ros 9 | DENSO WAVE INCORPORATED 10 | catkin 11 | controller_manager 12 | hardware_interface 13 | joint_limits_interface 14 | roscpp 15 | std_msgs 16 | transmission_interface 17 | actionlib 18 | actionlib_msgs 19 | denso_cobotta_lib 20 | denso_cobotta_driver 21 | controller_manager 22 | hardware_interface 23 | joint_limits_interface 24 | roscpp 25 | std_msgs 26 | transmission_interface 27 | controller_manager 28 | hardware_interface 29 | joint_limits_interface 30 | roscpp 31 | std_msgs 32 | transmission_interface 33 | message_runtime 34 | actionlib 35 | actionlib_msgs 36 | denso_cobotta_lib 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /denso_cobotta_descriptions/cobotta_description/vacuum.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | ConstructorInitializerIndentWidth: 2 5 | AlignEscapedNewlinesLeft: false 6 | AlignTrailingComments: true 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortIfStatementsOnASingleLine: false 9 | AllowShortLoopsOnASingleLine: false 10 | AllowShortFunctionsOnASingleLine: None 11 | AllowShortLoopsOnASingleLine: false 12 | AlwaysBreakTemplateDeclarations: true 13 | AlwaysBreakBeforeMultilineStrings: false 14 | BreakBeforeBinaryOperators: false 15 | BreakBeforeTernaryOperators: false 16 | BreakConstructorInitializersBeforeComma: true 17 | BinPackParameters: true 18 | ColumnLimit: 120 19 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 20 | DerivePointerBinding: false 21 | PointerBindsToType: true 22 | ExperimentalAutoDetectBinPacking: false 23 | IndentCaseLabels: true 24 | MaxEmptyLinesToKeep: 1 25 | NamespaceIndentation: None 26 | ObjCSpaceBeforeProtocolList: true 27 | PenaltyBreakBeforeFirstCallParameter: 19 28 | PenaltyBreakComment: 60 29 | PenaltyBreakString: 1 30 | PenaltyBreakFirstLessLess: 1000 31 | PenaltyExcessCharacter: 1000 32 | PenaltyReturnTypeOnItsOwnLine: 90 33 | SpacesBeforeTrailingComments: 2 34 | Cpp11BracedListStyle: false 35 | Standard: Auto 36 | IndentWidth: 2 37 | TabWidth: 2 38 | UseTab: Never 39 | IndentFunctionDeclarationAfterType: false 40 | SpacesInParentheses: false 41 | SpacesInAngles: false 42 | SpaceInEmptyParentheses: false 43 | SpacesInCStyleCastParentheses: false 44 | SpaceAfterControlStatementKeyword: true 45 | SpaceBeforeAssignmentOperators: true 46 | ContinuationIndentWidth: 4 47 | SortIncludes: false 48 | SpaceAfterCStyleCast: false 49 | 50 | # Configure each individual brace in BraceWrapping 51 | BreakBeforeBraces: Custom 52 | 53 | # Control of individual brace wrapping cases 54 | BraceWrapping: { 55 | AfterClass: 'true' 56 | AfterControlStatement: 'true' 57 | AfterEnum : 'true' 58 | AfterFunction : 'true' 59 | AfterNamespace : 'true' 60 | AfterStruct : 'true' 61 | AfterUnion : 'true' 62 | BeforeCatch : 'true' 63 | BeforeElse : 'true' 64 | IndentBraces : 'false' 65 | } 66 | ... 67 | -------------------------------------------------------------------------------- /denso_cobotta_bringup/launch/denso_cobotta_bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /denso_cobotta_gripper/include/denso_cobotta_gripper/gripper_vacuum.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | #ifndef GRIPPER_VACUUM_H 19 | #define GRIPPER_VACUUM_H 20 | 21 | #include "denso_cobotta_gripper/gripper_base.h" 22 | #include "denso_cobotta_gripper/VacuumMoveAction.h" 23 | 24 | namespace denso_cobotta_gripper 25 | { 26 | enum class VacuumMoveDirection 27 | { 28 | Suction = -1, 29 | Stop = 0, 30 | Blow = 1, 31 | }; 32 | 33 | using namespace denso_cobotta_gripper; 34 | 35 | class GripperVacuum : public GripperBase 36 | { 37 | public: 38 | static constexpr const char* TAG = "GripperVacuum"; 39 | 40 | GripperVacuum(); 41 | virtual ~GripperVacuum() = default; 42 | bool initialize(ros::NodeHandle& nh) override; 43 | bool read(void) override; 44 | bool write(void) override; 45 | bool update(void) override; 46 | 47 | private: 48 | bool publish(void); 49 | bool subscribe(void); 50 | bool initActionServers(ros::NodeHandle& nh); 51 | bool initGripperMove(const double& target_position, const double& speed_percentage); 52 | bool updateVacuumDetectParameter(double speed_percentage); 53 | 54 | // Action callback functions. 55 | bool vacuumMoveActionGoal(const VacuumMoveGoalConstPtr& goal); 56 | void actionCancel(void); 57 | void actionFeedback(); 58 | 59 | // Action servers. 60 | std::shared_ptr> as_vacuum_move_; 61 | }; 62 | 63 | } // namespace denso_cobotta_gripper 64 | 65 | #endif // GRIPPER_VACUUM_H 66 | -------------------------------------------------------------------------------- /denso_cobotta_lib/include/denso_cobotta_lib/mini_io.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #ifndef MINI_IO_H 20 | #define MINI_IO_H 21 | 22 | #include 23 | #include 24 | 25 | #include "denso_cobotta_lib/cobotta_exception.h" 26 | #include "denso_cobotta_lib/cobotta.h" 27 | 28 | namespace denso_cobotta_lib 29 | { 30 | namespace cobotta 31 | { 32 | class Cobotta; 33 | 34 | class MiniIo 35 | { 36 | public: 37 | static constexpr const char* TAG = "MiniIo"; 38 | 39 | MiniIo(std::shared_ptr parent); 40 | MiniIo(std::shared_ptr parent, uint16_t input, uint16_t output); 41 | virtual ~MiniIo() = default; 42 | 43 | bool update(const uint16_t input, uint16_t output); 44 | void sendOutputStateValue(const uint16_t state) throw(CobottaException, std::runtime_error); 45 | uint16_t receiveInputStateValue() const throw(CobottaException, std::runtime_error); 46 | uint16_t receiveOutputStateValue() const throw(CobottaException, std::runtime_error); 47 | 48 | private: 49 | static void writeHwOutput(const int fd, uint16_t value) throw(CobottaException, std::runtime_error); 50 | static uint16_t readHwOutput(const int fd) throw(CobottaException, std::runtime_error); 51 | static uint16_t readHwInput(const int fd) throw(CobottaException, std::runtime_error); 52 | 53 | std::shared_ptr getParent() const; 54 | 55 | std::shared_ptr parent_; 56 | uint16_t input_; 57 | uint16_t output_; 58 | 59 | }; 60 | 61 | } /* namespace cobotta */ 62 | } /* namespace denso_cobotta_lib */ 63 | 64 | #endif /* MINI_IO_H */ 65 | -------------------------------------------------------------------------------- /denso_cobotta_control/src/denso_cobotta_control.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #include 20 | #include "denso_cobotta_control/denso_cobotta_hw.h" 21 | 22 | int main(int argc, char** argv) 23 | { 24 | ros::init(argc, argv, "denso_cobotta_control"); 25 | ros::NodeHandle nh; 26 | 27 | denso_cobotta_control::DensoCobottaHW cobotta; 28 | 29 | bool success = cobotta.initialize(nh); 30 | if (!success) 31 | { 32 | ROS_ERROR_STREAM("Failed to initialize denso_cobotta_control."); 33 | return 1; 34 | } 35 | 36 | cobotta.checkMotorState(); 37 | 38 | controller_manager::ControllerManager cm(&cobotta, nh); 39 | ros::Rate rate(1.0 / cobotta_common::COMMAND_CYCLE); 40 | ros::AsyncSpinner spinner(1); 41 | spinner.start(); 42 | 43 | ros::Time start = ros::Time::now(); 44 | ros::Duration dt = cobotta_common::getPeriod(); 45 | while (ros::ok()) 46 | { 47 | ros::Time now = ros::Time::now(); 48 | 49 | success = cobotta.read(now, dt); 50 | if (!success) 51 | { 52 | break; 53 | } 54 | 55 | if (cobotta.shouldReset()) 56 | { 57 | cm.update(now, ros::Duration(0), true); 58 | continue; 59 | } 60 | 61 | cm.update(now, dt); 62 | if (cobotta.isMotorOn()) 63 | { 64 | success = cobotta.write(now, dt); 65 | if (!success) 66 | break; 67 | } 68 | rate.sleep(); 69 | } 70 | spinner.stop(); 71 | 72 | cobotta.sendStayHere(cobotta.getFd()); 73 | ROS_INFO("DensoCobotttaControl has stopped."); 74 | 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /denso_cobotta_lib/include/denso_cobotta_lib/message.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #ifndef MESSAGE_H 20 | #define MESSAGE_H 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include "denso_cobotta_lib/cobotta_exception.h" 27 | 28 | namespace denso_cobotta_lib 29 | { 30 | namespace cobotta 31 | { 32 | class CobottaException; 33 | 34 | struct MessageInfo 35 | { 36 | int level; 37 | std::string message; 38 | }; 39 | 40 | /** 41 | * Message defined by COBOTTA 42 | */ 43 | class Message 44 | { 45 | public: 46 | static constexpr const char* TAG = "Message"; 47 | 48 | Message(const uint32_t code); 49 | Message(const uint32_t main_code, const uint32_t sub_code); 50 | virtual ~Message() = default; 51 | 52 | static struct MessageInfo getMessageInfo(const uint32_t); 53 | static bool isWatchdogTimerError(const uint32_t code); 54 | static void putRosConsole(const char* tag, 55 | const uint32_t main_code, const uint32_t sub_code = 0); 56 | static void putRosConsole(const char* tag, const CobottaException& e); 57 | 58 | int getErrorLevel() const; 59 | uint32_t getMainCode() const; 60 | const std::string& getMessage() const; 61 | uint32_t getSubCode() const; 62 | 63 | private: 64 | uint32_t main_code_; 65 | uint32_t sub_code_; 66 | int error_level_; 67 | std::string message_; 68 | static const std::unordered_map code_map_; 69 | 70 | }; 71 | 72 | } /* namespace cobotta */ 73 | } /* namespace denso_cobotta_lib */ 74 | 75 | #endif /* MESSAGE_H */ 76 | -------------------------------------------------------------------------------- /denso_cobotta_lib/include/denso_cobotta_lib/publish_info.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #ifndef PUBLISH_INFO_H 20 | #define PUBLISH_INFO_H 21 | 22 | #include 23 | #include 24 | 25 | #include "denso_cobotta_lib/cobotta_ioctl.h" 26 | #include "denso_cobotta_lib/cobotta_exception.h" 27 | 28 | namespace denso_cobotta_lib 29 | { 30 | namespace cobotta 31 | { 32 | class PublishInfo 33 | { 34 | public: 35 | PublishInfo(); 36 | virtual ~PublishInfo() = default; 37 | 38 | bool isMinusButton() const; 39 | void setMinusButton(bool minus_button); 40 | uint32_t getDriverQueueSize(long arm_no) const throw(std::invalid_argument); 41 | std::array getDriverQueueSize() const; 42 | void setDriverQueueSize(long arm_no, uint32_t queue_size) throw(std::invalid_argument); 43 | void setDriverQueueSize(const std::array& driver_queue); 44 | bool isFunctionButton() const; 45 | void setFunctionButton(bool function_button); 46 | bool isGripperState() const; 47 | void setGripperState(bool gripper_state); 48 | uint16_t getMiniIo() const; 49 | void setMiniIo(uint16_t miniio); 50 | bool isPlusButton() const; 51 | void setPlusButton(bool plus_button); 52 | uint32_t getSafetyMcuQueueSize() const; 53 | void setSafetyMcuQueueSize(uint32_t safety_mcu_queue_size); 54 | 55 | private: 56 | std::array driver_queue_size_; 57 | uint32_t safety_mcu_queue_size_; 58 | uint16_t mini_io_; 59 | bool function_button_; 60 | bool minus_button_; 61 | bool plus_button_; 62 | bool gripper_state_; 63 | }; 64 | 65 | } /* namespace cobotta */ 66 | } /* namespace denso_cobotta_lib */ 67 | 68 | #endif /* PUBLISH_INFO_H */ 69 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/config/vacuum.srdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /denso_cobotta_lib/include/denso_cobotta_lib/motor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #ifndef MOTOR_H 20 | #define MOTOR_H 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include "denso_cobotta_lib/cobotta_exception.h" 27 | #include "denso_cobotta_lib/cobotta.h" 28 | 29 | namespace denso_cobotta_lib 30 | { 31 | namespace cobotta 32 | { 33 | 34 | class Cobotta; 35 | 36 | enum class MotorState 37 | { 38 | Off = 0, 39 | On = 1, 40 | OnProc = 2, 41 | SlowDownStop = 3, 42 | }; 43 | 44 | /** 45 | * Motor class 46 | * - state transition: off -> on proc -> on -> slow down stop -> off 47 | */ 48 | class Motor 49 | { 50 | public: 51 | static constexpr const char* TAG = "Motor"; 52 | 53 | Motor(std::shared_ptr parent); 54 | Motor(std::shared_ptr parent, const enum MotorState state); 55 | virtual ~Motor() = default; 56 | 57 | bool update(const enum MotorState); 58 | bool shouldStop(); 59 | bool isRunning(void) const; 60 | bool canStart(void) const; 61 | void start(void) throw(CobottaException, std::runtime_error); 62 | void stop(void) throw(CobottaException, std::runtime_error); 63 | static void sendStop(int fd) throw(CobottaException, std::runtime_error); 64 | 65 | enum MotorState getState() const; 66 | 67 | static int readHw(int fd) throw(CobottaException, std::runtime_error); 68 | 69 | private: 70 | static void writeHwOn(int fd) throw(CobottaException, std::runtime_error); 71 | static void writeHwOff(int fd) throw(CobottaException, std::runtime_error); 72 | 73 | std::shared_ptr getParent() const; 74 | 75 | std::shared_ptr parent_ = nullptr; 76 | enum MotorState state_ = MotorState::Off; 77 | 78 | }; 79 | 80 | } /* namespace cobotta */ 81 | } /* namespace denso_cobotta_lib */ 82 | 83 | #endif /* MOTOR_H */ 84 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | [move_group/fake_controller_joint_states] 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /denso_cobotta_lib/include/denso_cobotta_lib/brake.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | 20 | #ifndef BRAKE_H 21 | #define BRAKE_H 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include "denso_cobotta_lib/cobotta_exception.h" 28 | #include "denso_cobotta_lib/cobotta.h" 29 | 30 | namespace denso_cobotta_lib 31 | { 32 | namespace cobotta 33 | { 34 | class Cobotta; 35 | 36 | /** 37 | * Brake class 38 | */ 39 | class Brake 40 | { 41 | public: 42 | static constexpr const char* TAG = "Brake"; 43 | 44 | Brake(const std::shared_ptr& parent); 45 | Brake(std::shared_ptr parent, const std::array, ARM_MAX>& state_set); 46 | virtual ~Brake() = default; 47 | 48 | bool update(const std::array, ARM_MAX>& state_set); 49 | bool isLocked(void) const; 50 | bool canChange(void) const; 51 | void change(const int arm_no, const std::array& state) throw(std::invalid_argument,CobottaException); 52 | void releaseAll(const int arm_no) throw(std::invalid_argument); 53 | void releaseAll(void) throw(std::invalid_argument); 54 | void lockAll(const int arm_no) throw(std::invalid_argument); 55 | void lockAll(void) throw(std::invalid_argument); 56 | bool equal(std::array, std::array); 57 | std::array getArmState(int arm_no) throw(std::invalid_argument); 58 | 59 | std::array, ARM_MAX> getState() const; 60 | 61 | private: 62 | static void writeHw(int fd, const int arm_no, const std::array& state) throw(CobottaException); 63 | static std::array readHw(int fd, int arm_no) throw(CobottaException); 64 | 65 | std::shared_ptr getParent() const; 66 | 67 | std::shared_ptr parent_; 68 | std::array, ARM_MAX> state_set_; 69 | }; 70 | 71 | } /* namespace cobotta */ 72 | } /* namespace denso_cobotta_lib */ 73 | 74 | #endif /* BRAKE_H */ 75 | -------------------------------------------------------------------------------- /denso_cobotta_gripper/include/denso_cobotta_gripper/gripper_parallel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | #ifndef GRIPPER_PARALLEL_H 19 | #define GRIPPER_PARALLEL_H 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include "denso_cobotta_gripper/GripperMoveAction.h" 31 | 32 | #include "denso_cobotta_lib/driver.h" 33 | #include "denso_cobotta_gripper/gripper_base.h" 34 | 35 | namespace denso_cobotta_gripper 36 | { 37 | class GripperParallel : public GripperBase 38 | { 39 | public: 40 | static constexpr const char* TAG = "GripperParallel"; 41 | 42 | GripperParallel(); 43 | virtual ~GripperParallel() = default; 44 | bool initialize(ros::NodeHandle& nh) override; 45 | bool read(void) override; 46 | bool write(void) override; 47 | bool update(void) override; 48 | 49 | static void sendStayHere(int fd); 50 | 51 | private: 52 | bool calcGripperCommand(void); 53 | bool sendServoUpdateData(void); 54 | 55 | bool publish(void); 56 | bool subscribe(void); 57 | bool initActionServers(ros::NodeHandle& nh); 58 | bool initGripperMove(const double& target_position, const double& speed_percentage, const double& effort); 59 | 60 | // Action callback functions. 61 | bool gripperCommandActionGoal(const control_msgs::GripperCommandGoalConstPtr& goal); 62 | bool gripperMoveActionGoal(const denso_cobotta_gripper::GripperMoveGoalConstPtr& goal); 63 | void actionCancel(void); 64 | void gripperCommandActionFeedback(void); 65 | void gripperMoveActionFeedback(void); 66 | 67 | // Action servers. 68 | std::shared_ptr> as_gripper_move_; 69 | std::shared_ptr> as_gripper_cmd_; 70 | 71 | // Publisher 72 | ros::Publisher pub_joint_state_; 73 | }; 74 | 75 | } // namespace denso_cobotta_gripper 76 | 77 | #endif // GRIPPER_PRALLEL_H 78 | -------------------------------------------------------------------------------- /denso_cobotta_lib/src/cobotta_exception.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #include 20 | 21 | #include 22 | 23 | #include "denso_cobotta_lib/cobotta_exception.h" 24 | 25 | namespace denso_cobotta_lib 26 | { 27 | namespace cobotta 28 | { 29 | /** 30 | * Constructs a CobottaException object. 31 | * @code 32 | * try { 33 | * throw CobottaException(0x85400001); 34 | * 35 | * } catch (CobottaException& e) { 36 | * ROS_ERROR("%s", e.what()); 37 | * exit(1); 38 | * } 39 | * @endcode 40 | * @param code COBOTTA code defined by Message class 41 | */ 42 | CobottaException::CobottaException(const uint32_t code) 43 | { 44 | struct MessageInfo info = Message::getMessageInfo(code); 45 | 46 | this->code_= code; 47 | this->message_ = info.message; 48 | this->error_level_ = info.level; 49 | 50 | std::stringstream ss; 51 | ss << "[Lv" << this->getErrorLevel(); 52 | ss << ":" << std::hex << std::uppercase << std::setfill('0') << std::setw(8)<< this->getCode(); 53 | ss << "] "; 54 | ss << this->getMessage(); 55 | this->what_message_ = ss.str(); 56 | } 57 | 58 | /** 59 | * Get string identifying CobottaException 60 | * Returns a null terminated character sequence 61 | * that may be used to identify the CobottaException. 62 | * The particular representation pointed 63 | * by the returned value is implementation-defined. 64 | * As a virtual function, derived classes may redefine this function 65 | * so that specific values are returned. 66 | * 67 | * @return A pointer to a c-string: " [error_level:error_code]" 68 | */ 69 | const char* CobottaException::what() const throw () 70 | { 71 | return this->what_message_.c_str(); 72 | } 73 | 74 | int CobottaException::getErrorLevel() const 75 | { 76 | return error_level_; 77 | } 78 | 79 | uint32_t CobottaException::getCode() const 80 | { 81 | return code_; 82 | } 83 | 84 | const std::string CobottaException::getMessage() const 85 | { 86 | return message_; 87 | } 88 | 89 | const std::string& CobottaException::getWhatMessage() const 90 | { 91 | return what_message_; 92 | } 93 | 94 | } /* namespace cobotta */ 95 | } /* namespace denso_cobotta_lib */ 96 | -------------------------------------------------------------------------------- /denso_cobotta_gazebo/launch/denso_cobotta_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /denso_cobotta_lib/src/buttons.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #include "denso_cobotta_lib/buttons.h" 20 | 21 | namespace denso_cobotta_lib 22 | { 23 | namespace cobotta 24 | { 25 | Buttons::Buttons() 26 | { 27 | } 28 | 29 | /** 30 | * Constructs a Button object. 31 | * 32 | * @param[in] function_button initial function button state 33 | * @param[in] plus_button initial plus button state 34 | * @param[in] minus_button initial minus button state 35 | * @param[in] ip_reset_button initial IP reset button state 36 | */ 37 | Buttons::Buttons(bool function_button, bool plus_button, bool minus_button, bool ip_reset_button) 38 | { 39 | this->function_button_ = function_button; 40 | this->plus_button_ = plus_button; 41 | this->minus_button_ = minus_button; 42 | } 43 | 44 | /** 45 | * Update the states of buttons 46 | * @param[in] function_button the update input 47 | * @param[in] plus_button the update input 48 | * @param[in] minus_button the update input 49 | * @return true:changed false:no change 50 | */ 51 | bool Buttons::update(bool function_button, bool plus_button, bool minus_button) 52 | { 53 | bool changed = false; 54 | 55 | if (this->function_button_ != function_button) 56 | { 57 | this->function_button_ = function_button; 58 | changed = true; 59 | } 60 | 61 | if (this->plus_button_ != plus_button) 62 | { 63 | this->plus_button_ = plus_button; 64 | changed = true; 65 | } 66 | 67 | if (this->minus_button_ != minus_button) 68 | { 69 | this->minus_button_ = minus_button; 70 | changed = true; 71 | } 72 | 73 | return changed; 74 | } 75 | 76 | /** 77 | * @return the state of minus button 78 | */ 79 | bool Buttons::isMinusButtonOn() const 80 | { 81 | return minus_button_; 82 | } 83 | 84 | /** 85 | * @return the state of function button 86 | */ 87 | bool Buttons::isFunctionButtonOn() const 88 | { 89 | return function_button_; 90 | } 91 | 92 | /** 93 | * @return the state of plus button 94 | */ 95 | bool Buttons::isPlusButtonOn() const 96 | { 97 | return plus_button_; 98 | } 99 | 100 | } /* namespace cobotta */ 101 | } /* namespace denso_cobotta_lib */ 102 | -------------------------------------------------------------------------------- /denso_cobotta_lib/include/denso_cobotta_lib/cobotta.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | #ifndef COBOTTA_H 19 | #define COBOTTA_H 20 | 21 | #include 22 | #include 23 | 24 | #include "denso_cobotta_lib/publish_info.h" 25 | #include "denso_cobotta_lib/message.h" 26 | #include "denso_cobotta_lib/cobotta_exception.h" 27 | 28 | #include "denso_cobotta_lib/motor.h" 29 | #include "denso_cobotta_lib/brake.h" 30 | #include "denso_cobotta_lib/buttons.h" 31 | #include "denso_cobotta_lib/driver.h" 32 | #include "denso_cobotta_lib/led.h" 33 | #include "denso_cobotta_lib/mini_io.h" 34 | #include "denso_cobotta_lib/safety_mcu.h" 35 | #include "denso_cobotta_lib/gripper.h" 36 | 37 | namespace denso_cobotta_lib 38 | { 39 | namespace cobotta 40 | { 41 | class Brake; 42 | class Buttons; 43 | class Driver; 44 | class Led; 45 | class MiniIo; 46 | class Motor; 47 | class SafetyMcu; 48 | class Gripper; 49 | class PublishInfo; 50 | 51 | struct StateCode 52 | { 53 | uint32_t main_code; 54 | uint32_t sub_code; 55 | }; 56 | 57 | class Cobotta : public std::enable_shared_from_this 58 | { 59 | public: 60 | static constexpr const char* TAG = "Cobotta"; 61 | 62 | Cobotta(); 63 | virtual ~Cobotta() = default; 64 | 65 | bool initialize() throw(CobottaException, std::runtime_error); 66 | void terminate(); 67 | PublishInfo update() throw(CobottaException, std::runtime_error); 68 | 69 | int getFd() const; 70 | const std::unique_ptr& getBrake() const; 71 | const std::unique_ptr& getButtons() const; 72 | const std::unique_ptr& getDriver() const; 73 | const std::unique_ptr& getLed() const; 74 | const std::unique_ptr& getMiniIo() const; 75 | const std::unique_ptr& getMotor() const; 76 | const std::unique_ptr& getSafetyMcu() const; 77 | const std::unique_ptr& getGripper() const; 78 | 79 | private: 80 | int fd_; 81 | std::unique_ptr motor_; 82 | std::unique_ptr brake_; 83 | std::unique_ptr buttons_; 84 | std::unique_ptr driver_; 85 | std::unique_ptr led_; 86 | std::unique_ptr mini_io_; 87 | std::unique_ptr safety_mcu_; 88 | std::unique_ptr gripper_; 89 | 90 | void open() throw(CobottaException); 91 | void close(); 92 | }; 93 | 94 | } /* namespace cobotta */ 95 | } /* namespace denso_cobotta_lib */ 96 | 97 | #endif /* COBOTTA_H */ 98 | -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # CHANGELOG 2 | 3 | ## [1.2.2] -- 2019-09-11 4 | ### Added 5 | - Gazebo 6 | * Added tuned PID parameters. 7 | 8 | ### Changed 9 | - joint_limits.yaml 10 | * Changed max_velocity 11 | 12 | ## [1.2.2-dev] -- 2019-07-09 13 | ### Added 14 | - [Testing] Gazebo 15 | * Supported arm simulation with gazebo. 16 | * Gripper simulation is currently not supported. 17 | * PID gains are currently not tuned. 18 | 19 | Before starting gazebo simulation, **gazebo_ros_control** package is required. 20 | 21 | * Add `sim` argument to denso_cobotta_bringup.launch for gazebo simulation. 22 | ```sh 23 | $ roslaunch denso_cobotta_bringup denso_cobotta_bringup.launch sim:=true gripper_type:=none 24 | ``` 25 | 26 | * [Gazebo system requirements: Intel i5 and Nvidia card.](http://gazebosim.org/tutorials?tut=guided_b1&cat=) 27 | 28 | COBOTTA has Atom CPU and no graphics card, 29 | so it doesn't meet the requirements of Gazebo. 30 | Do Gazebo simulation on your PC. 31 | 32 | ### Changed 33 | - cobotta_description 34 | * Changed for Gazebo simulation. 35 | - Parallel gripper 36 | * Fix gripper_move and gripper_action for RViz. 37 | 38 | Changed parameter 'target_position' to set half width '0 - 0.015' 39 | as we use mimic joint. 40 | 41 | * Fix cancel action. 42 | * Fix feedback action. 43 | - Vacuum gripper 44 | * Fix cancel action. 45 | * Fix feedback action. 46 | - scripts/packing_pose.py 47 | * Supported Gazebo simulation. 48 | * Changed the library moveit-python to moveit-commander. 49 | 50 | ## [1.2.1] -- 2019-07-03 51 | ### Added 52 | - CALSET 53 | * Added CALSET to denso_cobotta_driver node for high precision arm control. 54 | - Vacuum gripper 55 | * Added vacuum gripper action to denso_cobotta_gripper node. 56 | - gripper_type argument 57 | * Added `gripper_type` argument to denso_cobotta_bringup.launch for supporting vacuum gripper. 58 | - packing_pose.py 59 | * Python script for packing COBOTTA in a box. 60 | 61 | ### Changed 62 | - Performance 63 | * Changed update rate of denso_cobotta_gripper. 64 | - joint_limits.yaml 65 | * Changed max_velocity and max_acceleration to default factory setting values. 66 | 67 | ## [1.2.0] -- 2019-04-25 68 | ### Added 69 | - [Testing] packing_pose.py 70 | - Added to comfirm the linux-driver version 71 | 72 | ### Changed 73 | - Changed some asynchronous services to synchronous ones. 74 | * /cobotta/set_motor_state 75 | * /cobotta/set_brake_state 76 | * /cobotta/clear_error 77 | * /cobotta/clear_robot_error 78 | * /cobotta/clear_safe_state 79 | 80 | - Renamed 3 buttons on 3-axis 81 | * position button -> function button 82 | * open button -> plus button 83 | * close button -> minus button 84 | 85 | - Renamed topic to under_scored (cf. wiki.ros.org/CppStyleGuide) 86 | * GripperMove -> gripper_move 87 | 88 | - LED 89 | * Changed LED it can not change color on error or STO by default 90 | * On STO, LED changed to red 91 | 92 | ### Fixed 93 | - Fixed the bug not to open the gripper 94 | - Some fixes to STO 95 | 96 | ## [1.1.0] -- 2019-02-26 97 | 98 | - Publish on the web 99 | 100 | ## [0.1.0] -- 2019-01-23 101 | 102 | - Initial release 103 | -------------------------------------------------------------------------------- /denso_cobotta_lib/include/denso_cobotta_lib/cobotta_common.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #ifndef COBOTTA_COMMON_H 20 | #define COBOTTA_COMMON_H 21 | 22 | #include 23 | #include 24 | #include 25 | #include "denso_cobotta_lib/cobotta_ioctl.h" 26 | 27 | namespace cobotta_common 28 | { 29 | static constexpr int DRIVER_VERSION_MAJOR = 1; 30 | static constexpr int DRIVER_VERSION_MINOR = 2; 31 | 32 | static constexpr const char* PATH_DEVFILE = "/dev/denso_cobotta"; 33 | static constexpr const char* TEMP_PARAMS_PATH = "/tmp/cobotta_parameters.yaml"; 34 | static constexpr uint32_t CONTROL_JOINT_MAX = 6; 35 | static constexpr double SERVO_PERIOD = 0.008; //[s] 36 | static constexpr double DRIVER_UPDATE_PERIOD = 0.01; //[s] 37 | static constexpr uint32_t SRVSTATE_CB_LED = 0x00010101; 38 | static constexpr std::array ARM_COEFF_OUTPOS_TO_PULSE = { 33827.5138204596, 113081.9899244610, 39 | -53656.4738294077, -48164.2778752878, 40 | 46157.4329638175, -46157.4329638175 }; 41 | static constexpr std::array CALSET_POSE = { 150, -60, 140, -170, 135, 170 }; // [deg] 42 | static constexpr std::array HOME_POSE = { 0, 30, 100, 0, 50, 0 }; // [deg] 43 | static constexpr std::array OFFSET_LIMIT = { 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 }; // [rad] 44 | static constexpr uint16_t ACYCLIC_WRITE_MASK = 0x8000; 45 | static constexpr uint16_t POSITION_DEVIATION_ADDRESS = 0x0203;; 46 | static constexpr uint16_t GRIPPER_TYPE_ADDRESS = 0x0800; 47 | static constexpr uint16_t VACUUM_DETECT_ADDRESS = 0x0827; 48 | static constexpr std::array POSITION_DEVIATION_PARAMS = { 16, 53, 47, 43, 40, 61, 37, 37, 10240 }; 49 | 50 | static ros::Duration getPeriod() 51 | { 52 | return ros::Duration(SERVO_PERIOD); 53 | } 54 | 55 | static constexpr double COMMAND_CYCLE = 0.004; /** Update command rate: 4ms (less than 8ms) */ 56 | static constexpr double COMMAND_SHORT_BREAK = SERVO_PERIOD * 2; /** Sleep short time to avoid command overflow */ 57 | static constexpr double COMMAND_LONG_BREAK = SERVO_PERIOD * 8; /** Sleep long time to avoid command overflow */ 58 | 59 | } // namespace cobotta_common 60 | 61 | #endif // COBOTTA_COMMON_H 62 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/config/parallel.srdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /denso_cobotta_lib/src/gripper.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "denso_cobotta_lib/cobotta_ioctl.h" 26 | #include "denso_cobotta_lib/cobotta_exception.h" 27 | #include "denso_cobotta_lib/cobotta.h" 28 | #include "denso_cobotta_lib/gripper.h" 29 | 30 | namespace denso_cobotta_lib 31 | { 32 | namespace cobotta 33 | { 34 | /** 35 | * @brief Constructs a Gripper object. 36 | * 37 | * @param Cobotta object 38 | */ 39 | Gripper::Gripper(const std::shared_ptr& parent) 40 | { 41 | parent_ = parent; 42 | gripper_state_ = false; 43 | } 44 | 45 | /** 46 | * Update the state of gripper. 47 | * @param gripper_state true:hold false:not hold 48 | * @exception CobottaException An error defined by cobotta 49 | * @exception RuntimeError An other error 50 | */ 51 | bool Gripper::update(bool gripper_state) 52 | { 53 | bool changed = false; 54 | 55 | if (this->gripper_state_ != gripper_state) 56 | { 57 | this->gripper_state_ = gripper_state; 58 | changed = true; 59 | } 60 | return changed; 61 | } 62 | 63 | long Gripper::readHwGripperState(int fd) throw(CobottaException, std::runtime_error) 64 | { 65 | IOCTL_DATA_GRIPPER_GETSTATE dat{ 0 }; 66 | 67 | errno = 0; 68 | int ret = ioctl(fd, COBOTTA_IOCTL_GRIPPER_GETSTATE, &dat); 69 | int myerrno = errno; 70 | if (ret != 0) 71 | { 72 | if (myerrno == ECANCELED) 73 | { 74 | throw CobottaException(dat.result); 75 | } 76 | else 77 | { 78 | throw std::runtime_error(std::strerror(myerrno)); 79 | } 80 | } 81 | return dat.hold; 82 | } 83 | 84 | /** 85 | * Convert string to GripperType(none/parallel/vacuum) 86 | * @param string 87 | * @return 88 | */ 89 | GripperType Gripper::convertGripperType(const std::string& gripper_type) 90 | { 91 | if (gripper_type == "parallel") 92 | { 93 | return GripperType::Parallel; 94 | } 95 | else if (gripper_type == "vacuum") 96 | { 97 | return GripperType::Vacuum; 98 | } 99 | else if (gripper_type == "none") 100 | { 101 | return GripperType::None; 102 | } 103 | return GripperType::Undefined; 104 | } 105 | 106 | std::shared_ptr Gripper::getParent() const 107 | { 108 | return parent_; 109 | } 110 | 111 | } /* namespace cobotta */ 112 | } /* namespace denso_cobotta_lib */ 113 | -------------------------------------------------------------------------------- /denso_cobotta_control/include/denso_cobotta_control/denso_cobotta_hw.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #ifndef DENSO_COBOTTA_HW_H 20 | #define DENSO_COBOTTA_HW_H 21 | 22 | // C++ standard 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | // ROS 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include "denso_cobotta_driver/SetMotorState.h" 37 | #include "denso_cobotta_driver/SetLEDState.h" 38 | #include "denso_cobotta_driver/ClearError.h" 39 | #include "denso_cobotta_driver/RobotState.h" 40 | 41 | // COBOTTA device driver 42 | #include 43 | #include 44 | #include 45 | #include "denso_cobotta_lib/cobotta_ioctl.h" 46 | #include "denso_cobotta_lib/cobotta_common.h" 47 | #include "denso_cobotta_lib/cobotta_exception.h" 48 | 49 | namespace denso_cobotta_control 50 | { 51 | using namespace cobotta_common; 52 | 53 | class DensoCobottaHW : public hardware_interface::RobotHW 54 | { 55 | public: 56 | DensoCobottaHW(); 57 | virtual ~DensoCobottaHW(); 58 | 59 | bool initialize(ros::NodeHandle& nh); 60 | void checkMotorState(void); 61 | 62 | bool read(ros::Time, ros::Duration); 63 | bool write(ros::Time, ros::Duration); 64 | bool isMotorOn(); 65 | bool shouldReset(); 66 | void clearReset(); 67 | 68 | static void sendStayHere(int fd); 69 | int getFd() const; 70 | 71 | // Subscriber callback functions. 72 | void subRobotState(const denso_cobotta_driver::RobotState& msg); 73 | 74 | // Subscriber 75 | ros::Subscriber sub_robot_state_; 76 | 77 | private: 78 | bool loadCalsetData(); 79 | bool sendServoUpdateData(); 80 | bool recvEncoderData(); 81 | 82 | hardware_interface::JointStateInterface jntStInterface_; 83 | hardware_interface::PositionJointInterface posJntInterface_; 84 | 85 | double cmd_[CONTROL_JOINT_MAX]; 86 | double pos_[CONTROL_JOINT_MAX]; 87 | double vel_[CONTROL_JOINT_MAX]; 88 | double eff_[CONTROL_JOINT_MAX]; 89 | bool motor_on_; // true:on false:off 90 | bool reset_; 91 | 92 | std::vector pulse_offset_; 93 | 94 | IOCTL_DATA_UPDATE upd_; 95 | 96 | int fd_; 97 | }; 98 | } // namespace denso_cobotta_control 99 | #endif // DENSO_COBOTTA_HW_H 100 | -------------------------------------------------------------------------------- /denso_cobotta_gripper/src/denso_cobotta_gripper.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | #include 19 | #include 20 | 21 | #include "denso_cobotta_lib/cobotta_common.h" 22 | #include "denso_cobotta_lib/gripper.h" 23 | #include "denso_cobotta_gripper/denso_cobotta_gripper.h" 24 | #include "denso_cobotta_gripper/gripper_base.h" 25 | #include "denso_cobotta_gripper/gripper_parallel.h" 26 | #include "denso_cobotta_gripper/gripper_vacuum.h" 27 | 28 | using namespace denso_cobotta_gripper; 29 | using namespace denso_cobotta_lib::cobotta; 30 | 31 | int main(int argc, char** argv) 32 | { 33 | ros::init(argc, argv, "denso_cobotta_gripper"); 34 | ros::NodeHandle nh; 35 | 36 | std::unique_ptr gripper; 37 | std::string gripper_type; 38 | if (!nh.getParam("gripper_type", gripper_type)) 39 | { 40 | ROS_ERROR("Failed to get gripper_type."); 41 | return 1; 42 | } 43 | 44 | switch (Gripper::convertGripperType(gripper_type)) 45 | { 46 | case GripperType::Parallel: 47 | gripper = std::make_unique(); 48 | break; 49 | case GripperType::Vacuum: 50 | gripper = std::make_unique(); 51 | break; 52 | case GripperType::None: 53 | ROS_ERROR("denso_cobotta_gripper was launched although gripper_type is none."); 54 | return 1; 55 | default: 56 | ROS_ERROR("denso_cobotta_gripper was launched although gripper_type %s is not specified.", 57 | gripper_type.c_str()); 58 | return 1; 59 | } 60 | 61 | bool success = gripper->initialize(nh); 62 | if (!success) 63 | { 64 | ROS_ERROR_STREAM("Failed to initialize denso_cobotta_gripper."); 65 | return 1; 66 | } 67 | ROS_INFO_STREAM("Success to initialize denso_cobotta_gripper."); 68 | 69 | gripper->checkMotorState(); 70 | 71 | ros::AsyncSpinner spinner(1); 72 | ros::Rate rate(1.0 / cobotta_common::COMMAND_CYCLE); 73 | spinner.start(); 74 | 75 | while (ros::ok()) 76 | { 77 | success = gripper->read(); 78 | if (!success) 79 | { 80 | break; 81 | } 82 | 83 | if (gripper->isMotorState()) 84 | { 85 | success = gripper->update(); 86 | if (!success) 87 | { 88 | break; 89 | } 90 | 91 | success = gripper->write(); 92 | if (!success) 93 | { 94 | break; 95 | } 96 | } 97 | rate.sleep(); 98 | } 99 | spinner.stop(); 100 | 101 | gripper->sendStayHere(gripper->getFd()); 102 | ROS_INFO("DensoCobotttaGripper has stopped."); 103 | return 0; 104 | } 105 | -------------------------------------------------------------------------------- /denso_cobotta_lib/include/denso_cobotta_lib/led.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #ifndef LED_H 20 | #define LED_H 21 | 22 | #include 23 | #include 24 | 25 | #include "denso_cobotta_lib/cobotta_exception.h" 26 | #include "denso_cobotta_lib/cobotta.h" 27 | 28 | namespace denso_cobotta_lib 29 | { 30 | namespace cobotta 31 | { 32 | 33 | class Cobotta; 34 | 35 | struct LedColor 36 | { 37 | uint8_t blink; 38 | uint8_t red; 39 | uint8_t green; 40 | uint8_t blue; 41 | }; 42 | 43 | enum class LedColorTable : uint32_t 44 | { 45 | White = 0x00645028, 46 | Red = 0x00640000, 47 | Green = 0xff053205, 48 | Blue = 0x00000032, 49 | Yellow = 0x005f3c00 50 | }; 51 | 52 | class Led 53 | { 54 | public: 55 | static constexpr const char* TAG = "Led"; 56 | 57 | Led(std::shared_ptr parent); 58 | virtual ~Led() = default; 59 | 60 | void update() throw(CobottaException, std::runtime_error); 61 | 62 | enum LedColorTable getColorOfState(); 63 | bool canChange(); 64 | 65 | bool change(const uint32_t color) throw(CobottaException, std::runtime_error); 66 | bool change(const uint8_t blink, const uint8_t red, const uint8_t green, const uint8_t blue) 67 | throw(CobottaException, std::runtime_error); 68 | bool change(const struct LedColor color) throw(CobottaException, std::runtime_error); 69 | bool change(const enum LedColorTable color) throw(CobottaException, std::runtime_error); 70 | 71 | void forceChange(const uint32_t color) throw(CobottaException, std::runtime_error); 72 | void forceChange(const uint8_t blink, const uint8_t red, const uint8_t green, const uint8_t blue) 73 | throw(CobottaException, std::runtime_error); 74 | void forceChange(const struct LedColor color) throw(CobottaException, std::runtime_error); 75 | void forceChange(const enum LedColorTable color) throw(CobottaException, std::runtime_error); 76 | 77 | uint32_t receiveColor(void) throw(CobottaException, std::runtime_error); 78 | static struct LedColor toLedColor(const uint32_t state); 79 | static uint32_t toUint32(const struct LedColor color); 80 | 81 | private: 82 | static void writeHw(int fd, uint32_t color) throw(CobottaException, std::runtime_error); 83 | static uint32_t readHw(int fd) throw(CobottaException, std::runtime_error); 84 | 85 | uint32_t getLastColor() const; 86 | void setLastColor(uint32_t color); 87 | 88 | std::shared_ptr getParent() const; 89 | std::shared_ptr parent_; 90 | /** Last setting color */ 91 | uint32_t last_color_; 92 | 93 | static const bool LED_FUNCTIONAL_SAFETY; 94 | static const uint32_t LED_IOCTL_ARG; 95 | }; 96 | 97 | } /* namespace cobotta */ 98 | } /* namespace denso_cobotta_lib */ 99 | 100 | #endif /* LED_H */ 101 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/launch/move_group.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | [/cobotta/joint_states] 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /scripts/packing_pose.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (C) 2019 DENSO WAVE INCORPORATED 3 | # 4 | # -*- coding: utf-8 -*- 5 | # 6 | # usage: python ./packing_pose.py 7 | # 8 | #!/usr/bin/env python 9 | import os 10 | import sys 11 | import rospy 12 | import actionlib 13 | import math 14 | import moveit_commander 15 | import rosservice 16 | from geometry_msgs.msg import Pose, Point, Quaternion 17 | from denso_cobotta_gripper.msg import GripperMoveAction, GripperMoveGoal 18 | from denso_cobotta_driver.srv import GetMotorState 19 | 20 | # NOTE: Before start this program, please launch denso_cobotta_bring.launch 21 | 22 | joints_name = ["joint_1", "joint_2", 23 | "joint_3", "joint_4", "joint_5", "joint_6"] 24 | 25 | # 26 | # Poses 27 | # 28 | joints_packing_old = [90, -60, 125, 90, -95, 0] 29 | joints_packing_new = [90, -30, 120, -170, -94, 0] 30 | joints_home = [0, 30, 100, 0, 50, 0] 31 | 32 | # 33 | # Parallel gripper 34 | # 35 | gripper_parallel_open = 0.015 36 | gripper_parallel_close = 0.0 37 | gripper_parallel_speed = 10.0 38 | gripper_parallel_effort = 10.0 39 | 40 | def arm_move(move_group, joint_goal): 41 | pose_radian = [x / 180.0 * math.pi for x in joint_goal] 42 | move_group.go(pose_radian, wait=True) 43 | move_group.stop() 44 | 45 | 46 | def gripper_move(gripper_client, width, speed, effort): 47 | goal = GripperMoveGoal() 48 | goal.target_position = width 49 | goal.speed = speed 50 | goal.effort = effort 51 | gripper_client.send_goal(goal) 52 | 53 | 54 | def is_motor_running(): 55 | rospy.wait_for_service('/cobotta/get_motor_state', 3.0) 56 | try: 57 | get_motor_state = rospy.ServiceProxy('/cobotta/get_motor_state', 58 | GetMotorState) 59 | res = get_motor_state() 60 | return res.state 61 | except rospy.ServiceException, e: 62 | print >> sys.stderr, " Service call failed: %s" % e 63 | 64 | def is_simulation(): 65 | service_list = rosservice.get_service_list() 66 | if '/cobotta/get_motor_state' in service_list: 67 | return False 68 | return True 69 | 70 | if __name__ == '__main__': 71 | rospy.init_node("packing_pose") 72 | moveit_commander.roscpp_initialize(sys.argv) 73 | move_group = moveit_commander.MoveGroupCommander("arm") 74 | gripper_client = actionlib.SimpleActionClient('/cobotta/gripper_move', 75 | GripperMoveAction) 76 | 77 | 78 | print(os.path.basename(__file__) + " sets pose goal and moves COBOTTA.") 79 | print("0: Old packing pose, 1: New packing pose, 2: Home pose, Others: Exit") 80 | while True: 81 | input = raw_input(" Select the value: ") 82 | if input.isdigit(): 83 | input = int(input) 84 | 85 | joints = [] 86 | gripper_width = 0.0 87 | 88 | if input == 0: 89 | joints = joints_packing_old 90 | gripper_width = gripper_parallel_open 91 | elif input == 1: 92 | joints = joints_packing_new 93 | gripper_width = gripper_parallel_open 94 | elif input == 2: 95 | joints = joints_home 96 | gripper_width = gripper_parallel_close 97 | else: 98 | break 99 | 100 | if not is_simulation() and is_motor_running() is not True: 101 | print >> sys.stderr, " Please motor on." 102 | continue 103 | 104 | gripper_move(gripper_client, gripper_width, 105 | gripper_parallel_speed, gripper_parallel_effort) 106 | arm_move(move_group, joints) 107 | 108 | print("Bye...") 109 | -------------------------------------------------------------------------------- /denso_cobotta_moveit_config/config/cobotta.srdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /denso_cobotta_lib/include/denso_cobotta_lib/safety_mcu.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #ifndef SAFETY_MCU_H 20 | #define SAFETY_MCU_H 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include "denso_cobotta_lib/cobotta_exception.h" 27 | #include "denso_cobotta_lib/cobotta.h" 28 | 29 | namespace denso_cobotta_lib 30 | { 31 | namespace cobotta 32 | { 33 | 34 | class Cobotta; 35 | 36 | enum class SafetyMcuState : int 37 | { 38 | StandBy = 0, 39 | Normal = 1, 40 | SafeState = 2, 41 | }; 42 | 43 | enum class SafetyMcuCommand : uint32_t 44 | { 45 | StandBy = 0, /** safe state -> standby */ 46 | Normal = 2, /** standby -> normal */ 47 | }; 48 | 49 | /** 50 | * SafetyMCU has 3 states. 51 | * - safe state: COBOTTA keeps a safe stop state and contains emergency-stop and protective-stop. 52 | * - standby: - 53 | * - normal: COBOTTA can start motor. 54 | */ 55 | class SafetyMcu 56 | { 57 | public: 58 | static constexpr const char* TAG = "SafetyMcu"; 59 | 60 | SafetyMcu(const std::shared_ptr& parent); 61 | SafetyMcu(const std::shared_ptr& parent, 62 | const enum SafetyMcuState state, const int state_queue, 63 | const bool fatal_error, const bool error, 64 | const bool emergency_button, const bool protective_button); 65 | virtual ~SafetyMcu() = default; 66 | 67 | 68 | bool update(const enum SafetyMcuState state, const int state_queue, 69 | const bool fatal_error, const bool error, 70 | const bool emergency_button, const bool protective_button); 71 | 72 | bool canMoveState(); 73 | void moveToStandby() throw(CobottaException, std::runtime_error); 74 | void moveToNormal() throw(CobottaException, std::runtime_error); 75 | void forceMoveToStandby() throw(CobottaException, std::runtime_error); 76 | struct StateCode dequeue() throw(CobottaException, std::runtime_error); 77 | 78 | bool isNormal() const; 79 | bool isSafeState() const; 80 | bool isEmergencyButton() const; 81 | bool isError() const; 82 | bool isFatalError() const; 83 | bool isProtectiveButton() const; 84 | enum SafetyMcuState getState() const; 85 | int getStateQueue() const; 86 | 87 | private: 88 | static void writeHwState(const int fd, 89 | const enum SafetyMcuCommand command) throw(CobottaException, std::runtime_error); 90 | static struct StateCode readHwQueue(const int fd) throw(CobottaException, std::runtime_error); 91 | 92 | std::shared_ptr getParent() const; 93 | 94 | std::shared_ptr parent_ = nullptr; 95 | enum SafetyMcuState state_ = SafetyMcuState::SafeState; 96 | int state_queue_ = 0; 97 | bool fatal_error_ = false; 98 | bool error_ = false; 99 | bool emergency_button_ = false; 100 | bool protective_button_ = false; 101 | 102 | }; 103 | 104 | } /* namespace cobotta */ 105 | } /* namespace denso_cobotta_lib */ 106 | 107 | #endif /* SAFETY_MCU_H */ 108 | -------------------------------------------------------------------------------- /denso_cobotta_descriptions/cobotta_description/parallel.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | transmission_interface/SimpleTransmission 56 | 57 | hardware_interface/PositionJointInterface 58 | 59 | 60 | hardware_interface/PositionJointInterface 61 | 1 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | -------------------------------------------------------------------------------- /denso_cobotta_gripper/include/denso_cobotta_gripper/gripper_base.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | #ifndef GRIPPER_BASE_H 19 | #define GRIPPER_BASE_H 20 | 21 | // C++ Standards 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | // ROS 29 | #include 30 | #include 31 | #include 32 | 33 | // COBOTTA 34 | #include 35 | #include 36 | #include "denso_cobotta_lib/cobotta_ioctl.h" 37 | #include "denso_cobotta_lib/driver.h" 38 | #include "denso_cobotta_lib/cobotta_exception.h" 39 | #include "denso_cobotta_driver/RobotState.h" 40 | #include "denso_cobotta_lib/cobotta_common.h" 41 | #include "denso_cobotta_lib/gripper.h" 42 | 43 | namespace denso_cobotta_gripper 44 | { 45 | 46 | class GripperBase 47 | { 48 | public: 49 | static constexpr const char* TAG = "GripperBase"; 50 | 51 | GripperBase(); 52 | virtual ~GripperBase() = default; 53 | virtual bool read(void) = 0; 54 | virtual bool write(void) = 0; 55 | virtual bool update(void) = 0; 56 | virtual bool initialize(ros::NodeHandle& nh); 57 | 58 | void checkMotorState(void); 59 | bool isMotorState(void) const; 60 | bool isGraspState() const; 61 | int getFd() const; 62 | 63 | static void sendStayHere(int fd); 64 | 65 | protected: 66 | bool openDeviceFile(void); 67 | bool verifyGripperType(const std::string& gripper_str); 68 | bool loadConfigParams(ros::NodeHandle& nh); 69 | bool initSubscribers(ros::NodeHandle& nh); 70 | 71 | virtual bool publish(void) = 0; 72 | virtual bool subscribe(void) = 0; 73 | 74 | // Subscriber callback functions. 75 | void subRobotState(const denso_cobotta_driver::RobotState& msg); 76 | void subGraspState(const std_msgs::Bool::ConstPtr& msg); 77 | 78 | bool initCurPos(void); 79 | bool isBusy(void); 80 | bool recvEncoderData(void); 81 | bool calcGripperCommand(void); 82 | bool stopMove(void); 83 | bool sendServoUpdateData(void); 84 | void setMotorState(bool); 85 | 86 | int fd_; 87 | enum denso_cobotta_lib::cobotta::GripperType gripper_type_; 88 | std::mutex move_lock_; 89 | bool motor_state_; 90 | bool moving_; 91 | bool grasp_state_; 92 | 93 | // Control parameters. 94 | double current_cmd_position_; 95 | double current_cmd_velocity_; 96 | double current_target_position_; 97 | double current_speed_percentage_; 98 | double current_effort_; 99 | double start_position_; 100 | 101 | double current_position_; 102 | double max_soft_limit_; 103 | double min_soft_limit_; 104 | double max_velocity_; 105 | double max_acceleration_; 106 | double max_speed_percentage_; 107 | double min_speed_percentage_; 108 | double max_effort_; 109 | double min_effort_; 110 | double coeff_outpos_to_pulse_; 111 | double coeff_effort_to_torque_; 112 | constexpr static int TIMES_CURRENT = 1000; 113 | 114 | // Subscribers. 115 | ros::Subscriber sub_robot_state_; 116 | ros::Subscriber sub_grasp_state_; 117 | }; 118 | 119 | } // namespace denso_cobotta_gripper 120 | #endif // GRIPPER_BASE_H 121 | -------------------------------------------------------------------------------- /denso_cobotta_lib/src/publish_info.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #include "denso_cobotta_lib/publish_info.h" 20 | 21 | namespace denso_cobotta_lib 22 | { 23 | namespace cobotta 24 | { 25 | /** 26 | * Constructs PublishInfo object 27 | */ 28 | PublishInfo::PublishInfo() 29 | { 30 | } 31 | 32 | /** 33 | * @return the state of minus button 34 | */ 35 | bool PublishInfo::isMinusButton() const 36 | { 37 | return minus_button_; 38 | } 39 | 40 | /** 41 | * @param[in] the update input 42 | */ 43 | void PublishInfo::setMinusButton(bool minus_button) 44 | { 45 | this->minus_button_ = minus_button; 46 | } 47 | 48 | /** 49 | * @param[in] the arm no of driver queue size 50 | */ 51 | uint32_t PublishInfo::getDriverQueueSize(long arm_no) const throw(std::invalid_argument) 52 | { 53 | return driver_queue_size_[arm_no]; 54 | } 55 | 56 | /** 57 | * @return the array of driver queue size 58 | */ 59 | std::array PublishInfo::getDriverQueueSize() const 60 | { 61 | return driver_queue_size_; 62 | } 63 | 64 | /** 65 | * @param[in] arm_no the arm no of driver queue 66 | * @param[in] queue_size queue_size 67 | */ 68 | void PublishInfo::setDriverQueueSize(long arm_no, uint32_t queue_size) throw(std::invalid_argument) 69 | { 70 | driver_queue_size_[arm_no] = queue_size; 71 | } 72 | 73 | /** 74 | * 75 | */ 76 | void PublishInfo::setDriverQueueSize(const std::array& driver_queue) 77 | { 78 | this->driver_queue_size_ = driver_queue; 79 | } 80 | 81 | /** 82 | * @return the state of function button 83 | */ 84 | bool PublishInfo::isFunctionButton() const 85 | { 86 | return function_button_; 87 | } 88 | 89 | /** 90 | * @param[in] the update input 91 | */ 92 | void PublishInfo::setFunctionButton(bool function_button) 93 | { 94 | this->function_button_ = function_button; 95 | } 96 | 97 | /** 98 | * @return the values of miniI/O 99 | */ 100 | uint16_t PublishInfo::getMiniIo() const 101 | { 102 | return mini_io_; 103 | } 104 | 105 | /** 106 | * @param[in] the update input 107 | */ 108 | void PublishInfo::setMiniIo(uint16_t mini_io) 109 | { 110 | this->mini_io_ = mini_io; 111 | } 112 | 113 | /** 114 | * @return the state of plus button 115 | */ 116 | bool PublishInfo::isPlusButton() const 117 | { 118 | return plus_button_; 119 | } 120 | 121 | /** 122 | * @param[input] the update input 123 | */ 124 | void PublishInfo::setPlusButton(bool plus_button) 125 | { 126 | this->plus_button_ = plus_button; 127 | } 128 | 129 | /** 130 | * @return safety mcu queue 131 | */ 132 | uint32_t PublishInfo::getSafetyMcuQueueSize() const 133 | { 134 | return safety_mcu_queue_size_; 135 | } 136 | 137 | /** 138 | * @param[input] the update input 139 | */ 140 | void PublishInfo::setSafetyMcuQueueSize(uint32_t safety_mcu_queue_size) 141 | { 142 | this->safety_mcu_queue_size_ = safety_mcu_queue_size; 143 | } 144 | 145 | /** 146 | * @return the state of gripper (grasping or not) 147 | */ 148 | bool PublishInfo::isGripperState() const 149 | { 150 | return gripper_state_; 151 | } 152 | 153 | /** 154 | * @param[in] the state of gripper (grasping or not) 155 | */ 156 | void PublishInfo::setGripperState(bool gripper_state) 157 | { 158 | this->gripper_state_ = gripper_state; 159 | } 160 | 161 | } /* namespace cobotta */ 162 | } /* namespace denso_cobotta_lib */ 163 | -------------------------------------------------------------------------------- /denso_cobotta_lib/include/denso_cobotta_lib/driver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #ifndef DRIVER_H 20 | #define DRIVER_H 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include "denso_cobotta_lib/cobotta_exception.h" 28 | #include "denso_cobotta_lib/cobotta.h" 29 | 30 | namespace denso_cobotta_lib 31 | { 32 | namespace cobotta 33 | { 34 | class Cobotta; 35 | struct StateCode; 36 | 37 | /** 38 | * IOCTL_GET_ALLSTATE 39 | */ 40 | struct DriverStateInfo 41 | { 42 | std::array driver_queue_size; 43 | uint32_t safety_mcu_queue_size; 44 | int motor_state; 45 | int safety_mcu_state; 46 | std::array, ARM_MAX> brake_state; 47 | bool emergency_stop; 48 | bool protection_stop; 49 | bool function_button; 50 | bool plus_button; 51 | bool minus_button; 52 | bool ip_reset_button; 53 | bool gripper_state; 54 | bool driver_error; 55 | bool driver_fatal_error; 56 | bool safety_mcu_error; 57 | bool safety_mcu_fatal_error; 58 | uint16_t mini_io_input; 59 | uint16_t mini_io_output; 60 | }; 61 | 62 | /** 63 | * IOCTL_GET_VERSION 64 | */ 65 | struct DriverVersion 66 | { 67 | int driver_major; 68 | int driver_minor; 69 | int driver_revision; 70 | int driver_build; 71 | std::string fpga_main; 72 | std::string fpga_main_backup; 73 | std::string mcu; 74 | std::string safety_mcu; 75 | }; 76 | 77 | /** 78 | * IOCTL_SRV_UPDATE 79 | */ 80 | struct DriverCommandInfo 81 | { 82 | uint32_t result = 0; 83 | int queue_num = 0; 84 | bool stay_here = false; 85 | }; 86 | 87 | class Driver 88 | { 89 | public: 90 | static constexpr const char* TAG = "Driver"; 91 | 92 | Driver(const std::shared_ptr& parent); 93 | virtual ~Driver() = default; 94 | 95 | int openDeviceFile() throw(CobottaException, std::runtime_error); 96 | void closeDeviceFile(int fd); 97 | 98 | bool update(const std::array& driver_queue_size, bool driver_error, bool driver_fatal_error, 99 | bool ip_reset_button); 100 | bool isTargetVersion() const; 101 | void printVersion() const; 102 | void clearError() throw(CobottaException, std::runtime_error); 103 | StateCode dequeue(long arm_no) throw(CobottaException, std::runtime_error, std::invalid_argument); 104 | DriverStateInfo receiveAllState() throw(CobottaException, std::runtime_error); 105 | uint32_t getStateQueueSize(long arm_no) const throw(std::invalid_argument); 106 | DriverVersion getVersion() const; 107 | bool isError() const; 108 | bool isFatalError() const; 109 | bool isIpResetButtonOn() const; 110 | 111 | static void checkArmNo(long arm_no) throw(std::invalid_argument); 112 | static void checkJointNo(long joint_no) throw(std::invalid_argument); 113 | 114 | static struct DriverCommandInfo writeHwUpdate(int fd, const SRV_COMM_SEND& send_data) throw(std::runtime_error); 115 | static SRV_COMM_RECV readHwEncoder(int fd, long arm_no) throw(CobottaException, std::runtime_error, 116 | std::invalid_argument); 117 | static void writeHwAcyclicCommAll(int fd, uint16_t address, std::array send_values, 118 | std::array& recv_values) throw(CobottaException, 119 | std::runtime_error); 120 | static void writeHwAcyclicComm(int fd, long arm_no, long joint_no, uint16_t address, uint16_t send_value, 121 | uint16_t& recv_value) throw(CobottaException, std::invalid_argument, 122 | std::runtime_error); 123 | 124 | private: 125 | static DriverVersion readHwVersion(int fd) throw(CobottaException, std::runtime_error); 126 | static void writeHwClear(int fd) throw(CobottaException, std::runtime_error); 127 | static StateCode readHwQueue(int fd, long arm_no) throw(CobottaException, std::runtime_error); 128 | static DriverStateInfo readHwState(int fd) throw(CobottaException, std::runtime_error); 129 | 130 | std::shared_ptr parent_; 131 | 132 | std::array queue_size_; 133 | 134 | bool ip_reset_button_; 135 | bool error_; 136 | bool fatal_error_; 137 | DriverVersion version_; 138 | }; 139 | 140 | } /* namespace cobotta */ 141 | } /* namespace denso_cobotta_lib */ 142 | 143 | #endif /* DRIVER_H */ 144 | -------------------------------------------------------------------------------- /denso_cobotta_descriptions/cobotta_description/J6.dae: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | DENSO WAVE 6 | 7 | 8 | 9 | Y_UP 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 0.04 0.04 0.04 1 23 | 24 | 25 | 1 1 1 1 26 | 27 | 28 | 0.2 0.2 0.2 1 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 0.0221634 -0.0092925 -0.00349998 0.0221634 -0.0092925 0 0.024 0 0 0.024 0 -0.00349998 0.0169706 -0.0169706 -0.00349998 0.0169706 -0.0169706 0 0.00929247 -0.0221634 -0.00349998 0.00929247 -0.0221634 0 0 -0.024 -0.00349998 0 -0.024 0 0 0.024 -0.00349998 0.00929247 0.0221634 0 0 0.024 0 0.00929247 0.0221634 -0.00349998 0.0169706 0.0169706 0 0.0169706 0.0169706 -0.00349998 0.0221634 0.0092925 0 0.0221634 0.0092925 -0.00349998 0.024 0 0 0.024 0 -0.00349998 -0.0221634 -0.0092925 0 -0.024 0 0 -0.0169706 -0.0169706 0 -0.0092925 -0.0221634 0 0.00929247 0.0221634 0 0 -0.024 0 0 0.024 0 0.00929247 -0.0221634 0 0.0169706 -0.0169706 0 0.0221634 -0.0092925 0 0.024 0 0 0.0169706 0.0169706 0 0.0221634 0.0092925 0 -0.0092925 0.0221634 0 -0.0169706 0.0169706 0 -0.0221634 0.0092925 0 -0.024 0 -0.00349998 -0.0221634 0.0092925 0 -0.024 0 0 -0.0221634 0.0092925 -0.00349998 -0.0169706 0.0169706 0 -0.0169706 0.0169706 -0.00349998 -0.0092925 0.0221634 0 -0.0092925 0.0221634 -0.00349998 0 0.024 0 0 0.024 -0.00349998 -0.024 0 0 -0.0221634 -0.0092925 -0.00349998 -0.024 0 -0.00349998 -0.0221634 -0.0092925 0 -0.0169706 -0.0169706 -0.00349998 -0.0169706 -0.0169706 0 -0.0092925 -0.0221634 -0.00349998 -0.0092925 -0.0221634 0 0 -0.024 -0.00349998 0 -0.024 0 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 0.92064 -0.390412 0 0.92064 -0.390412 0 0.99991 0.013433 0 0.99991 0.013433 0 0.707107 -0.707107 0 0.707107 -0.707107 0 0.390412 -0.92064 0 0.390412 -0.92064 0 -0.013433 -0.99991 0 -0.013433 -0.99991 0 -0.013433 0.99991 0 0.390412 0.92064 0 -0.013433 0.99991 0 0.390412 0.92064 0 0.707107 0.707107 0 0.707107 0.707107 0 0.92064 0.390412 0 0.92064 0.390412 0 0.99991 -0.013433 0 0.99991 -0.013433 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 -0.99991 -0.013433 0 -0.92064 0.390412 0 -0.99991 -0.013433 0 -0.92064 0.390412 0 -0.707107 0.707107 0 -0.707107 0.707107 0 -0.390412 0.92064 0 -0.390412 0.92064 0 0.013433 0.99991 0 0.013433 0.99991 0 -0.99991 0.013433 0 -0.92064 -0.390412 0 -0.99991 0.013433 0 -0.92064 -0.390412 0 -0.707107 -0.707107 0 -0.707107 -0.707107 0 -0.390412 -0.92064 0 -0.390412 -0.92064 0 0.013433 -0.99991 0 0.013433 -0.99991 0 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 |

0 0 2 2 1 1 3 3 2 2 0 0 4 4 1 1 5 5 0 0 1 1 4 4 6 6 5 5 7 7 4 4 5 5 6 6 8 8 7 7 9 9 6 6 7 7 8 8 10 10 12 12 11 11 13 13 10 10 11 11 13 13 11 11 14 14 15 15 13 13 14 14 15 15 14 14 16 16 17 17 15 15 16 16 17 17 16 16 18 18 19 19 17 17 18 18 20 20 22 22 21 21 23 23 25 25 24 24 24 24 26 26 23 23 25 25 27 27 24 24 27 27 28 28 24 24 28 28 29 29 24 24 29 29 30 30 24 24 24 24 30 30 31 31 32 32 31 31 30 30 26 26 33 33 23 23 33 33 34 34 23 23 34 34 35 35 23 23 35 35 21 21 23 23 23 23 21 21 22 22 36 36 38 38 37 37 39 39 36 36 37 37 39 39 37 37 40 40 41 41 39 39 40 40 41 41 40 40 42 42 43 43 41 41 42 42 43 43 42 42 44 44 45 45 43 43 44 44 46 46 48 48 47 47 49 49 46 46 47 47 49 49 47 47 50 50 51 51 49 49 50 50 51 51 50 50 52 52 53 53 51 51 52 52 53 53 52 52 54 54 55 55 53 53 54 54

65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /denso_cobotta_driver/include/denso_cobotta_driver/denso_cobotta_driver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #ifndef DENSO_COBOTTA_DRIVER_H 20 | #define DENSO_COBOTTA_DRIVER_H 21 | 22 | // C++ standard 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | // ROS 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include "denso_cobotta_driver/GetBrakeState.h" 36 | #include "denso_cobotta_driver/GetMotorState.h" 37 | #include "denso_cobotta_driver/SetBrakeState.h" 38 | #include "denso_cobotta_driver/SetMotorState.h" 39 | #include "denso_cobotta_driver/SetLEDState.h" 40 | #include "denso_cobotta_driver/ClearError.h" 41 | #include "denso_cobotta_driver/ClearRobotError.h" 42 | #include "denso_cobotta_driver/ClearSafeState.h" 43 | #include "denso_cobotta_driver/RobotState.h" 44 | #include "denso_cobotta_driver/SafeState.h" 45 | #include "denso_cobotta_driver/ExecCalset.h" 46 | 47 | // COBOTTA device driver 48 | #include 49 | #include 50 | #include "denso_cobotta_lib/cobotta_ioctl.h" 51 | #include "denso_cobotta_lib/cobotta_common.h" 52 | #include "denso_cobotta_lib/cobotta.h" 53 | #include "denso_cobotta_lib/publish_info.h" 54 | 55 | namespace denso_cobotta_driver 56 | { 57 | using namespace cobotta_common; 58 | using namespace denso_cobotta_lib; 59 | 60 | /** 61 | * ROS node: DensoCobottaDriver 62 | */ 63 | class DensoCobottaDriver 64 | { 65 | public: 66 | DensoCobottaDriver(); 67 | virtual ~DensoCobottaDriver() = default; 68 | 69 | bool initialize(ros::NodeHandle& nh); 70 | void start(); 71 | void stop(); 72 | void terminate(); 73 | void update(); 74 | void publish(const bool sync, const denso_cobotta_lib::cobotta::PublishInfo pi); 75 | 76 | // Service callback functions. 77 | bool setMotorStateSv(SetMotorState::Request& req, SetMotorState::Response& res); 78 | bool getMotorStateSv(GetMotorState::Request& req, GetMotorState::Response& res); 79 | bool setBrakeStateSv(SetBrakeState::Request& req, SetBrakeState::Response& res); 80 | bool getBrakeStateSv(GetBrakeState::Request& req, GetBrakeState::Response& res); 81 | bool execCalsetSv(ExecCalset::Request& req, ExecCalset::Response& res); 82 | bool clearErrorSv(ClearError::Request& req, ClearError::Response& res); 83 | bool clearRobotErrorSv(ClearError::Request& req, ClearError::Response& res); 84 | bool clearSafeStateSv(ClearError::Request& req, ClearError::Response& res); 85 | bool setLedStateSv(SetLEDState::Request& req, SetLEDState::Response& res); 86 | 87 | // Subscriber callback functions. 88 | void miniIoOutputCallback(const std_msgs::UInt16::ConstPtr& msg); 89 | 90 | private: 91 | struct MoveParam 92 | { 93 | std::array target_position; 94 | std::array max_velocity; 95 | std::array max_acceleration; 96 | int16_t current_limit[JOINT_MAX]; 97 | int16_t current_offset[JOINT_MAX]; 98 | }; 99 | bool activateCalset(ros::NodeHandle& nh); 100 | bool loadJointLimitsParams(ros::NodeHandle& nh); 101 | 102 | void dequeueDriver(long arm_no, int count, bool sync = false); 103 | void dequeueSafetyMcu(int count, bool sync = false); 104 | void putRosLog(const char* tag, uint32_t main_code, uint32_t sub_code); 105 | const std::shared_ptr& getCobotta() const; 106 | 107 | bool setDeviationParameters(const std::array& values); 108 | bool recvPulse(long arm_no, std::array& pulse); 109 | bool sineMove(const MoveParam& move_param); 110 | bool calculateVelocity(const MoveParam& move_param, const std::array& rotation_angle, 111 | std::array& velocity, int32_t& max_count); 112 | bool sendServoUpdateData(const SRV_COMM_SEND& send_data); 113 | 114 | bool isForceClearFlag() const; 115 | void setForceClearFlag(bool); 116 | 117 | // Service server 118 | ros::ServiceServer sv_set_motor_; 119 | ros::ServiceServer sv_get_motor_; 120 | ros::ServiceServer sv_clear_error_; 121 | ros::ServiceServer sv_clear_robot_error_; 122 | ros::ServiceServer sv_clear_safe_state_; 123 | ros::ServiceServer sv_set_brake_; 124 | ros::ServiceServer sv_get_brake_; 125 | ros::ServiceServer sv_exec_calset_; 126 | ros::ServiceServer sv_set_led_; 127 | 128 | // Publisher 129 | ros::Publisher pub_function_button_; 130 | ros::Publisher pub_plus_button_; 131 | ros::Publisher pub_minus_button_; 132 | ros::Publisher pub_mini_io_input_; 133 | ros::Publisher pub_robot_state_; 134 | ros::Publisher pub_safe_state_; 135 | ros::Publisher pub_gripper_state_; 136 | 137 | // Subscriber 138 | ros::Subscriber sub_mini_io_output_; 139 | 140 | // cobotta 141 | std::shared_ptr cobotta_; 142 | 143 | std_msgs::Bool function_button_state_; 144 | std_msgs::Bool plus_button_state_; 145 | std_msgs::Bool minus_button_state_; 146 | std_msgs::UInt16 mini_io_state_; 147 | 148 | bool force_clear_flag_ = false; 149 | 150 | std::array max_acceleration_; 151 | std::array max_velocity_; 152 | // Rang value 153 | std::vector rang_value_; 154 | }; 155 | } // namespace denso_cobotta_driver 156 | 157 | #endif // DENSO_COBOTTA_DRIVER_H 158 | -------------------------------------------------------------------------------- /denso_cobotta_lib/src/cobotta.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | #include 19 | #include 20 | 21 | #include "denso_cobotta_lib/cobotta_common.h" 22 | #include "denso_cobotta_lib/cobotta.h" 23 | 24 | namespace denso_cobotta_lib 25 | { 26 | namespace cobotta 27 | { 28 | /** 29 | * Constructs a Cobotta object. 30 | */ 31 | Cobotta::Cobotta() 32 | { 33 | this->fd_ = 0; 34 | } 35 | 36 | /** 37 | * @brief Initialize 38 | * 39 | * Prepare motor_, brake_, ... . 40 | * Open device file. 41 | * Check version 42 | * Update all state. 43 | * @return true or false 44 | * @exception CobottaException An error defined by COBOTTA 45 | * @exception RuntimeError An other error 46 | */ 47 | bool Cobotta::initialize() throw(CobottaException, std::runtime_error) 48 | { 49 | auto motor = std::make_unique(shared_from_this()); 50 | motor_.swap(motor); 51 | auto brake = std::make_unique(shared_from_this()); 52 | brake_.swap(brake); 53 | auto buttons = std::make_unique(); 54 | buttons_.swap(buttons); 55 | auto driver = std::make_unique(shared_from_this()); 56 | driver_.swap(driver); 57 | auto led = std::make_unique(shared_from_this()); 58 | led_.swap(led); 59 | auto mini_io = std::make_unique(shared_from_this()); 60 | mini_io_.swap(mini_io); 61 | auto safety_mcu = std::make_unique(shared_from_this()); 62 | safety_mcu_.swap(safety_mcu); 63 | auto gripper = std::make_unique(shared_from_this()); 64 | gripper_.swap(gripper); 65 | 66 | this->close(); 67 | this->open(); 68 | this->getDriver()->printVersion(); 69 | 70 | if (!getDriver()->isTargetVersion()) 71 | { 72 | std::runtime_error("Invalid version of DENSO COBOTTA driver."); 73 | return false; 74 | } 75 | std::array data; 76 | Driver::writeHwAcyclicCommAll(this->getFd(), 77 | cobotta_common::POSITION_DEVIATION_ADDRESS | cobotta_common::ACYCLIC_WRITE_MASK, 78 | cobotta_common::POSITION_DEVIATION_PARAMS, data); 79 | return true; 80 | } 81 | 82 | /** 83 | * @brief Terminate 84 | * Call me at last. 85 | */ 86 | void Cobotta::terminate() 87 | { 88 | this->close(); 89 | } 90 | 91 | /** 92 | * @brief Update all state. 93 | * 94 | * If state is changed, update LED color and motor. 95 | * 96 | * @return PublishInfo Driver and SafetyMcu state. 97 | * @exception CobottaException An error defined by COBOTTA 98 | * @exception RuntimeError An other error 99 | */ 100 | PublishInfo Cobotta::update() throw(CobottaException, std::runtime_error) 101 | { 102 | auto all_state = getDriver()->receiveAllState(); 103 | bool changed = false; 104 | 105 | changed |= getBrake()->update(all_state.brake_state); 106 | changed |= getButtons()->update(all_state.function_button, all_state.plus_button, all_state.minus_button); 107 | changed |= getDriver()->update(all_state.driver_queue_size, all_state.driver_error, all_state.driver_fatal_error, 108 | all_state.ip_reset_button); 109 | changed |= getMiniIo()->update(all_state.mini_io_input, all_state.mini_io_output); 110 | changed |= getMotor()->update((enum MotorState)all_state.motor_state); 111 | changed |= getSafetyMcu()->update((enum SafetyMcuState)all_state.safety_mcu_state, all_state.safety_mcu_queue_size, 112 | all_state.safety_mcu_fatal_error, all_state.safety_mcu_error, 113 | all_state.emergency_stop, all_state.protection_stop); 114 | changed |= getGripper()->update(all_state.gripper_state); 115 | 116 | if (changed) 117 | { 118 | getLed()->update(); 119 | if (this->getMotor()->shouldStop()) 120 | { 121 | Motor::sendStop(this->getFd()); 122 | } 123 | } 124 | 125 | PublishInfo pub_info; 126 | pub_info.setFunctionButton(all_state.function_button); 127 | pub_info.setPlusButton(all_state.plus_button); 128 | pub_info.setMinusButton(all_state.minus_button); 129 | pub_info.setDriverQueueSize(all_state.driver_queue_size); 130 | pub_info.setSafetyMcuQueueSize(all_state.safety_mcu_queue_size); 131 | pub_info.setMiniIo(all_state.mini_io_input); 132 | pub_info.setGripperState(all_state.gripper_state); 133 | 134 | return pub_info; 135 | } 136 | 137 | const std::unique_ptr& Cobotta::getBrake() const 138 | { 139 | return brake_; 140 | } 141 | 142 | const std::unique_ptr& Cobotta::getButtons() const 143 | { 144 | return buttons_; 145 | } 146 | 147 | const std::unique_ptr& Cobotta::getDriver() const 148 | { 149 | return driver_; 150 | } 151 | 152 | const std::unique_ptr& Cobotta::getLed() const 153 | { 154 | return led_; 155 | } 156 | 157 | const std::unique_ptr& Cobotta::getMiniIo() const 158 | { 159 | return mini_io_; 160 | } 161 | 162 | const std::unique_ptr& Cobotta::getMotor() const 163 | { 164 | return motor_; 165 | } 166 | 167 | const std::unique_ptr& Cobotta::getSafetyMcu() const 168 | { 169 | return safety_mcu_; 170 | } 171 | 172 | const std::unique_ptr& Cobotta::getGripper() const 173 | { 174 | return gripper_; 175 | } 176 | 177 | /** 178 | * Device open 179 | */ 180 | void Cobotta::open() throw(CobottaException) 181 | { 182 | if (fd_ == 0) 183 | { 184 | fd_ = getDriver()->openDeviceFile(); 185 | } 186 | } 187 | 188 | /** 189 | * Device close 190 | */ 191 | void Cobotta::close() 192 | { 193 | if (fd_ != 0) 194 | { 195 | getDriver()->closeDeviceFile(fd_); 196 | fd_ = 0; 197 | } 198 | } 199 | 200 | int Cobotta::getFd() const 201 | { 202 | return fd_; 203 | } 204 | 205 | } /* namespace cobotta */ 206 | } /* namespace denso_cobotta_lib */ 207 | -------------------------------------------------------------------------------- /denso_cobotta_lib/src/mini_io.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | #include "denso_cobotta_lib/cobotta_common.h" 25 | #include "denso_cobotta_lib/cobotta_ioctl.h" 26 | #include "denso_cobotta_lib/mini_io.h" 27 | 28 | namespace denso_cobotta_lib 29 | { 30 | namespace cobotta 31 | { 32 | /** 33 | * Constructs a MiniIo object. 34 | * @param parent cobotta object 35 | */ 36 | MiniIo::MiniIo(std::shared_ptr parent) 37 | { 38 | this->parent_ = parent; 39 | } 40 | 41 | /** 42 | * Constructs a MiniIo object. 43 | * @param parent 44 | * @param input 45 | * @param output 46 | */ 47 | MiniIo::MiniIo(std::shared_ptr parent, uint16_t input, uint16_t output) 48 | { 49 | this->parent_ = parent; 50 | this->input_ = input; 51 | this->output_ = output; 52 | } 53 | 54 | /** 55 | * Update my state. 56 | * @param input MiniIO input (16bit) 57 | * @param output MiniIO output (16bit) 58 | * @return true:changed false:no change 59 | */ 60 | bool MiniIo::update(const uint16_t input, uint16_t output) 61 | { 62 | bool isChanged = false; 63 | 64 | if (this->input_ != input) 65 | { 66 | this->input_ = input; 67 | isChanged = true; 68 | } 69 | if (this->output_ != output) 70 | { 71 | this->output_ = output; 72 | isChanged = true; 73 | } 74 | return isChanged; 75 | } 76 | 77 | /** 78 | * [sync] Send a mini-output data to cobotta 79 | * @param state 16bit output value 80 | * @exception CobottaException An error defined by cobotta 81 | * @exception RuntimeError An other error 82 | */ 83 | void MiniIo::sendOutputStateValue(const uint16_t state) throw(CobottaException, std::runtime_error) 84 | { 85 | this->writeHwOutput(this->getParent()->getFd(), state); 86 | while (state != this->output_) 87 | { 88 | ros::Duration(cobotta_common::getPeriod()).sleep(); 89 | } 90 | } 91 | 92 | /** 93 | * Receive a mini-output data from cobotta 94 | * @return 16bit output value 95 | * @exception CobottaException An error defined by cobotta 96 | * @exception RuntimeError An other error 97 | */ 98 | uint16_t MiniIo::receiveOutputStateValue() const throw(CobottaException, std::runtime_error) 99 | { 100 | return this->readHwOutput(this->getParent()->getFd()); 101 | } 102 | 103 | /** 104 | * Receive a mini-input data from cobotta 105 | * @return 16bit input value 106 | * @exception CobottaException An error defined by cobotta 107 | * @exception RuntimeError An other error 108 | */ 109 | uint16_t MiniIo::receiveInputStateValue() const throw(CobottaException, std::runtime_error) 110 | { 111 | return this->readHwInput(this->getParent()->getFd()); 112 | } 113 | 114 | /** 115 | * [ASYNC] IOCTL_MINI_OUTPUT 116 | * @param fd file descriptor 117 | * @param value 16bit output value 118 | * @exception CobottaException An error defined by cobotta 119 | * @exception RuntimeError An other error 120 | */ 121 | void MiniIo::writeHwOutput(const int fd, uint16_t value) throw(CobottaException, std::runtime_error) 122 | { 123 | int ret; 124 | IOCTL_DATA_MINI_OUTPUT dat; 125 | int myerrno; 126 | 127 | memset(&dat, 0, sizeof(dat)); 128 | errno = 0; 129 | dat.data = value; 130 | dat.mask = 0xffff; 131 | ret = ioctl(fd, COBOTTA_IOCTL_MINI_OUTPUT, &dat); 132 | myerrno = errno; 133 | ROS_DEBUG("%s: COBOTTA_IOCTL_MINI_OUTPUT ret=%d errno=%d result=%lX\n", 134 | TAG, ret, myerrno, dat.result); 135 | if (ret) 136 | { 137 | ROS_ERROR("%s: ioctl(COBOTTA_IOCTL_MINI_OUTPUT): %s", 138 | TAG, std::strerror(myerrno)); 139 | if (myerrno == ECANCELED) 140 | { 141 | throw CobottaException(dat.result); 142 | } 143 | throw std::runtime_error(std::strerror(myerrno)); 144 | } 145 | } 146 | /** 147 | * [ASYNC] IOCTL_MINI_OUTPUT_READ 148 | * @param fd file descriptor 149 | * @return 16bit output value 150 | * @exception CobottaException An error defined by cobotta 151 | * @exception RuntimeError An other error 152 | */ 153 | uint16_t MiniIo::readHwOutput(const int fd) throw(CobottaException, std::runtime_error) 154 | { 155 | int ret; 156 | IOCTL_DATA_MINI_OUTPUT_READ dat; 157 | int myerrno; 158 | 159 | memset(&dat, 0, sizeof(dat)); 160 | errno = 0; 161 | ret = ioctl(fd, COBOTTA_IOCTL_MINI_OUTPUT_READ, &dat); 162 | myerrno = errno; 163 | ROS_DEBUG("%s: COBOTTA_IOCTL_MINI_OUTPUT_READ ret=%d errno=%d result=%lX\n", 164 | TAG, ret, myerrno, dat.result); 165 | if (ret) 166 | { 167 | ROS_ERROR("%s: ioctl(COBOTTA_IOCTL_MINI_OUTPUT_READ): %s", TAG, std::strerror(myerrno)); 168 | if (myerrno == ECANCELED) 169 | { 170 | throw CobottaException(dat.result); 171 | } 172 | throw std::runtime_error(std::strerror(myerrno)); 173 | } 174 | return dat.data; 175 | } 176 | /** 177 | * [ASYNC] IOCTL_MINI_INPUT 178 | * @param fd file descriptor 179 | * @return 16bit input value 180 | * @exception CobottaException An error defined by cobotta 181 | * @exception RuntimeError An other error 182 | */ 183 | uint16_t MiniIo::readHwInput(const int fd) throw(CobottaException, std::runtime_error) 184 | { 185 | int ret; 186 | IOCTL_DATA_MINI_INPUT dat; 187 | int myerrno; 188 | 189 | memset(&dat, 0, sizeof(dat)); 190 | errno = 0; 191 | ret = ioctl(fd, COBOTTA_IOCTL_MINI_INPUT, &dat); 192 | myerrno = errno; 193 | ROS_DEBUG("%s: COBOTTA_IOCTL_MINI_INPUT ret=%d errno=%d result=%lX\n", 194 | TAG, ret, myerrno, dat.result); 195 | if (ret) 196 | { 197 | ROS_ERROR("%s: ioctl(COBOTTA_IOCTL_MINI_INPUT): %s", 198 | TAG, std::strerror(myerrno)); 199 | if (myerrno == ECANCELED) 200 | { 201 | throw CobottaException(dat.result); 202 | } 203 | throw std::runtime_error(std::strerror(myerrno)); 204 | } 205 | return dat.data; 206 | } 207 | 208 | std::shared_ptr MiniIo::getParent() const 209 | { 210 | return this->parent_; 211 | } 212 | 213 | } /* namespace cobotta */ 214 | } /* namespace denso_cobotta_lib */ 215 | -------------------------------------------------------------------------------- /denso_cobotta_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(denso_cobotta_gazebo) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 34 | ## but can be declared for certainty nonetheless: 35 | ## * add a exec_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # std_msgs # Or other packages containing msgs 71 | # ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if your package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | # LIBRARIES denso_cobotta_gazebo 105 | # CATKIN_DEPENDS gazebo gazebo_plugins gazebo_ros_control 106 | # DEPENDS system_lib 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | include_directories( 116 | # include 117 | ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(${PROJECT_NAME} 122 | # src/${PROJECT_NAME}/denso_cobotta_gazebo.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | ## With catkin_make all packages are built within a single CMake context 132 | ## The recommended prefix ensures that target names across packages don't collide 133 | # add_executable(${PROJECT_NAME}_node src/denso_cobotta_gazebo_node.cpp) 134 | 135 | ## Rename C++ executable without prefix 136 | ## The above recommended prefix causes long target names, the following renames the 137 | ## target back to the shorter version for ease of user use 138 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 139 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 140 | 141 | ## Add cmake target dependencies of the executable 142 | ## same as for the library above 143 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 144 | 145 | ## Specify libraries to link a library or executable target against 146 | # target_link_libraries(${PROJECT_NAME}_node 147 | # ${catkin_LIBRARIES} 148 | # ) 149 | 150 | ############# 151 | ## Install ## 152 | ############# 153 | 154 | # all install targets should use catkin DESTINATION variables 155 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 156 | 157 | ## Mark executable scripts (Python etc.) for installation 158 | ## in contrast to setup.py, you can choose the destination 159 | # install(PROGRAMS 160 | # scripts/my_python_script 161 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 162 | # ) 163 | 164 | ## Mark executables and/or libraries for installation 165 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 166 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 167 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 168 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 169 | # ) 170 | 171 | ## Mark cpp header files for installation 172 | # install(DIRECTORY include/${PROJECT_NAME}/ 173 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 174 | # FILES_MATCHING PATTERN "*.h" 175 | # PATTERN ".svn" EXCLUDE 176 | # ) 177 | 178 | ## Mark other files for installation (e.g. launch and bag files, etc.) 179 | # install(FILES 180 | # # myfile1 181 | # # myfile2 182 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 183 | # ) 184 | 185 | ############# 186 | ## Testing ## 187 | ############# 188 | 189 | ## Add gtest based cpp test target and link libraries 190 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_denso_cobotta_gazebo.cpp) 191 | # if(TARGET ${PROJECT_NAME}-test) 192 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 193 | # endif() 194 | 195 | ## Add folders to be run by python nosetests 196 | # catkin_add_nosetests(test) 197 | -------------------------------------------------------------------------------- /denso_cobotta_lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(denso_cobotta_lib) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++14) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################################ 24 | ## Declare ROS messages, services and actions ## 25 | ################################################ 26 | 27 | ## To declare and build messages, services or actions from within this 28 | ## package, follow these steps: 29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 31 | ## * In the file package.xml: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 34 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 35 | ## but can be declared for certainty nonetheless: 36 | ## * add a exec_depend tag for "message_runtime" 37 | ## * In this file (CMakeLists.txt): 38 | ## * add "message_generation" and every package in MSG_DEP_SET to 39 | ## find_package(catkin REQUIRED COMPONENTS ...) 40 | ## * add "message_runtime" and every package in MSG_DEP_SET to 41 | ## catkin_package(CATKIN_DEPENDS ...) 42 | ## * uncomment the add_*_files sections below as needed 43 | ## and list every .msg/.srv/.action file to be processed 44 | ## * uncomment the generate_messages entry below 45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 46 | 47 | ## Generate messages in the 'msg' folder 48 | # add_message_files( 49 | # FILES 50 | # Message1.msg 51 | # Message2.msg 52 | # ) 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | ## Generate added messages and services with any dependencies listed here 69 | # generate_messages( 70 | # DEPENDENCIES 71 | # std_msgs # Or other packages containing msgs 72 | # ) 73 | 74 | ################################################ 75 | ## Declare ROS dynamic reconfigure parameters ## 76 | ################################################ 77 | 78 | ## To declare and build dynamic reconfigure parameters within this 79 | ## package, follow these steps: 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 82 | ## * In this file (CMakeLists.txt): 83 | ## * add "dynamic_reconfigure" to 84 | ## find_package(catkin REQUIRED COMPONENTS ...) 85 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 86 | ## and list every .cfg file to be processed 87 | 88 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 89 | # generate_dynamic_reconfigure_options( 90 | # cfg/DynReconf1.cfg 91 | # cfg/DynReconf2.cfg 92 | # ) 93 | 94 | ################################### 95 | ## catkin specific configuration ## 96 | ################################### 97 | ## The catkin_package macro generates cmake config files for your package 98 | ## Declare things to be passed to dependent projects 99 | ## INCLUDE_DIRS: uncomment this if your package contains header files 100 | ## LIBRARIES: libraries you create in this project that dependent projects also need 101 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 102 | ## DEPENDS: system dependencies of this project that dependent projects also need 103 | catkin_package( 104 | INCLUDE_DIRS include 105 | LIBRARIES denso_cobotta_lib 106 | CATKIN_DEPENDS roscpp 107 | # DEPENDS system_lib 108 | ) 109 | 110 | ########### 111 | ## Build ## 112 | ########### 113 | 114 | ## Specify additional locations of header files 115 | ## Your package locations should be listed before other locations 116 | include_directories( 117 | include 118 | ${catkin_INCLUDE_DIRS} 119 | ) 120 | 121 | ## Declare a C++ library 122 | add_library(${PROJECT_NAME} 123 | src/cobotta.cpp 124 | src/brake.cpp 125 | src/buttons.cpp 126 | src/cobotta_exception.cpp 127 | src/driver.cpp 128 | src/led.cpp 129 | src/message.cpp 130 | src/mini_io.cpp 131 | src/motor.cpp 132 | src/publish_info.cpp 133 | src/safety_mcu.cpp 134 | src/gripper.cpp 135 | # src/${PROJECT_NAME}/denso_cobotta_lib.cpp 136 | ) 137 | 138 | ## Add cmake target dependencies of the library 139 | ## as an example, code may need to be generated before libraries 140 | ## either from message generation or dynamic reconfigure 141 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 142 | 143 | ## Declare a C++ executable 144 | ## With catkin_make all packages are built within a single CMake context 145 | ## The recommended prefix ensures that target names across packages don't collide 146 | # add_executable(${PROJECT_NAME}_node src/denso_cobotta_lib_node.cpp) 147 | 148 | ## Rename C++ executable without prefix 149 | ## The above recommended prefix causes long target names, the following renames the 150 | ## target back to the shorter version for ease of user use 151 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 152 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 153 | 154 | ## Add cmake target dependencies of the executable 155 | ## same as for the library above 156 | #add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 157 | 158 | ## Specify libraries to link a library or executable target against 159 | # target_link_libraries(${PROJECT_NAME}_node 160 | # ${catkin_LIBRARIES} 161 | # ) 162 | 163 | ############# 164 | ## Install ## 165 | ############# 166 | 167 | # all install targets should use catkin DESTINATION variables 168 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 169 | 170 | ## Mark executable scripts (Python etc.) for installation 171 | ## in contrast to setup.py, you can choose the destination 172 | # install(PROGRAMS 173 | # scripts/my_python_script 174 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark executables and/or libraries for installation 178 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 179 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 180 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 181 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 182 | # ) 183 | 184 | ## Mark cpp header files for installation 185 | # install(DIRECTORY include/${PROJECT_NAME}/ 186 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 187 | # FILES_MATCHING PATTERN "*.h" 188 | # PATTERN ".svn" EXCLUDE 189 | # ) 190 | 191 | ## Mark other files for installation (e.g. launch and bag files, etc.) 192 | # install(FILES 193 | # # myfile1 194 | # # myfile2 195 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 196 | # ) 197 | 198 | ############# 199 | ## Testing ## 200 | ############# 201 | 202 | ## Add gtest based cpp test target and link libraries 203 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_denso_cobotta_lib.cpp) 204 | # if(TARGET ${PROJECT_NAME}-test) 205 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 206 | # endif() 207 | 208 | ## Add folders to be run by python nosetests 209 | # catkin_add_nosetests(test) 210 | -------------------------------------------------------------------------------- /denso_cobotta_control/src/denso_cobotta_hw.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | #include 19 | 20 | #include 21 | #include "denso_cobotta_control/denso_cobotta_hw.h" 22 | #include "denso_cobotta_lib/driver.h" 23 | 24 | namespace denso_cobotta_control 25 | { 26 | using namespace cobotta_common; 27 | using namespace denso_cobotta_lib::cobotta; 28 | 29 | DensoCobottaHW::DensoCobottaHW() 30 | { 31 | memset(cmd_, 0, sizeof(cmd_)); 32 | memset(pos_, 0, sizeof(pos_)); 33 | memset(vel_, 0, sizeof(vel_)); 34 | memset(eff_, 0, sizeof(eff_)); 35 | } 36 | 37 | DensoCobottaHW::~DensoCobottaHW() 38 | { 39 | } 40 | 41 | bool DensoCobottaHW::initialize(ros::NodeHandle& nh) 42 | { 43 | // Subscribers. (Start until motor_on) 44 | reset_ = false; 45 | sub_robot_state_ = nh.subscribe("robot_state", 64, &DensoCobottaHW::subRobotState, this); 46 | 47 | // Open cobotta device file 48 | errno = 0; 49 | fd_ = open(PATH_DEVFILE, O_RDWR); 50 | if (fd_ < 0) 51 | { 52 | ROS_ERROR("open(%s): %s", PATH_DEVFILE, std::strerror(errno)); 53 | return false; 54 | } 55 | 56 | // Load the CALSET data. 57 | loadCalsetData(); 58 | 59 | // Get the current encoder value and initialize joint positions. 60 | recvEncoderData(); 61 | 62 | // Register hardware interface. 63 | for (int i = 0; i < CONTROL_JOINT_MAX; i++) 64 | { 65 | std::stringstream ss; 66 | ss << "joint_" << i + 1; 67 | 68 | hardware_interface::JointStateHandle state_handle(ss.str(), &pos_[i], &vel_[i], &eff_[i]); 69 | jntStInterface_.registerHandle(state_handle); 70 | hardware_interface::JointHandle pos_handle(jntStInterface_.getHandle(ss.str()), &cmd_[i]); 71 | posJntInterface_.registerHandle(pos_handle); 72 | 73 | cmd_[i] = pos_[i]; 74 | } 75 | registerInterface(&posJntInterface_); 76 | registerInterface(&jntStInterface_); 77 | 78 | return true; 79 | } 80 | 81 | void DensoCobottaHW::checkMotorState(void) 82 | { 83 | // motor status 84 | try 85 | { 86 | int motor_state = Motor::readHw(fd_); 87 | ROS_INFO("motor_state is %d", motor_state); 88 | if (motor_state) 89 | this->motor_on_ = true; 90 | else 91 | this->motor_on_ = false; 92 | } 93 | catch (const CobottaException& e) 94 | { 95 | Message::putRosConsole(nullptr, e); 96 | } 97 | catch (const std::exception& e) 98 | { 99 | ROS_ERROR_STREAM(e.what()); 100 | } 101 | } 102 | 103 | bool DensoCobottaHW::read(ros::Time time, ros::Duration period) 104 | { 105 | bool success = recvEncoderData(); 106 | if (!success) 107 | { 108 | return false; 109 | } 110 | return true; 111 | } 112 | 113 | bool DensoCobottaHW::write(ros::Time time, ros::Duration period) 114 | { 115 | bool success = sendServoUpdateData(); 116 | if (!success) 117 | { 118 | return false; 119 | } 120 | return true; 121 | } 122 | 123 | /** 124 | * [ASYNC] Send to stay here 125 | */ 126 | void DensoCobottaHW::sendStayHere(int fd) 127 | { 128 | SRV_COMM_SEND send_data = { 0 }; 129 | send_data.arm_no = 0; 130 | send_data.discontinuous = 0; 131 | send_data.disable_cur_lim = 0; 132 | send_data.stay_here = 1; 133 | 134 | try 135 | { 136 | Driver::writeHwUpdate(fd, send_data); 137 | } 138 | catch (const std::exception& e) 139 | { 140 | //ROS_ERROR_STREAM(e.what()); 141 | } 142 | } 143 | 144 | bool DensoCobottaHW::loadCalsetData() 145 | { 146 | try 147 | { 148 | YAML::Node cobotta_params = YAML::LoadFile(cobotta_common::TEMP_PARAMS_PATH); 149 | pulse_offset_ = cobotta_params["pulse_offset"].as>(); 150 | if (pulse_offset_.size() == CONTROL_JOINT_MAX) 151 | { 152 | ROS_INFO("Success to apply the CALSET data."); 153 | return true; 154 | } 155 | ROS_WARN("The number of elements is invalid."); 156 | } 157 | catch (YAML::BadFile& e) 158 | { 159 | ROS_INFO("Cannot read %s.", cobotta_common::TEMP_PARAMS_PATH); 160 | } 161 | catch (std::exception& e) 162 | { 163 | ROS_WARN("Cannot parse 'pulse_offset' in %s.", cobotta_common::TEMP_PARAMS_PATH); 164 | } 165 | 166 | pulse_offset_.resize(CONTROL_JOINT_MAX); 167 | for (auto& i : pulse_offset_) 168 | { 169 | i = 0; 170 | } 171 | return false; 172 | } 173 | 174 | bool DensoCobottaHW::sendServoUpdateData() 175 | { 176 | SRV_COMM_SEND send_data; 177 | send_data.arm_no = 0; 178 | send_data.discontinuous = 0; 179 | send_data.disable_cur_lim = 0; 180 | send_data.stay_here = 0; 181 | 182 | for (int i = 0; i < CONTROL_JOINT_MAX; i++) 183 | { 184 | send_data.position[i] = ARM_COEFF_OUTPOS_TO_PULSE[i] * cmd_[i] - pulse_offset_[i]; 185 | send_data.current_limit[i] = 0x0FFF; 186 | send_data.current_offset[i] = 0; 187 | } 188 | 189 | try 190 | { 191 | struct DriverCommandInfo info = Driver::writeHwUpdate(fd_, send_data); 192 | // ROS_DEBUG("result:%08X queue:%d stay_here:%d", info.result, info.queue_num, info.stay_here); 193 | if (info.result == 0x0F408101) 194 | { 195 | // The current number of commands in buffer is 11. 196 | // To avoid buffer overflow, sleep short time 197 | ros::Duration(cobotta_common::COMMAND_SHORT_BREAK).sleep(); 198 | } 199 | else if (info.result == 0x84400502) 200 | { 201 | // buffer full 202 | // To avoid buffer overflow, sleep long time 203 | ROS_WARN("Command buffer full..."); 204 | ros::Duration(cobotta_common::COMMAND_LONG_BREAK).sleep(); 205 | } 206 | } 207 | catch (const std::exception& e) 208 | { 209 | ROS_ERROR_STREAM(e.what()); 210 | return false; 211 | } 212 | return true; 213 | } 214 | 215 | bool DensoCobottaHW::recvEncoderData() 216 | { 217 | SRV_COMM_RECV recv_data{ 0 }; 218 | 219 | try 220 | { 221 | recv_data = Driver::readHwEncoder(fd_, 0); 222 | } 223 | catch (const CobottaException& e) 224 | { 225 | Message::putRosConsole(nullptr, e); 226 | return false; 227 | } 228 | catch (const std::exception& e) 229 | { 230 | ROS_ERROR_STREAM(e.what()); 231 | return false; 232 | } 233 | 234 | // Update current position. 235 | for (int i = 0; i < CONTROL_JOINT_MAX; i++) 236 | { 237 | pos_[i] = (1 / ARM_COEFF_OUTPOS_TO_PULSE[i]) * (recv_data.encoder[i] + pulse_offset_[i]); 238 | } 239 | return true; 240 | } 241 | 242 | void DensoCobottaHW::subRobotState(const denso_cobotta_driver::RobotState& msg) 243 | { 244 | switch (msg.state_code) 245 | { 246 | case 0x0f200201: // motor on 247 | motor_on_ = true; 248 | reset_ = true; 249 | ROS_DEBUG("motor on"); 250 | break; 251 | case 0x0f200202: // motor off 252 | motor_on_ = false; 253 | ROS_DEBUG("motor off"); 254 | break; 255 | } 256 | ROS_DEBUG("msg received. ArmNo=%d code=0x%08X sub=0x%08X", msg.arm_no, msg.state_code, msg.state_subcode); 257 | } 258 | 259 | // @return ture:on false:off 260 | bool DensoCobottaHW::isMotorOn(void) 261 | { 262 | return motor_on_; 263 | } 264 | 265 | bool DensoCobottaHW::shouldReset(void) 266 | { 267 | if (this->reset_) 268 | { 269 | this->reset_ = false; 270 | return true; 271 | } 272 | return false; 273 | } 274 | 275 | int DensoCobottaHW::getFd() const 276 | { 277 | return fd_; 278 | } 279 | } // namespace denso_cobotta_control 280 | -------------------------------------------------------------------------------- /denso_cobotta_gripper/src/gripper_vacuum.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | #include 19 | #include "denso_cobotta_gripper/gripper_vacuum.h" 20 | #include "denso_cobotta_lib/driver.h" 21 | #include "denso_cobotta_gripper/VacuumMoveAction.h" 22 | 23 | namespace denso_cobotta_gripper 24 | { 25 | using namespace denso_cobotta_lib::cobotta; 26 | 27 | GripperVacuum::GripperVacuum() 28 | { 29 | ROS_INFO("GripperVacuum loading..."); 30 | gripper_type_ = GripperType::Vacuum; 31 | } 32 | 33 | bool GripperVacuum::initialize(ros::NodeHandle& nh) 34 | { 35 | bool success = GripperBase::initialize(nh); 36 | if (!success) 37 | { 38 | return false; 39 | } 40 | 41 | success = initActionServers(nh); 42 | if (!success) 43 | { 44 | return false; 45 | } 46 | 47 | success = GripperBase::initCurPos(); 48 | if (!success) 49 | { 50 | return false; 51 | } 52 | 53 | start_position_ = current_position_; 54 | current_target_position_ = current_position_; 55 | current_cmd_position_ = current_position_; 56 | current_cmd_velocity_ = 0.0; 57 | current_speed_percentage_ = 0.0; 58 | moving_ = false; 59 | 60 | return true; 61 | } 62 | 63 | bool GripperVacuum::read() 64 | { 65 | if (!GripperBase::recvEncoderData()) 66 | { 67 | return false; 68 | } 69 | 70 | return true; 71 | } 72 | 73 | bool GripperVacuum::write() 74 | { 75 | SRV_COMM_SEND send_data{ 0 }; 76 | send_data.arm_no = 1; 77 | send_data.discontinuous = 0; 78 | send_data.disable_cur_lim = 0; 79 | send_data.stay_here = 0; 80 | 81 | send_data.position[0] = current_cmd_position_ * coeff_outpos_to_pulse_; 82 | // For vacuum gripper, we fix the maximum current limit. 83 | send_data.current_limit[0] = 0x0FFF; 84 | send_data.current_offset[0] = 0; 85 | 86 | try 87 | { 88 | struct DriverCommandInfo info = Driver::writeHwUpdate(fd_, send_data); 89 | if (info.result == 0x0F408101) 90 | { 91 | // The current number of commands in buffer is 11. 92 | // To avoid buffer overflow, sleep 8 msec 93 | ros::Duration(cobotta_common::COMMAND_SHORT_BREAK).sleep(); 94 | } 95 | else if (info.result == 0x84400502) 96 | { 97 | // buffer full 98 | ROS_WARN("Command buffer overflow..."); 99 | ros::Duration(cobotta_common::COMMAND_LONG_BREAK).sleep(); 100 | } 101 | } 102 | catch (const std::exception& e) 103 | { 104 | ROS_ERROR_STREAM(e.what()); 105 | return false; 106 | } 107 | 108 | return true; 109 | } 110 | 111 | bool GripperVacuum::update() 112 | { 113 | GripperBase::calcGripperCommand(); 114 | 115 | return true; 116 | } 117 | 118 | bool GripperVacuum::initActionServers(ros::NodeHandle& nh) 119 | { 120 | as_vacuum_move_ = std::make_shared >( 121 | nh, "vacuum_move", 122 | std::bind(&GripperVacuum::vacuumMoveActionGoal, this, std::placeholders::_1), false); 123 | as_vacuum_move_->registerPreemptCallback(std::bind(&GripperVacuum::actionCancel, this)); 124 | as_vacuum_move_->start(); 125 | 126 | return true; 127 | } 128 | 129 | /** 130 | * @param target_position blow:minimum value suction:maximum value 131 | * @param speed_percentage speed_percentage is the same as power_percentage 132 | */ 133 | bool GripperVacuum::initGripperMove(const double& target_position, const double& speed_percentage) 134 | { 135 | // For vacuum gripper, we check only speed percentage. 136 | if ((speed_percentage < min_speed_percentage_) || (speed_percentage > max_speed_percentage_)) 137 | { 138 | ROS_ERROR("DensoCobottaGripper: speed is out of range(%lf). Min is %lf. Max is %lf.", speed_percentage, 139 | min_speed_percentage_, max_speed_percentage_); 140 | return false; 141 | } 142 | 143 | if (!updateVacuumDetectParameter(target_position > 0 ? speed_percentage : 100)) 144 | { 145 | return false; 146 | } 147 | // Set command parameters. 148 | { 149 | std::lock_guard gripper_lock(move_lock_); 150 | start_position_ = current_cmd_position_; 151 | current_speed_percentage_ = speed_percentage; 152 | current_effort_ = 0.0; 153 | current_target_position_ = target_position; 154 | moving_ = true; 155 | } 156 | 157 | return true; 158 | } 159 | 160 | bool GripperVacuum::updateVacuumDetectParameter(double speed_percentage) 161 | { 162 | try 163 | { 164 | const double DETECT_SLOPE = -0.1; 165 | const double DETECT_INTERCEPT = 1.8; 166 | const double DETECT_MAGNIFICATION = 10; 167 | std::array send_values = { 0 }; 168 | std::array recv_values = { 0 }; 169 | Driver::writeHwAcyclicCommAll(fd_, cobotta_common::VACUUM_DETECT_ADDRESS, send_values, recv_values); 170 | send_values = recv_values; 171 | send_values[send_values.size() - 1] = (uint16_t)(DETECT_MAGNIFICATION * (DETECT_SLOPE * speed_percentage + DETECT_INTERCEPT)); 172 | Driver::writeHwAcyclicCommAll(fd_, cobotta_common::VACUUM_DETECT_ADDRESS | cobotta_common::ACYCLIC_WRITE_MASK, 173 | send_values, recv_values); 174 | } 175 | catch (const CobottaException& e) 176 | { 177 | Message::putRosConsole(TAG, e); 178 | return false; 179 | } 180 | catch (const std::exception& e) 181 | { 182 | ROS_ERROR_STREAM(e.what()); 183 | return false; 184 | } 185 | return true; 186 | } 187 | 188 | bool GripperVacuum::vacuumMoveActionGoal(const denso_cobotta_gripper::VacuumMoveGoalConstPtr& goal) 189 | { 190 | ROS_DEBUG("vacuumMoveActionGoal: direction=%d power_percentage=%lf", 191 | goal->direction, goal->power_percentage); 192 | 193 | if (!GripperBase::isMotorState()) 194 | { 195 | Message::putRosConsole(TAG, 0x81400014); 196 | as_vacuum_move_->setAborted(); 197 | return false; 198 | } 199 | 200 | VacuumMoveResult result; 201 | VacuumMoveDirection direction = static_cast(goal->direction); 202 | 203 | // When the power percentage is 0, request stop. 204 | if (std::abs(goal->power_percentage) <= std::numeric_limits::epsilon()) 205 | { 206 | direction = VacuumMoveDirection::Stop; 207 | } 208 | 209 | double target_position; 210 | if (direction == VacuumMoveDirection::Blow) 211 | { 212 | target_position = std::numeric_limits::lowest(); 213 | } 214 | else if (direction == VacuumMoveDirection::Suction) 215 | { 216 | target_position = std::numeric_limits::max(); 217 | } 218 | else if (direction == VacuumMoveDirection::Stop) 219 | { 220 | GripperBase::stopMove(); 221 | result.success = true; 222 | as_vacuum_move_->setSucceeded(result); 223 | return true; 224 | } 225 | else 226 | { 227 | ROS_ERROR("Vacuum goal direction is invalid: %d", goal->direction); 228 | as_vacuum_move_->setAborted(); 229 | return false; 230 | } 231 | 232 | bool success = initGripperMove(target_position, goal->power_percentage); 233 | if (!success) 234 | { 235 | as_vacuum_move_->setAborted(); 236 | return false; 237 | } 238 | 239 | ros::Rate rate(1.0 / cobotta_common::SERVO_PERIOD); 240 | while (moving_) 241 | { 242 | // Wait until move is complete. 243 | this->actionFeedback(); 244 | rate.sleep(); 245 | } 246 | this->actionFeedback(); 247 | 248 | if (as_vacuum_move_->isPreemptRequested()) 249 | { 250 | result.success = false; 251 | as_vacuum_move_->setPreempted(result); 252 | return false; 253 | } 254 | else if (as_vacuum_move_->isActive()) 255 | { 256 | result.success = true; 257 | as_vacuum_move_->setSucceeded(result); 258 | } 259 | else 260 | { 261 | as_vacuum_move_->setAborted(); 262 | return false; 263 | } 264 | return true; 265 | } 266 | 267 | void GripperVacuum::actionCancel() 268 | { 269 | GripperBase::stopMove(); 270 | return; 271 | } 272 | 273 | /* 274 | * The pressure of feedback is not implemented. 275 | * It needs Linux driver fix. 276 | */ 277 | void GripperVacuum::actionFeedback() 278 | { 279 | VacuumMoveFeedback feedback; 280 | feedback.pressure = 0; // Not implemented. 281 | feedback.stalled = this->isGraspState(); 282 | as_vacuum_move_->publishFeedback(feedback); 283 | 284 | return; 285 | } 286 | 287 | bool GripperVacuum::publish() 288 | { 289 | return true; 290 | } 291 | 292 | bool GripperVacuum::subscribe() 293 | { 294 | return true; 295 | } 296 | 297 | } /* namespace denso_cobotta_gripper */ 298 | 299 | -------------------------------------------------------------------------------- /denso_cobotta_lib/src/motor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2019 DENSO WAVE INCORPORATED 3 | * 4 | * This program is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU General Public License 6 | * as published by the Free Software Foundation; either version 2 7 | * of the License, or (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | #include "denso_cobotta_lib/cobotta_common.h" 28 | #include "denso_cobotta_lib/motor.h" 29 | #include "denso_cobotta_lib/cobotta_ioctl.h" 30 | 31 | namespace denso_cobotta_lib 32 | { 33 | namespace cobotta 34 | { 35 | /** 36 | * Constructs a Motor object. 37 | * @param parent cobotta object 38 | */ 39 | Motor::Motor(std::shared_ptr parent) 40 | { 41 | this->parent_ = parent; 42 | } 43 | 44 | /** 45 | * Constructs a Motor object. 46 | * @param parent cobotta object 47 | * @param state motor state 48 | */ 49 | Motor::Motor(std::shared_ptr parent, const enum MotorState state) 50 | { 51 | this->parent_ = parent; 52 | this->state_ = state; 53 | } 54 | 55 | /** 56 | * Update my state. 57 | * @param state motor state 58 | * @return true:changed false:no change 59 | */ 60 | bool Motor::update(const enum MotorState state) 61 | { 62 | if (this->getState() == state) 63 | return false; 64 | 65 | this->state_ = state; 66 | } 67 | 68 | /** 69 | * @return true: Motor should STOP at STO or errors 70 | */ 71 | bool Motor::shouldStop() 72 | { 73 | if ((this->getState() == MotorState::SlowDownStop) 74 | || (this->getState() == MotorState::Off)) 75 | return false; 76 | 77 | /* SafetyMCU state */ 78 | if (this->getParent()->getSafetyMcu()->isSafeState()) 79 | return true; 80 | /* fatal error */ 81 | if (this->getParent()->getDriver()->isFatalError()) 82 | return true; 83 | /* XXX: Lv4 error */ 84 | 85 | return false; 86 | } 87 | 88 | /** 89 | * @return true:On false:Off/OnProc/SlowDownStop 90 | */ 91 | bool Motor::isRunning(void) const 92 | { 93 | if (this->getState() == MotorState::On) 94 | return true; 95 | 96 | return false; 97 | } 98 | 99 | /** 100 | * @return true:yes false:no 101 | */ 102 | bool Motor::canStart(void) const 103 | { 104 | if (this->getParent()->getSafetyMcu()->isFatalError() 105 | || this->getParent()->getDriver()->isFatalError()) 106 | { 107 | // Fatal Error 108 | Message::putRosConsole(TAG, 0x85400001); 109 | return false; 110 | } 111 | /* emergency button */ 112 | if (this->getParent()->getSafetyMcu()->isEmergencyButton()) 113 | { 114 | // Turn OFF Emergency-stop and execute the command clear_safe_state. 115 | Message::putRosConsole(TAG, 0x81400016); 116 | return false; 117 | } 118 | /* protective button */ 119 | if (this->getParent()->getSafetyMcu()->isProtectiveButton()) 120 | { 121 | // Turn OFF Protective-stop signal to execute the command. 122 | Message::putRosConsole(TAG, 0x81400019); 123 | return false; 124 | } 125 | /* SafetyMCU state */ 126 | if (this->getParent()->getSafetyMcu()->isSafeState()) 127 | { 128 | // You cannot execute a command while motion preparation has not been performed. 129 | Message::putRosConsole(TAG, 0x81501070); 130 | return false; 131 | } 132 | /* other errors */ 133 | if (this->getParent()->getDriver()->isError()) 134 | { 135 | // You cannot execute a command while an error occurs. 136 | Message::putRosConsole(TAG, 0x81400015); 137 | return false; 138 | } 139 | /* brake */ 140 | if (!this->getParent()->getBrake()->isLocked()) 141 | { 142 | // Lock the brake to execute the command. 143 | Message::putRosConsole(TAG, 0x8140001B); 144 | return false; 145 | } 146 | 147 | return true; 148 | } 149 | 150 | /** 151 | * [sync] Motor on 152 | * @exception CobottaException An error defined by COBOTTA 153 | * @exception RuntimeError An other error 154 | */ 155 | void Motor::start(void) throw(CobottaException, std::runtime_error) 156 | { 157 | /* check */ 158 | if (this->getState() != MotorState::Off) 159 | { 160 | return; 161 | } 162 | if (!this->canStart()) 163 | { 164 | // It can not execute in the current state. 165 | throw CobottaException(0x0F20FFFF); 166 | } 167 | 168 | /* start */ 169 | ROS_INFO("%s: Starting...", TAG); 170 | this->writeHwOn(this->getParent()->getFd()); 171 | /* sync */ 172 | while (this->getState() != MotorState::On) 173 | { 174 | if (this->getParent()->getDriver()->isFatalError() 175 | || this->getParent()->getDriver()->isError()) 176 | throw CobottaException(0x83201F83); /* Operation failed */ 177 | if (this->getParent()->getSafetyMcu()->isSafeState()) 178 | throw CobottaException(0x83201F83); /* Operation failed */ 179 | 180 | ros::Duration(cobotta_common::getPeriod()).sleep(); 181 | } 182 | ROS_INFO("%s: ... Motor has started.", TAG); 183 | } 184 | 185 | /** 186 | * [sync] Motor off 187 | * @exception CobottaException An error defined by COBOTTA 188 | * @exception RuntimeError An other error 189 | */ 190 | void Motor::stop(void) throw(CobottaException, std::runtime_error) 191 | { 192 | /* 193 | * check 194 | * except for MotorState::MOTOR_OFF to stop() brake::lock on error. 195 | */ 196 | if (this->getState() == MotorState::Off 197 | || this->getState() == MotorState::SlowDownStop) 198 | { 199 | return; 200 | } 201 | /* off */ 202 | ROS_INFO("%s: Stopping...", TAG); 203 | this->writeHwOff(this->getParent()->getFd()); 204 | /* sync */ 205 | while (this->getState() == MotorState::Off) 206 | { 207 | ros::Duration(cobotta_common::getPeriod()).sleep(); 208 | } 209 | ROS_INFO("%s: ... Motor has stopped.", TAG); 210 | } 211 | 212 | /** 213 | * [ASYNC] Motor off 214 | * @exception CobottaException An error defined by COBOTTA 215 | * @exception RuntimeError An other error 216 | */ 217 | void Motor::sendStop(int fd) throw(CobottaException, std::runtime_error) 218 | { 219 | ROS_INFO("%s: Stop anyway.", TAG); 220 | Motor::writeHwOff(fd); 221 | } 222 | 223 | /** 224 | * IOCTL_MOTOR_ON 225 | * @param fd File descriptor 226 | * @exception CobottaException An error defined by COBOTTA 227 | * @exception RuntimeError An other error 228 | */ 229 | void Motor::writeHwOn(int fd) throw(CobottaException, std::runtime_error) 230 | { 231 | int ret; 232 | IOCTL_DATA_RESULT dat; 233 | int myerrno; 234 | 235 | memset(&dat, 0, sizeof(dat)); 236 | errno = 0; 237 | ret = ioctl(fd, COBOTTA_IOCTL_MOTOR_ON, &dat); 238 | myerrno = errno; 239 | ROS_DEBUG("%s: COBOTTA_IOCTL_MOTOR_ON ret=%d errno=%d result=%lX", 240 | TAG, ret, myerrno, dat.result); 241 | if (ret) { 242 | ROS_ERROR("%s: ioctl(COBOTTA_IOCTL_MOTOR_ON): %s", TAG, std::strerror(myerrno)); 243 | if (myerrno == ECANCELED) { 244 | throw CobottaException(dat.result); 245 | } 246 | throw std::runtime_error(std::strerror(myerrno)); 247 | } 248 | } 249 | 250 | /** 251 | * IOCTL_MOTOR_OFF 252 | * @param fd File descriptor 253 | * @exception CobottaException An error defined by COBOTTA 254 | * @exception RuntimeError An other error 255 | */ 256 | void Motor::writeHwOff(int fd) throw(CobottaException, std::runtime_error) 257 | { 258 | int ret; 259 | IOCTL_DATA_RESULT dat; 260 | int myerrno; 261 | 262 | memset(&dat, 0, sizeof(dat)); 263 | errno = 0; 264 | ret = ioctl(fd, COBOTTA_IOCTL_MOTOR_OFF, &dat); 265 | myerrno = errno; 266 | ROS_DEBUG("%s: COBOTTA_IOCTL_MOTOR_OFF ret=%d errno=%d result=%lX", 267 | TAG, ret, myerrno, dat.result); 268 | if (ret) { 269 | ROS_ERROR("%s: ioctl(COBOTTA_IOCTL_MOTOR_OFF): %s", TAG, std::strerror(myerrno)); 270 | if (myerrno == ECANCELED) { 271 | throw CobottaException(dat.result); 272 | } 273 | throw std::runtime_error(std::strerror(myerrno)); 274 | } 275 | } 276 | 277 | /** 278 | * COBOTTA_IOCTL_MOTOR_STATE 279 | * @param fd File descriptor 280 | * @return 1:running 0:stop(on_proc&slowdownstop) 281 | * @exception CobottaException An error defined by COBOTTA 282 | * @exception RuntimeError An other error 283 | */ 284 | int Motor::readHw(int fd) throw(CobottaException, std::runtime_error) 285 | { 286 | int ret; 287 | IOCTL_DATA_MOTOR_STATE dat; 288 | int myerrno; 289 | 290 | memset(&dat, 0, sizeof(dat)); 291 | errno = 0; 292 | ret = ioctl(fd, COBOTTA_IOCTL_MOTOR_STATE, &dat); 293 | myerrno = errno; 294 | ROS_DEBUG("%s: COBOTTA_IOCTL_MOTOR_STATE ret=%d errno=%d result=%lX", 295 | TAG, ret, myerrno, dat.result); 296 | if (ret) { 297 | ROS_ERROR("%s: ioctl(COBOTTA_IOCTL_MOTOR_STATE): %s", TAG, std::strerror(myerrno)); 298 | if (myerrno == ECANCELED) { 299 | throw CobottaException(dat.result); 300 | } 301 | throw std::runtime_error(std::strerror(myerrno)); 302 | } 303 | return dat.state; 304 | } 305 | 306 | enum MotorState Motor::getState() const 307 | { 308 | return state_; 309 | } 310 | 311 | std::shared_ptr Motor::getParent() const 312 | { 313 | return parent_; 314 | } 315 | } /* namespace cobotta */ 316 | } /* namespace denso_cobotta_lib */ 317 | --------------------------------------------------------------------------------