├── .catkin_workspace ├── .gitignore ├── README.md └── src ├── CMakeLists.txt ├── assembly_line ├── CMakeLists.txt ├── package.xml └── scripts │ ├── assembly_line_node.py │ ├── driver.py │ ├── driver_ser.py │ └── test_ir_serial.py ├── aubo_ctrl ├── CMakeLists.txt ├── README.md ├── assets │ ├── calibration_ex_params_mi.yml │ └── calibration_in_params_mi.yml ├── cmake │ └── FindAuBo.cmake ├── package.xml ├── scripts │ ├── aubo │ │ ├── __init__.py │ │ ├── libconfig++.a │ │ ├── libconfig++.la │ │ ├── libconfig++.so │ │ ├── libconfig++.so.11 │ │ ├── libconfig++.so.11.0.2 │ │ ├── libconfig.a │ │ ├── libconfig.la │ │ ├── libconfig.so │ │ ├── libconfig.so.11 │ │ ├── libconfig.so.11.0.2 │ │ ├── libconfig.so.9 │ │ ├── libconfig.so.9.2.0 │ │ ├── liblog4cplus-1.2.so.5 │ │ ├── libprotobuf.so.9 │ │ ├── libpyauboi5.so │ │ ├── libpyauboi5.so.1 │ │ ├── libpyauboi5.so.1.2 │ │ ├── libpyauboi5.so.1.2.2 │ │ ├── logfiles │ │ │ └── robot-ctl-python.log │ │ └── robotcontrol.py │ └── aubo_state_node.py └── src │ ├── aubo │ ├── BlockingQueue.h │ ├── Robot.cpp │ ├── Robot.h │ ├── util.cpp │ └── util.h │ ├── audo_ctrl_node.cpp │ ├── test_blanding.cpp │ ├── test_dh_gripper.cpp │ ├── test_feeding.cpp │ └── utils │ ├── CMakeLists.txt │ ├── Rotation3DUtils.cpp │ └── Rotation3DUtils.h ├── dh_gripper_ros ├── CMakeLists.txt ├── README.md ├── action │ └── ActuateHand.action ├── doc │ ├── 寄存器列表 - 用户.xlsx │ └── 寄存器列表 -三指.xlsx ├── include │ └── dh_hand_driver │ │ ├── ActuateHandAction.h │ │ ├── ActuateHandActionFeedback.h │ │ ├── ActuateHandActionGoal.h │ │ ├── ActuateHandActionResult.h │ │ ├── ActuateHandFeedback.h │ │ ├── ActuateHandGoal.h │ │ ├── ActuateHandResult.h │ │ ├── DH_datastream.cpp │ │ ├── DH_datastream.h │ │ ├── definition.h │ │ ├── hand_controller.cpp │ │ ├── hand_controller.h │ │ ├── hand_driver.cpp │ │ ├── hand_driver.h │ │ ├── hand_state.h │ │ ├── hand_stateRequest.h │ │ └── hand_stateResponse.h ├── launch │ └── dh_hand_controller.launch ├── package.xml ├── scripts │ ├── DH_hand.rules │ ├── create_udev_rules.sh │ └── delete_udev_rules.sh ├── src │ ├── node.cpp │ └── test_client.cpp └── srv │ └── hand_state.srv ├── iai_kinect2 ├── LICENSE ├── README.md ├── iai_kinect2 │ ├── CMakeLists.txt │ └── package.xml ├── kinect2_bridge │ ├── CMakeLists.txt │ ├── README.md │ ├── data │ │ ├── 040942235247 │ │ │ ├── calib_color.yaml │ │ │ ├── calib_depth.yaml │ │ │ ├── calib_ir.yaml │ │ │ └── calib_pose.yaml │ │ ├── 196605135147 │ │ │ ├── calib_color.yaml │ │ │ ├── calib_depth.yaml │ │ │ ├── calib_ir.yaml │ │ │ └── calib_pose.yaml │ │ └── 299150235147 │ │ │ ├── calib_color.yaml │ │ │ ├── calib_ir.yaml │ │ │ └── calib_pose.yaml │ ├── include │ │ └── kinect2_bridge │ │ │ └── kinect2_definitions.h │ ├── launch │ │ └── kinect2_bridge.launch │ ├── nodelet_plugins.xml │ ├── package.xml │ └── src │ │ └── kinect2_bridge.cpp ├── kinect2_calibration │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── kinect2_calibration │ │ │ └── kinect2_calibration_definitions.h │ ├── package.xml │ ├── patterns │ │ ├── chess5x7x0.03.pdf │ │ ├── chess5x7x0.03.svg │ │ ├── chess7x9x0.025.pdf │ │ ├── chess7x9x0.025.svg │ │ ├── chess9x11x0.02.pdf │ │ └── chess9x11x0.02.svg │ ├── scripts │ │ └── convert_calib_pose_to_urdf_format.py │ └── src │ │ └── kinect2_calibration.cpp ├── kinect2_registration │ ├── CMakeLists.txt │ ├── README.md │ ├── cmake │ │ ├── CheckOpenCLICDLoader.cmake │ │ ├── FindOpenCL.cmake │ │ └── kinect2_registration.cmake.in │ ├── include │ │ ├── internal │ │ │ └── CL │ │ │ │ └── cl.hpp │ │ └── kinect2_registration │ │ │ ├── kinect2_console.h │ │ │ └── kinect2_registration.h │ ├── package.xml │ └── src │ │ ├── depth_registration.cl │ │ ├── depth_registration_cpu.cpp │ │ ├── depth_registration_cpu.h │ │ ├── depth_registration_opencl.cpp │ │ ├── depth_registration_opencl.h │ │ └── kinect2_registration.cpp └── kinect2_viewer │ ├── CMakeLists.txt │ ├── README.md │ ├── package.xml │ └── src │ └── viewer.cpp ├── itheima_msgs ├── CMakeLists.txt ├── action │ ├── ArmWork.action │ ├── CarMove.action │ └── LaserMark.action ├── msg │ ├── AssemblyIR.msg │ ├── AssemblyLine.msg │ ├── BoxPose.msg │ └── Order.msg ├── package.xml └── srv │ ├── AssemblyLineCtrl.srv │ ├── GetBoxPoses.srv │ └── GetLaserBoxLocator.srv ├── joystick_drivers ├── .gitignore ├── README.md ├── joy │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── config │ │ └── ps4joy.yaml │ ├── launch │ │ └── ps4joy.launch │ ├── migration_rules │ │ └── Joy.bmr │ ├── package.xml │ ├── scripts │ │ └── joy_remap.py │ ├── src │ │ └── joy_node.cpp │ └── test │ │ ├── saved │ │ └── Joy.saved │ │ └── test_joy_msg_migration.py ├── ps3joy │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── diagnostics.yaml │ ├── doc │ │ ├── bluetooth_devices.md │ │ ├── testing.md │ │ └── troubleshooting.md │ ├── launch │ │ └── ps3.launch │ ├── package.xml │ ├── scripts │ │ ├── ps3joy.py │ │ ├── ps3joy_node.py │ │ └── ps3joysim.py │ └── src │ │ └── sixpair.c ├── spacenav_node │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── launch │ │ ├── classic.launch │ │ ├── no_deadband.launch │ │ └── static_deadband.launch │ ├── package.xml │ └── src │ │ └── spacenav_node.cpp └── wiimote │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── doc │ ├── testing.md │ └── tutorials │ │ └── teleop.md │ ├── include │ └── wiimote │ │ ├── stat_vector_3d.h │ │ ├── teleop_wiimote.h │ │ └── wiimote_controller.h │ ├── launch │ ├── feedback_test_cpp.launch │ ├── feedback_test_py.launch │ ├── turtlesim_cpp.launch │ └── turtlesim_py.launch │ ├── msg │ ├── IrSourceInfo.msg │ ├── State.msg │ └── TimedSwitch.msg │ ├── nodes │ ├── feedbackTester.py │ └── wiimote_node.py │ ├── package.xml │ ├── scripts │ └── wiimote_test.py │ ├── setup.py │ └── src │ ├── gpl-2.0.txt │ ├── stat_vector_3d.cpp │ ├── teleop_wiimote.cpp │ ├── wiimote │ ├── WIIMote.py │ ├── __init__.py │ ├── wiimoteConstants.py │ ├── wiimoteExceptions.py │ ├── wiimoteTests.py │ ├── wiistate.py │ └── wiiutils.py │ └── wiimote_controller.cpp ├── lab_bringup ├── CMakeLists.txt ├── launch │ ├── startup.launch │ ├── test_env_assembly.launch │ ├── test_env_aubo_ctrl.launch │ ├── test_env_laser_mark.launch │ ├── test_env_slam_move.launch │ ├── test_run_aubo_blanding.launch │ ├── test_run_aubo_feeding.launch │ ├── test_run_laser_mark.launch │ └── test_run_slam_move.launch └── package.xml ├── lab_vision ├── CMakeLists.txt ├── config │ ├── env │ │ ├── locator_offset.json │ │ ├── mask-BoxLaserLocator.json │ │ └── mask-LaserRect.json │ └── env1 │ │ ├── locator_offset.json │ │ ├── mask-BoxLaserLocator.json │ │ ├── mask-LaserRect.json │ │ ├── mask-agv.json │ │ └── mask-line.json ├── data │ └── usb_camera.yml ├── package.xml └── scripts │ ├── __init__.py │ ├── box_detector_node.py │ ├── common │ ├── __init__.py │ ├── abs_detector.py │ ├── curve_tools.py │ ├── geometry_util.py │ └── global_ctl.py │ ├── detector │ ├── __init__.py │ ├── agv_detector.py │ ├── assembly_line_detector.py │ ├── box_detector.py │ └── detector_main.py │ ├── imgs │ ├── laser_0.jpg │ ├── laser_1.jpg │ ├── laser_2.jpg │ ├── laser_rect1.jpg │ ├── laser_rect2.png │ ├── rect2.webm │ └── test_aubo2.png │ ├── laser_locator_node.py │ ├── laser_test.py │ ├── locator │ ├── __init__.py │ ├── box_locator.py │ ├── laser_locator.py │ ├── laser_on_box_locator.py │ └── locator_main.py │ ├── main_test.py │ └── test │ ├── __init__.py │ ├── shrink_curve.py │ ├── test.py │ ├── test_camera.py │ └── test_transform.py ├── laser_mark ├── CMakeLists.txt ├── package.xml └── scripts │ ├── driver.py │ ├── laser_mark_node.py │ └── test_laser_mark.py ├── order_manager ├── CMakeLists.txt ├── package.xml └── scripts │ ├── order_manager_node.py │ ├── order_tip1.ogg │ ├── order_tip2.ogg │ └── order_tip_node.py ├── slam ├── CMakeLists.txt ├── heima_msgs │ ├── CMakeLists.txt │ ├── msg │ │ ├── Battery.msg │ │ ├── DHT22.msg │ │ ├── Imu.msg │ │ ├── Infrared.msg │ │ ├── PID.msg │ │ ├── Servo.msg │ │ ├── Ultrasonic.msg │ │ └── Velocities.msg │ └── package.xml ├── heima_serial │ ├── CMakeLists.txt │ ├── include │ │ ├── BlockingQueue.h │ │ ├── Kinematics.h │ │ ├── heima_protocol.h │ │ ├── heima_serial_node.h │ │ └── wheeltec_robot.h │ ├── launch │ │ └── serial_bridge.launch │ ├── package.xml │ └── src │ │ ├── Kinematics.cpp │ │ ├── heima_serial_node.cpp │ │ └── wheeltec_robot.cpp ├── heimarobot │ ├── CMakeLists.txt │ ├── include │ │ └── heima_base.h │ ├── launch │ │ ├── amcl.launch │ │ ├── bringup.launch │ │ ├── lidar │ │ │ ├── deltalidar.launch │ │ │ ├── rplidar.launch │ │ │ └── ydlidar.launch │ │ ├── lidar_slam.launch │ │ ├── lidar_slam_rviz.launch │ │ ├── navigate.launch │ │ └── navigate_rviz.launch │ ├── maps │ │ ├── house.pgm │ │ ├── house.yaml │ │ └── map.sh │ ├── package.xml │ ├── param │ │ ├── box_filter.yaml │ │ ├── ekf │ │ │ └── robot_localization.yaml │ │ ├── imu │ │ │ └── imu_calib.yaml │ │ └── navigation │ │ │ ├── 2wd │ │ │ ├── base_local_planner_params.yaml │ │ │ └── costmap_common_params.yaml │ │ │ ├── global_costmap_params.yaml │ │ │ ├── local_costmap_params.yaml │ │ │ ├── move_base.xml │ │ │ ├── move_base_params.yaml │ │ │ └── slam_gmapping.xml │ ├── rviz │ │ ├── navigate.rviz │ │ └── slam.rviz │ ├── script │ │ ├── LineFollower.py │ │ ├── common │ │ │ ├── __init__.py │ │ │ └── transform_tools.py │ │ ├── exploring.py │ │ ├── move_path.py │ │ ├── pose_estimation_demo.py │ │ ├── pose_estimator.py │ │ ├── test.jpg │ │ └── usb_camera.yml │ └── src │ │ ├── heima_base.cpp │ │ └── heima_base_node.cpp ├── heimarobot_nav │ ├── .gitignore │ ├── CMakeLists.txt │ ├── cfg │ │ ├── CalibrateAngular.cfg │ │ ├── CalibrateLinear.cfg │ │ └── TestPath.cfg │ ├── nodes │ │ ├── calibrate_angular.py │ │ ├── calibrate_linear.py │ │ ├── move_base_square.py │ │ ├── move_path.py │ │ ├── odom_out_and_back.py │ │ └── timed_out_and_back.py │ ├── package.xml │ ├── setup.py │ └── src │ │ ├── __init__.py │ │ └── heimarobot_nav │ │ ├── __init__.py │ │ ├── cfg │ │ └── __init__.py │ │ └── transform_utils.py └── imu_calib │ ├── .gitignore │ ├── CMakeLists.txt │ ├── LICENSE │ ├── README.md │ ├── include │ └── imu_calib │ │ ├── accel_calib.h │ │ ├── apply_calib.h │ │ └── do_calib.h │ ├── package.xml │ └── src │ ├── accel_calib │ └── accel_calib.cpp │ ├── apply_calib.cpp │ ├── apply_calib_node.cpp │ ├── do_calib.cpp │ └── do_calib_node.cpp ├── slam_ctrl ├── CMakeLists.txt ├── package.xml └── scripts │ ├── LineFollower.py │ ├── common │ ├── __init__.py │ └── transform_tools.py │ ├── exploring.py │ ├── move_path.py │ ├── pose_estimation_demo.py │ ├── pose_estimator.py │ ├── test.jpg │ └── usb_camera.yml ├── ui_sync ├── CMakeLists.txt ├── package.xml └── scripts │ ├── Connector.py │ └── ui_sync_node.py └── usb_cam ├── .travis.yml ├── AUTHORS.md ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── data └── usb_camera.yml ├── include └── usb_cam │ └── usb_cam.h ├── launch ├── usb_cam-test.launch ├── uvc_cam-test.launch └── uvc_client.launch ├── nodes └── usb_cam_node.cpp ├── package.xml └── src ├── LICENSE ├── usb_cam.cpp ├── usb_reset └── usb_reset.c /.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Created by .ignore support plugin (hsz.mobi) 2 | ### ROS template 3 | devel/ 4 | logs/ 5 | build/ 6 | bin/ 7 | lib/ 8 | msg_gen/ 9 | srv_gen/ 10 | msg/*Action.msg 11 | msg/*ActionFeedback.msg 12 | msg/*ActionGoal.msg 13 | msg/*ActionResult.msg 14 | msg/*Feedback.msg 15 | msg/*Goal.msg 16 | msg/*Result.msg 17 | msg/_*.py 18 | build_isolated/ 19 | devel_isolated/ 20 | 21 | # Generated by dynamic reconfigure 22 | *.cfgc 23 | /cfg/cpp/ 24 | /cfg/*.py 25 | 26 | # Ignore generated docs 27 | *.dox 28 | *.wikidoc 29 | 30 | # eclipse stuff 31 | .project 32 | .cproject 33 | 34 | # qcreator stuff 35 | CMakeLists.txt.user 36 | 37 | srv/_*.py 38 | *.pcd 39 | *.pyc 40 | qtcreator-* 41 | *.user 42 | 43 | /planning/cfg 44 | /planning/docs 45 | /planning/src 46 | 47 | *~ 48 | 49 | # Emacs 50 | .#* 51 | 52 | # Catkin custom files 53 | CATKIN_IGNORE 54 | 55 | cmake-build-debug/ 56 | .idea/ 57 | 58 | src/aubo_ctrl/3rdparty/aubo_dependents 59 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Mini工业互联网产线 2 | 3 | 4 | ## 模块介绍 5 | 目前v1.0版包含以下包及模块 6 | 7 | **ROS相关** 8 | 9 | - 公共Msg/Srv/Action（[itheima_msgs](src/itheima_msgs)） 10 | - 产品装配流水线（[assembly_line](src/assembly_line)） 11 | - 红外信号信息获取 12 | - 传送带启停控制 13 | - 相机驱动模块 14 | - USB相机驱动 15 | - Astra奥比中光相机驱动 16 | - Kinect2相机驱动 17 | - 机械臂控制（[aubo_ctrl](src/aubo_ctrl)） 18 | - Aubo机械臂控制 19 | - DH大环夹爪控制 20 | - 因时夹爪控制（计划） 21 | - 吸盘控制（计划） 22 | - 视觉同步（[ui_sync](src/ui_sync)） 23 | - 统一启动包（[lab_bringup](src/lab_bringup)） 24 | - 无人小车模块 25 | - 激光打标机模块 26 | 27 | **Unity相关** 28 | 29 | 实现装配线数字孪生，用于实时同步展示流水线各个环节状态 30 | 31 | unity_mini_industry 32 | 33 | **前端显示** 34 | 35 | 实现直观的前端数据可视化 36 | 37 | ui_mini_industry 38 | 39 | 40 | ## 环境准备Requirements 41 | 42 | - 安装USB相机依赖 43 | ```bash 44 | sudo apt install ros-$ROS_DISTRO-libuvc-camera 45 | ``` 46 | 47 | - 根据ROS项目需要自动安装ros依赖包 48 | ```bash 49 | cd ros_mini_industry/src 50 | rosdep install -y --from-paths . --ignore-src --rosdistro $ROS_DISTRO 51 | ``` 52 | 53 | ## 如何使用Usage 54 | 55 | 先在项目根目录执行一次编译及环境初始化 56 | ```bash 57 | cd ros_mini_industry/src 58 | catkin_make 59 | source devel/setup.bash 60 | ``` 61 | 62 | ### 完整流程 63 | ```bash 64 | roslaunch lab_bringup startup.launch 65 | ``` 66 | 67 | ### 独立单元流程 68 | - **流水线** 69 | ```bash 70 | roslaunch lab_bringup test_env_assembly.launch 71 | ``` 72 | 73 | - **机械臂上下料** 74 | 75 | 在独立的terminal启动一个服务端长期运行 76 | ```bash 77 | roslaunch lab_bringup test_env_aubo_ctrl.launch 78 | ``` 79 | 80 | 在新的terminal可多次执行以下上下料测试 81 | ```bash 82 | # 上料 83 | roslaunch lab_bringup test_run_aubo_feeding.launch 84 | # 下料 85 | roslaunch lab_bringup test_run_aubo_blanding.launch 86 | ``` 87 | 88 | - **激光打标** 89 | 90 | 在独立的terminal启动一个服务端长期运行 91 | ```bash 92 | roslaunch lab_bringup test_env_laser_mark.launch 93 | ``` 94 | 95 | 在新的terminal可多次执行以下打标测试 96 | ```bash 97 | roslaunch lab_bringup test_run_laser_mark.launch 98 | ``` 99 | 100 | - **夹爪开合** 101 | 102 | 开启服务 103 | 104 | ```bash 105 | roslaunch dh_hand_driver dh_hand_controller.launch 106 | ``` 107 | 108 | 测试开合 109 | 110 | ```bash 111 | rosrun dh_hand_driver hand_controller_client 1 50 80 112 | ``` 113 | 114 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/melodic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /src/assembly_line/scripts/test_ir_serial.py: -------------------------------------------------------------------------------- 1 | import serial 2 | import threading 3 | import time 4 | 5 | 6 | def do_recv(): 7 | while True: 8 | buffer = ser.read(6) 9 | buffer = bytearray(buffer) 10 | 11 | ir_1 = buffer[3] & 0x04 != 0 12 | ir_2 = buffer[3] & 0x01 != 0 13 | 14 | print "{} {}".format(ir_1, ir_2) 15 | 16 | 17 | if __name__ == '__main__': 18 | ser = serial.Serial(port="/dev/ttyUSB1", baudrate=9600) 19 | 20 | threading.Thread(target=do_recv).start() 21 | 22 | while True: 23 | ser.write(b'\xFE\x02\x00\x00\x00\x04\x6D\xC6') 24 | time.sleep(0.1) 25 | -------------------------------------------------------------------------------- /src/aubo_ctrl/README.md: -------------------------------------------------------------------------------- 1 | # 机械臂控制模块 2 | 3 | ## 包含模块 4 | 5 | - 机械臂控制驱动（Aubo） 6 | - 夹爪控制驱动（大寰） 7 | - 业务控制节点 8 | 9 | ## 核心业务逻辑 10 | 11 | 1. 通过摄像头观察小车原材料位置，确认原材料个数，等待接收上料制作命令 12 | 13 | 2. 收到上料命令，定位一个原料，夹取到传送带，放料 14 | 15 | 3. 摄像头观察成品位置，如果发现成品，则定位并将成品码垛到成品区 16 | 17 | ## 问题及解决 18 | 19 | 1. 如果已收到上料制作命令，但是发现原材料位置并没有材料，则将命令加入等待队列 20 | 2. 夹取原材料到传送到时，可能会和其他原材料碰撞，所以要在放料时可以有视觉确认 21 | 3. 根据打标机执行打标后的时间，估算下一个到来的原材料是否已经完成制作 22 | 4. 成品区如果没有及时清理，则要根据视觉进行合理的码垛操作 23 | 24 | 25 | - 启动kinect2节点 C++ 26 | 27 | > 发布彩色RGB图像 sensor_msgs/Image 28 | > 29 | > 发布深度Depth图像 sensor_msgs/Image 30 | > 31 | > 发布点云图 sensor_msgs/PointCloud2 32 | 33 | - 启动盒子定位节点 Python 34 | 35 | > 获取kinect相机图像 36 | > 37 | > 检测不同区域的盒子 38 | > 39 | > 发布盒子信息 40 | 41 | - 启动机械臂节点 C++ 42 | 43 | > in接收盒子信息 44 | > 45 | > out控制机械臂移动到指定位置 46 | > 47 | > out控制夹爪进行抓取 48 | > 49 | > out通知已夹取完毕 50 | 51 | 52 | - TODO: 把传送带和AGV小车距离相机的距离作为参数 53 | - TODO: 把原料中心（AGV小车右上角）像素位置作为参数 -------------------------------------------------------------------------------- /src/aubo_ctrl/assets/calibration_ex_params_mi.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | calibration_time: "Tue Jul 21 15:10:27 2020" 4 | Angle: 3.1354643261166100e+00 5 | AxisX: 9.9995420644796928e-01 6 | AxisY: 6.9848610741646512e-03 7 | AxisZ: 6.5419204203870512e-03 8 | TranslationX: -2.2533910325145919e+02 9 | TranslationY: -2.1568049999430761e+02 10 | TranslationZ: 1.2190305186608682e+03 11 | -------------------------------------------------------------------------------- /src/aubo_ctrl/assets/calibration_in_params_mi.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | calibration_time: "Tue Jul 21 15:09:30 2020" 4 | frame_count: 60 5 | image_width: 1920 6 | image_height: 1080 7 | board_width: 6 8 | board_height: 9 9 | square_size: 20. 10 | rms: 1.2080106724261146e-01 11 | cameraMatrix: !!opencv-matrix 12 | rows: 3 13 | cols: 3 14 | dt: d 15 | data: [ 1.0559770133085560e+03, 0., 9.4354406395265005e+02, 0., 16 | 1.0572581278994355e+03, 5.3305657200616622e+02, 0., 0., 1. ] 17 | distCoeffs: !!opencv-matrix 18 | rows: 5 19 | cols: 1 20 | dt: d 21 | data: [ 2.7586118575507777e-02, -2.0723718526815971e-03, 22 | 7.3785094517489610e-04, 1.1082037672689841e-03, 23 | -3.7098822961964838e-02 ] 24 | -------------------------------------------------------------------------------- /src/aubo_ctrl/cmake/FindAuBo.cmake: -------------------------------------------------------------------------------- 1 | 2 | set(AuBo_DIR ${PREBUILT_DIR}/aubo_dependents) 3 | 4 | # 定义头文件路径 5 | set(AuBo_INCLUDE_DIRS 6 | ${AuBo_DIR}/libconfig/linux_x64/inc; 7 | ${AuBo_DIR}/log4cplus/linux_x64/inc; 8 | ${AuBo_DIR}/protobuf/linux_x64/google/protobuf/inc; 9 | ${AuBo_DIR}/robotController/Header; 10 | ${AuBo_DIR}/robotSDK/inc) 11 | 12 | # 链接库的路径 13 | set(AuBo_LINK_DIRS 14 | ${AuBo_DIR}/protobuf/linux-x64/lib 15 | ${AuBo_DIR}/libconfig/linux_x64/lib 16 | ${AuBo_DIR}/log4cplus/linux_x64/lib 17 | ${AuBo_DIR}/robotController/linux_x64 18 | ${AuBo_DIR}/robotSDK/lib/linux_x64) 19 | 20 | SET(AuBo_LIBRARIES config;log4cplus;protobuf;protobuf-lite;protoc;auborobotcontroller) 21 | 22 | #FOREACH(aubo_component ${AuBo_LIB_COMPONENTS}) 23 | # find_library(lib_${aubocomponent} NAMES ${aubocomponent} PATHS 24 | # ${AuBo_DIR}/protobuf/linux-x64/lib 25 | # ${AuBo_DIR}/libconfig/linux_x64/lib 26 | # ${AuBo_DIR}/log4cplus/linux_x64/lib 27 | # ${AuBo_DIR}/robotController/linux_x64 28 | # ${AuBo_DIR}/robotSDK/lib/linux_x64 29 | # NO_DEFAULT_PATH) 30 | # set(AuBo_LIBRARIES ${AuBo_LIBRARIES};${aubo_component}) 31 | #ENDFOREACH() 32 | #MESSAGE(STATUS "AuBo_LIBRARIES: " ${AuBo_LIBRARIES}) -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/__init__.py -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libconfig++.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/libconfig++.a -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libconfig++.la: -------------------------------------------------------------------------------- 1 | # libconfig++.la - a libtool library file 2 | # Generated by libtool (GNU libtool) 2.4.2 3 | # 4 | # Please DO NOT delete this file! 5 | # It is necessary for linking the library. 6 | 7 | # The name that we can dlopen(3). 8 | dlname='libconfig++.so.11' 9 | 10 | # Names of this library. 11 | library_names='libconfig++.so.11.0.2 libconfig++.so.11 libconfig++.so' 12 | 13 | # The name of the static archive. 14 | old_library='libconfig++.a' 15 | 16 | # Linker flags that can not go in dependency_libs. 17 | inherited_linker_flags='' 18 | 19 | # Libraries that this one depends upon. 20 | dependency_libs='' 21 | 22 | # Names of additional weak libraries provided by this library 23 | weak_library_names='' 24 | 25 | # Version information for libconfig++. 26 | current=11 27 | age=0 28 | revision=2 29 | 30 | # Is this an already installed library? 31 | installed=yes 32 | 33 | # Should we warn about portability when linking against -modules? 34 | shouldnotlink=no 35 | 36 | # Files to dlopen/dlpreopen 37 | dlopen='' 38 | dlpreopen='' 39 | 40 | # Directory that this library needs to be installed in: 41 | libdir='/usr/local/lib' 42 | -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libconfig++.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/libconfig++.so -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libconfig++.so.11: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/libconfig++.so.11 -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libconfig++.so.11.0.2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/libconfig++.so.11.0.2 -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libconfig.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/libconfig.a -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libconfig.la: -------------------------------------------------------------------------------- 1 | # libconfig.la - a libtool library file 2 | # Generated by libtool (GNU libtool) 2.4.2 3 | # 4 | # Please DO NOT delete this file! 5 | # It is necessary for linking the library. 6 | 7 | # The name that we can dlopen(3). 8 | dlname='libconfig.so.11' 9 | 10 | # Names of this library. 11 | library_names='libconfig.so.11.0.2 libconfig.so.11 libconfig.so' 12 | 13 | # The name of the static archive. 14 | old_library='libconfig.a' 15 | 16 | # Linker flags that can not go in dependency_libs. 17 | inherited_linker_flags='' 18 | 19 | # Libraries that this one depends upon. 20 | dependency_libs='' 21 | 22 | # Names of additional weak libraries provided by this library 23 | weak_library_names='' 24 | 25 | # Version information for libconfig. 26 | current=11 27 | age=0 28 | revision=2 29 | 30 | # Is this an already installed library? 31 | installed=yes 32 | 33 | # Should we warn about portability when linking against -modules? 34 | shouldnotlink=no 35 | 36 | # Files to dlopen/dlpreopen 37 | dlopen='' 38 | dlpreopen='' 39 | 40 | # Directory that this library needs to be installed in: 41 | libdir='/usr/local/lib' 42 | -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libconfig.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/libconfig.so -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libconfig.so.11: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/libconfig.so.11 -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libconfig.so.11.0.2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/libconfig.so.11.0.2 -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libconfig.so.9: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/libconfig.so.9 -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libconfig.so.9.2.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/libconfig.so.9.2.0 -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/liblog4cplus-1.2.so.5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/liblog4cplus-1.2.so.5 -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libprotobuf.so.9: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/libprotobuf.so.9 -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libpyauboi5.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/libpyauboi5.so -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libpyauboi5.so.1: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/libpyauboi5.so.1 -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libpyauboi5.so.1.2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/libpyauboi5.so.1.2 -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/libpyauboi5.so.1.2.2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/aubo_ctrl/scripts/aubo/libpyauboi5.so.1.2.2 -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo/logfiles/robot-ctl-python.log: -------------------------------------------------------------------------------- 1 | 2019-07-26 20:44:55,816 [140014906681152] INFO: Jul 26 2019 20:44:55 test beginning... 2 | 2019-07-26 20:44:55,817 [140014906681152] INFO: robot.rshd=0 3 | 2019-07-26 20:44:55,817 [140014906681152] INFO: ip=192.168.1.101, port=8899 4 | 2019-07-26 20:44:56,471 [140014906681152] ERROR: login failed! 5 | 2019-07-26 20:44:56,472 [140014906681152] INFO: connect server192.168.1.101:8899 failed. 6 | 2019-07-26 20:44:57,000 [140014906681152] INFO: Jul 26 2019 20:44:57 test completed. 7 | 2019-07-26 20:44:57,001 [140014906681152] INFO: client_count=0 8 | 2019-07-26 20:44:57,001 [140014906681152] INFO: test completed 9 | 2019-07-26 20:46:48,735 [139806870153024] INFO: Jul 26 2019 20:46:48 test beginning... 10 | 2019-07-26 20:46:48,735 [139806870153024] INFO: robot.rshd=0 11 | 2019-07-26 20:46:48,735 [139806870153024] INFO: ip=192.168.1.101, port=8899 12 | 2019-07-26 20:46:50,615 [139806870153024] ERROR: login failed! 13 | 2019-07-26 20:46:50,616 [139806870153024] INFO: connect server192.168.1.101:8899 failed. 14 | 2019-07-26 20:46:52,000 [139806870153024] INFO: Jul 26 2019 20:46:52 test completed. 15 | 2019-07-26 20:46:52,000 [139806870153024] INFO: client_count=0 16 | 2019-07-26 20:46:52,001 [139806870153024] INFO: test completed 17 | 2019-07-27 15:46:12,945 [140127415379776] INFO: Jul 27 2019 15:46:12 test beginning... 18 | 2019-07-27 15:46:12,946 [140127415379776] INFO: robot.rshd=0 19 | 2019-07-27 15:46:12,946 [140127415379776] INFO: ip=192.168.17.250, port=8899 20 | 2019-07-27 15:46:14,006 [140127415379776] INFO: client_count=0 21 | 2019-07-27 15:46:54,291 [140067021731648] INFO: Jul 27 2019 15:46:54 test beginning... 22 | 2019-07-27 15:46:54,292 [140067021731648] INFO: robot.rshd=0 23 | 2019-07-27 15:46:54,292 [140067021731648] INFO: ip=192.168.17.250, port=8899 24 | -------------------------------------------------------------------------------- /src/aubo_ctrl/scripts/aubo_state_node.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # coding=utf-8 3 | 4 | from aubo.robotcontrol import * 5 | from time import * 6 | from math import degrees 7 | from std_msgs.msg import Float32MultiArray 8 | 9 | 10 | import rospy 11 | 12 | 13 | def parse_point(point): 14 | joints = [] 15 | 16 | for j in point['joint']: 17 | joints.append(degrees(j)) 18 | 19 | return joints 20 | 21 | 22 | if __name__ == '__main__': 23 | rospy.init_node("aubo_state_node") 24 | 25 | ip = rospy.get_param("~aubo_host", "192.168.1.102") 26 | port = rospy.get_param("~aubo_port", 8899) 27 | 28 | publisher = rospy.Publisher("/aubo/joints", Float32MultiArray, queue_size=1000) 29 | 30 | # 初始化Aubo系统 31 | Auboi5Robot.initialize() 32 | 33 | # 创建aubo机器人对象 34 | robot = Auboi5Robot() 35 | 36 | # 创建上下文 37 | handle = robot.create_context() 38 | 39 | result = robot.connect(ip, port) 40 | 41 | if result == RobotErrorType.RobotError_SUCC: 42 | # 连接成功 43 | 44 | rate = rospy.Rate(10) 45 | while not rospy.is_shutdown(): 46 | point = robot.get_current_waypoint() 47 | 48 | joints = parse_point(point) 49 | # print joints 50 | 51 | msg = Float32MultiArray() 52 | msg.data = joints 53 | publisher.publish(msg) 54 | 55 | rate.sleep() 56 | 57 | # 系统退出 58 | Auboi5Robot.uninitialize() 59 | 60 | -------------------------------------------------------------------------------- /src/aubo_ctrl/src/aubo/BlockingQueue.h: -------------------------------------------------------------------------------- 1 | #ifndef AUBO_CTRL_BLOCKINGQUEUE_H 2 | #define AUBO_CTRL_BLOCKINGQUEUE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | template 10 | class BlockingQueue { 11 | public: 12 | using MutexLockGuard = std::lock_guard; 13 | 14 | BlockingQueue() 15 | : _mutex(), 16 | _notEmpty(), 17 | _queue() 18 | { 19 | } 20 | 21 | BlockingQueue(const BlockingQueue &) = delete; 22 | BlockingQueue& operator=(const BlockingQueue &) = delete; 23 | 24 | void put(const T &x) 25 | { 26 | { 27 | MutexLockGuard lock(_mutex); 28 | _queue.push_back(x); 29 | } 30 | _notEmpty.notify_one(); 31 | } 32 | 33 | void put(T &&x) 34 | { 35 | { 36 | MutexLockGuard lock(_mutex); 37 | _queue.push_back(std::move(x)); 38 | } 39 | _notEmpty.notify_one(); 40 | } 41 | 42 | T take() 43 | { 44 | std::unique_lock lock(_mutex); 45 | _notEmpty.wait(lock, [this]{ 46 | if (this->_is_release) { 47 | return true; 48 | } 49 | return !this->_queue.empty(); 50 | }); 51 | // assert(!_queue.empty()); 52 | 53 | if (_is_release) { 54 | throw std::exception(); 55 | } 56 | 57 | T front(std::move(_queue.front())); 58 | _queue.pop_front(); 59 | 60 | return front; 61 | } 62 | 63 | size_t size() const 64 | { 65 | MutexLockGuard lock(_mutex); 66 | return _queue.size(); 67 | } 68 | 69 | void release() { 70 | _is_release = true; 71 | _notEmpty.notify_one(); 72 | } 73 | 74 | private: 75 | mutable std::mutex _mutex; 76 | std::condition_variable _notEmpty; 77 | std::deque _queue; 78 | bool _is_release = false; 79 | }; 80 | 81 | #endif //AUBO_CTRL_BLOCKINGQUEUE_H 82 | -------------------------------------------------------------------------------- /src/aubo_ctrl/src/aubo/util.h: -------------------------------------------------------------------------------- 1 | #ifndef UTIL_H 2 | #define UTIL_H 3 | 4 | #include "AuboRobotMetaType.h" 5 | #include "serviceinterface.h" 6 | #include 7 | using namespace std; 8 | 9 | class Util 10 | { 11 | public: 12 | Util(); 13 | 14 | /** 打印路点信息 **/ 15 | static void printWaypoint(aubo_robot_namespace::wayPoint_S &wayPoint); 16 | 17 | /** 打印关节状态信息 **/ 18 | static void printJointStatus(const aubo_robot_namespace::JointStatus *jointStatus, int len); 19 | 20 | /** 打印事件信息 **/ 21 | static void printEventInfo(const aubo_robot_namespace::RobotEventInfo &eventInfo); 22 | 23 | /** 打印诊断信息 **/ 24 | static void printRobotDiagnosis(const aubo_robot_namespace::RobotDiagnosis &robotDiagnosis); 25 | 26 | 27 | static void initJointAngleArray(double *array, double joint0,double joint1,double joint2,double joint3,double joint4,double joint5); 28 | 29 | }; 30 | 31 | #endif // UTIL_H 32 | -------------------------------------------------------------------------------- /src/aubo_ctrl/src/test_blanding.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "actionlib/client/action_client.h" 5 | #include "actionlib/client/terminal_state.h" 6 | #include "itheima_msgs/ArmWorkAction.h" 7 | 8 | 9 | using namespace std; 10 | 11 | 12 | typedef actionlib::ClientGoalHandle ClientGoalHandle; 13 | typedef itheima_msgs::ArmWorkFeedbackConstPtr FeedbackConstPtr; 14 | typedef itheima_msgs::ArmWorkResultConstPtr ResultConstPtr; 15 | typedef actionlib::ActionClient ActionClient; 16 | 17 | 18 | void transition_cb(ClientGoalHandle goalHandle) { 19 | const actionlib::CommState &state = goalHandle.getCommState(); 20 | if (state == actionlib::CommState::ACTIVE) { 21 | ROS_INFO_STREAM("ACTIVE"); 22 | } else if (state == actionlib::CommState::DONE) { 23 | ROS_INFO_STREAM("DONE"); 24 | actionlib::TerminalState ts = goalHandle.getTerminalState(); 25 | if (ts == ts.ABORTED) { 26 | ROS_INFO("server working error, don't finish my job."); 27 | } else if (ts == ts.PREEMPTED) { 28 | ROS_INFO("client cancel job."); 29 | } else if (ts == ts.SUCCEEDED) { 30 | ROS_INFO("server finish job."); 31 | } else if(ts == ts.REJECTED) { 32 | ROS_INFO("server rejected job."); 33 | } 34 | ROS_INFO_STREAM(goalHandle.getResult()->result); 35 | } else { 36 | ROS_INFO_STREAM(state.toString()); 37 | } 38 | } 39 | 40 | void feedback_cb(ClientGoalHandle goalHandle, 41 | const FeedbackConstPtr feedback) { 42 | ROS_INFO("========= feedback_cb ========="); 43 | } 44 | 45 | 46 | int main(int argc, char **argv) { 47 | setlocale(LC_ALL, ""); 48 | 49 | ros::init(argc, argv, "test_blanding"); 50 | ros::NodeHandle node; 51 | 52 | ros::AsyncSpinner spinner(2); 53 | spinner.start(); 54 | 55 | ActionClient client(node, "/aubo/ctrl"); 56 | client.waitForActionServerToStart(); 57 | 58 | itheima_msgs::ArmWorkGoal goal; 59 | goal.type = 1; 60 | auto handle = client.sendGoal(goal, 61 | boost::bind(&transition_cb, _1), 62 | boost::bind(&feedback_cb, _1, _2)); 63 | 64 | 65 | ros::waitForShutdown(); 66 | 67 | return 0; 68 | } -------------------------------------------------------------------------------- /src/aubo_ctrl/src/test_dh_gripper.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | 8 | ros::NodeHandle initRos(int argc, char** argv); 9 | 10 | void sendGoal(string action_name, int motorID, int position, int force); 11 | 12 | int main(int argc, char** argv) { 13 | ROS_INFO("----dh_hand_client----"); 14 | 15 | // 初始化ros 16 | ros::NodeHandle nh = initRos(argc, argv); 17 | 18 | // 动作服务名称 19 | string action_name = "actuate_hand"; 20 | 21 | ros::Rate rate(0.5); 22 | 23 | for (int i = 0; i < 3; ++i) { 24 | // 打开 25 | sendGoal(action_name, 1, 100, 100); 26 | 27 | rate.sleep(); 28 | 29 | // 关闭 30 | sendGoal(action_name, 1, 0, 100); 31 | 32 | rate.sleep(); 33 | } 34 | 35 | ros::spinOnce(); 36 | ros::shutdown(); 37 | return 0; 38 | } 39 | 40 | /** 41 | * 初始化ros 42 | * @param argc 43 | * @param argv 44 | * @return 45 | */ 46 | ros::NodeHandle initRos(int argc, char** argv) { 47 | ros::init(argc, argv, "dh_hand_client"); 48 | ros::NodeHandle nh; 49 | return nh; 50 | } 51 | 52 | /** 53 | * 发送到指定夹爪的目的地位置 54 | * @param name action server name. 55 | * @param motorID 设备ID，默认为:1 56 | * @param position 夹爪位置，范围：0~100 57 | * @param force 力控值，范围：20~100 58 | */ 59 | void sendGoal(string action_name, int motorID, int position, int force) { 60 | actionlib::SimpleActionClient client(action_name, true); 61 | 62 | ROS_INFO("----wait for server----"); 63 | client.waitForServer(); 64 | 65 | dh_hand_driver::ActuateHandGoal goal; 66 | goal.MotorID = motorID; 67 | goal.position = position; 68 | goal.force = force; 69 | 70 | ROS_INFO("----send goal----"); 71 | client.sendGoal(goal); 72 | 73 | ROS_INFO("----wait for result----"); 74 | bool result = client.waitForResult(ros::Duration(10.0)); 75 | if(result) { 76 | ROS_INFO("action finished, state: %s.", client.getState().toString().c_str()); 77 | } else { 78 | ROS_INFO("action don't finish before the time out."); 79 | } 80 | } -------------------------------------------------------------------------------- /src/aubo_ctrl/src/test_feeding.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "actionlib/client/action_client.h" 5 | #include "actionlib/client/terminal_state.h" 6 | #include "itheima_msgs/ArmWorkAction.h" 7 | 8 | 9 | using namespace std; 10 | 11 | 12 | typedef actionlib::ClientGoalHandle ClientGoalHandle; 13 | typedef itheima_msgs::ArmWorkFeedbackConstPtr FeedbackConstPtr; 14 | typedef itheima_msgs::ArmWorkResultConstPtr ResultConstPtr; 15 | typedef actionlib::ActionClient ActionClient; 16 | 17 | 18 | void transition_cb(ClientGoalHandle goalHandle) { 19 | const actionlib::CommState &state = goalHandle.getCommState(); 20 | if (state == actionlib::CommState::ACTIVE) { 21 | ROS_INFO_STREAM("ACTIVE"); 22 | } else if (state == actionlib::CommState::DONE) { 23 | ROS_INFO_STREAM("DONE"); 24 | actionlib::TerminalState ts = goalHandle.getTerminalState(); 25 | if (ts == ts.ABORTED) { 26 | ROS_INFO("server working error, don't finish my job."); 27 | } else if (ts == ts.PREEMPTED) { 28 | ROS_INFO("client cancel job."); 29 | } else if (ts == ts.SUCCEEDED) { 30 | ROS_INFO("server finish job."); 31 | } else if (ts == ts.REJECTED) { 32 | ROS_INFO("server rejected job."); 33 | } 34 | ROS_INFO_STREAM(goalHandle.getResult()->result); 35 | } else { 36 | ROS_INFO_STREAM(state.toString()); 37 | } 38 | } 39 | 40 | void feedback_cb(ClientGoalHandle goalHandle, 41 | const FeedbackConstPtr feedback) { 42 | ROS_INFO("========= feedback_cb ========="); 43 | } 44 | 45 | 46 | int main(int argc, char **argv) { 47 | ros::init(argc, argv, "test_feeding"); 48 | ros::NodeHandle node; 49 | 50 | ros::AsyncSpinner spinner(2); 51 | spinner.start(); 52 | 53 | ActionClient client(node, "/aubo/ctrl"); 54 | client.waitForActionServerToStart(); 55 | 56 | itheima_msgs::ArmWorkGoal goal; 57 | goal.type = 0; 58 | auto handle = client.sendGoal(goal, 59 | boost::bind(&transition_cb, _1), 60 | boost::bind(&feedback_cb, _1, _2)); 61 | 62 | 63 | ros::waitForShutdown(); 64 | 65 | return 0; 66 | } -------------------------------------------------------------------------------- /src/aubo_ctrl/src/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(common_utils Rotation3DUtils.h Rotation3DUtils.cpp) 2 | target_link_libraries(common_utils ${OpenCV_LIBRARIES}) 3 | -------------------------------------------------------------------------------- /src/aubo_ctrl/src/utils/Rotation3DUtils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @Author: PoplarTang 3 | * @CreateTime: 2019-12-24 4 | * @Description: 5 | */ 6 | #include "Rotation3DUtils.h" 7 | int main(int argc, char **argv) { 8 | 9 | return 0; 10 | } -------------------------------------------------------------------------------- /src/dh_gripper_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dh_hand_driver) 3 | 4 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 6 | 7 | # Check C++11 or C++0x support 8 | include(CheckCXXCompilerFlag) 9 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 10 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 11 | if(COMPILER_SUPPORTS_CXX11) 12 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 13 | add_definitions(-DCOMPILEDWITHC11) 14 | message(STATUS "Using flag -std=c++11.") 15 | elseif(COMPILER_SUPPORTS_CXX0X) 16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 17 | add_definitions(-DCOMPILEDWITHC0X) 18 | message(STATUS "Using flag -std=c++0x.") 19 | else() 20 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 21 | endif() 22 | 23 | find_package(catkin REQUIRED COMPONENTS 24 | roscpp 25 | rospy 26 | serial 27 | std_msgs 28 | message_generation 29 | actionlib_msgs 30 | actionlib 31 | genmsg 32 | ) 33 | 34 | add_action_files( 35 | DIRECTORY action 36 | FILES ActuateHand.action 37 | ) 38 | 39 | add_service_files( 40 | DIRECTORY srv 41 | FILES hand_state.srv 42 | ) 43 | 44 | generate_messages( 45 | DEPENDENCIES 46 | actionlib_msgs 47 | std_msgs 48 | ) 49 | 50 | catkin_package( 51 | INCLUDE_DIRS 52 | include 53 | CATKIN_DEPENDS 54 | message_runtime 55 | roscpp 56 | rospy 57 | serial 58 | actionlib_msgs 59 | std_msgs 60 | ) 61 | 62 | # add_library(LIBS 63 | 64 | # ) 65 | 66 | include_directories( 67 | ${catkin_INCLUDE_DIRS} 68 | include 69 | include/dh_hand_driver 70 | ) 71 | 72 | #add_library(${PROJECT_NAME} SHARED 73 | #include/dh_hand_driver/hand_controller.cpp 74 | #include/dh_hand_driver/DH_datastream.cpp 75 | #include/dh_hand_driver/hand_driver.cpp 76 | #) 77 | 78 | 79 | add_executable(hand_controller 80 | include/dh_hand_driver/hand_controller.cpp 81 | include/dh_hand_driver/DH_datastream.cpp 82 | include/dh_hand_driver/hand_driver.cpp 83 | src/node.cpp) 84 | 85 | add_dependencies(hand_controller 86 | dh_hand_driver_generate_messages_cpp 87 | ${dh_hand_driver_EXPORTED_TARGETS}) 88 | 89 | target_link_libraries(hand_controller 90 | ${catkin_LIBRARIES}) 91 | 92 | add_executable(hand_controller_client 93 | src/test_client.cpp) 94 | 95 | add_dependencies(hand_controller_client 96 | dh_hand_driver_generate_messages_cpp 97 | ${dh_hand_driver_EXPORTED_TARGETS}) 98 | target_link_libraries(hand_controller_client ${catkin_LIBRARIES}) 99 | 100 | -------------------------------------------------------------------------------- /src/dh_gripper_ros/action/ActuateHand.action: -------------------------------------------------------------------------------- 1 | # Command to control the GRIPPERS 2 | # author: Jie Sun , AUG 2018 3 | # Copyright @ DH-Robotics 4 | 5 | 6 | # goal definition 7 | 8 | int32 MotorID 9 | int32 force 10 | int32 position 11 | 12 | --- 13 | 14 | # result definition 15 | 16 | bool opration_done 17 | 18 | --- 19 | 20 | # feedback definition 21 | 22 | bool position_reached 23 | -------------------------------------------------------------------------------- /src/dh_gripper_ros/doc/寄存器列表 - 用户.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/dh_gripper_ros/doc/寄存器列表 - 用户.xlsx -------------------------------------------------------------------------------- /src/dh_gripper_ros/doc/寄存器列表 -三指.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/dh_gripper_ros/doc/寄存器列表 -三指.xlsx -------------------------------------------------------------------------------- /src/dh_gripper_ros/include/dh_hand_driver/DH_datastream.cpp: -------------------------------------------------------------------------------- 1 |  2 | #include "DH_datastream.h" 3 | #include 4 | #include 5 | DH_Robotics::DH_DataStream::DH_DataStream() 6 | { 7 | DataStream_clear(); 8 | } 9 | 10 | DH_Robotics::DH_DataStream::~DH_DataStream() 11 | { 12 | 13 | } 14 | 15 | void DH_Robotics::DH_DataStream::DataStream_clear() 16 | { 17 | frame_head[0]=0xFF; 18 | frame_head[1]=0xFE; 19 | frame_head[2]=0xFD; 20 | frame_head[3]=0xFC; 21 | DeviceID=0x01; 22 | Register[0]=0x00; 23 | Register[1]=0x00; 24 | option=0x00; 25 | reserve=0x00; 26 | data[0]=0x00; 27 | data[1]=0x00; 28 | data[2]=0x00; 29 | data[3]=0x00; 30 | frame_end=0xFB; 31 | size = (uint8_t)(&size - frame_head); 32 | } 33 | 34 | int DH_Robotics::DH_DataStream::DatatoStream(uint8_t *buf,uint8_t len) 35 | { 36 | if(len!=size) 37 | { 38 | return -1; 39 | } 40 | memcpy(buf,frame_head,len); 41 | return 0; 42 | 43 | } 44 | 45 | int DH_Robotics::DH_DataStream::DatafromStream(uint8_t *buf,uint8_t len) 46 | { 47 | if(len!=size) 48 | { 49 | return -1; 50 | } 51 | memcpy(frame_head,buf,len); 52 | return 0; 53 | } -------------------------------------------------------------------------------- /src/dh_gripper_ros/include/dh_hand_driver/DH_datastream.h: -------------------------------------------------------------------------------- 1 | #ifndef DH_DATASTREAM_H 2 | #define DH_DATASTREAM_H 3 | #include 4 | 5 | namespace DH_Robotics{ 6 | 7 | class DH_DataStream 8 | { 9 | public: 10 | //Construct a data stream 11 | DH_DataStream(); 12 | ~DH_DataStream(); 13 | //set dataStream to defalut 14 | void DataStream_clear(); 15 | 16 | /** 17 | * @brief : transfer dataStream to uint8_t array 18 | * 19 | * @param buf : Source array address 20 | * @param len : Source array length 21 | * @return int : Returns 0 on success; otherwise returns -1. 22 | */ 23 | int DatatoStream(uint8_t *buf,uint8_t len); 24 | 25 | /** 26 | * @brief :transfer uint8_t array to dataStream 27 | * 28 | * @param buf : Destination array address 29 | * @param len : Destination array length 30 | * @return int : Returns 0 on success; otherwise returns -1. 31 | */ 32 | int DatafromStream(uint8_t *buf,uint8_t len); 33 | 34 | public: 35 | uint8_t frame_head[4]; //Frame head ,do not modify (defalut : 0xFF 0xFE 0xFD 0xFC ) 36 | uint8_t DeviceID; //Device ID (defalut : 0x01 ) 37 | uint8_t Register[2]; //register and subRegister (defalur : 0x00 0x00 ) 38 | uint8_t option; //read or write option 39 | uint8_t reserve; //reserve byte 40 | uint8_t data[4]; //frame data （defalut : 0x00 0x00 0x00 0x00 ） （Little_endian） 41 | uint8_t frame_end; //frame end ,do not modify (defalut : 0xFB) 42 | uint8_t size; //save dataSteam byte count 43 | 44 | }; 45 | 46 | } 47 | #endif // DH_DATASTREAM_H 48 | -------------------------------------------------------------------------------- /src/dh_gripper_ros/include/dh_hand_driver/definition.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Definition of the commands of the server motors and the stepper motors 3 | * auther: Jie Sun 4 | * email: jie.sun@dh-robotics.com 5 | * 6 | * date: 2018 April 23 7 | */ 8 | 9 | // For DH HAND 10 | 11 | #ifndef HAND_CONTROLLER_DEFINITION 12 | #define HAND_CONTROLLER_DEFINITION 13 | 14 | namespace DH_Robotics 15 | { 16 | 17 | enum OPTION 18 | { 19 | Read = 0x00, 20 | Write = 0x01 21 | }; 22 | 23 | enum RegisterType 24 | { 25 | R_Force = 0x05, 26 | R_Posistion_1 = 0x06, 27 | R_Posistion_2 = 0x07, 28 | R_Initialization = 0x08, 29 | R_Feedback = 0x0f, 30 | R_Slowmode = 0x11 31 | }; 32 | 33 | enum SlowModeType 34 | { 35 | S_stop = 0x00, 36 | S_close = 0x01, 37 | S_open = 0x02, 38 | }; 39 | 40 | enum FeedbackType 41 | { 42 | UNDEFINE = 0, 43 | ARRIVED = 2, 44 | CATCHED = 3, 45 | }; 46 | 47 | } // namespace DH_Robotics 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /src/dh_gripper_ros/include/dh_hand_driver/hand_driver.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "dh_hand_driver/hand_driver.h" 3 | DH_Hand_Base::DH_Hand_Base(): velocity_(0),position_1(0),target_reached_1(false) 4 | { 5 | } 6 | 7 | DH_Hand_Base::~DH_Hand_Base() 8 | { 9 | } 10 | 11 | void DH_Hand_Base::reset() 12 | { 13 | 14 | } 15 | 16 | std::vector DH_Hand_Base::getStream() 17 | { 18 | std::vector temp_data; 19 | uint8_t buf[14]; 20 | mDatastream.DatatoStream(buf,mDatastream.size); 21 | for(int i = 0;i<14;i++) 22 | temp_data.push_back(buf[i]); 23 | return temp_data; 24 | } 25 | 26 | void DH_Hand_Base::SetOpration(int Reg,int data,int Write,int submode) 27 | { 28 | mDatastream.DataStream_clear(); 29 | mDatastream.Register[0]=Reg &0xFF; 30 | mDatastream.Register[1]=submode&0xFF; 31 | mDatastream.option=Write&0xFF; 32 | mDatastream.data[0]=data&0xFF; 33 | mDatastream.data[1]=(data>>8)&0xFF; 34 | mDatastream.data[2]=(data>>16)&0xFF; 35 | mDatastream.data[3]=(data>>24)&0xFF; 36 | } 37 | 38 | void DH_Hand_Base::setInitialize() 39 | { 40 | //FF FE FD FC 01 08 02 01 00 00 00 00 00 FB 41 | SetOpration(); 42 | } 43 | 44 | void DH_Hand_Base::setMotorPosition(int MotorID ,const int &target_position) 45 | { 46 | if(MotorID == 1) 47 | SetOpration(DH_Robotics::R_Posistion_1,target_position,DH_Robotics::Write); 48 | else if(MotorID == 2) 49 | SetOpration(DH_Robotics::R_Posistion_2,target_position,DH_Robotics::Write); 50 | 51 | } 52 | 53 | void DH_Hand_Base::setMotorForce(const int &target_force) 54 | { 55 | //FF FE FD FC 01 05 02 01 00 5A 00 00 00 FB 56 | SetOpration(DH_Robotics::R_Force,target_force,DH_Robotics::Write); 57 | } 58 | 59 | void DH_Hand_Base::getMotorPosition(const int &MotorID) 60 | { 61 | //FF FE FD FC 01 06 02 01 00 5A 00 00 00 FB 62 | if(MotorID == 1) 63 | SetOpration(DH_Robotics::R_Posistion_1,0,DH_Robotics::Read); 64 | else if(MotorID == 2) 65 | SetOpration(DH_Robotics::R_Posistion_2,0,DH_Robotics::Read); 66 | } 67 | 68 | void DH_Hand_Base::getMotorForce() 69 | { 70 | //FF FE FD FC 01 05 02 01 00 5A 00 00 00 FB 71 | SetOpration(DH_Robotics::R_Force,0,DH_Robotics::Read); 72 | } 73 | 74 | void DH_Hand_Base::getFeedback(const int &MotorID) 75 | { 76 | SetOpration(DH_Robotics::R_Feedback,0,DH_Robotics::Read,MotorID); 77 | } 78 | -------------------------------------------------------------------------------- /src/dh_gripper_ros/launch/dh_hand_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /src/dh_gripper_ros/scripts/DH_hand.rules: -------------------------------------------------------------------------------- 1 | # make the device_port be fixed by DH_hand 2 | # 3 | KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE:="0777", SYMLINK+="DH_hand" 4 | 5 | -------------------------------------------------------------------------------- /src/dh_gripper_ros/scripts/create_udev_rules.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "remap serial port(ttyACMx) to DH_hand" 4 | echo "DH_hand connection as /dev/DH_hand , check it using the command : ls -l /dev|grep ttyACM" 5 | echo "start copy DH_hand.rules to /etc/udev/rules.d/" 6 | echo "`rospack find dh_hand_driver`/scripts/DH_hand.rules" 7 | sudo cp `rospack find dh_hand_driver`/scripts/DH_hand.rules /etc/udev/rules.d 8 | echo " " 9 | echo "Restarting udev" 10 | echo "" 11 | sudo service udev reload 12 | sudo service udev restart 13 | echo "finish " 14 | echo "If you are already insert the Hand usb, please unplug it and plug again" 15 | -------------------------------------------------------------------------------- /src/dh_gripper_ros/scripts/delete_udev_rules.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "delete remap tserial port(ttyACMx) to DH_hand" 4 | echo "sudo rm /etc/udev/rules.d/DH_hand.rules" 5 | sudo rm /etc/udev/rules.d/DH_hand.rules 6 | echo " " 7 | echo "Restarting udev" 8 | echo "" 9 | sudo service udev reload 10 | sudo service udev restart 11 | echo "finish delete" 12 | -------------------------------------------------------------------------------- /src/dh_gripper_ros/src/node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main(int argc, char** argv) 5 | { 6 | ros::init(argc, argv, "hand_controller"); 7 | ros::NodeHandle n; 8 | HandController controller(n,"actuate_hand"); 9 | ros::spin(); 10 | 11 | return 0; 12 | } 13 | 14 | -------------------------------------------------------------------------------- /src/dh_gripper_ros/srv/hand_state.srv: -------------------------------------------------------------------------------- 1 | # get Force : = 0 2 | # get Position 1 : = 1 3 | # get Position 2 : = 2 4 | 5 | int32 get_target 6 | --- 7 | int32 return_data -------------------------------------------------------------------------------- /src/iai_kinect2/iai_kinect2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(iai_kinect2) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /src/iai_kinect2/iai_kinect2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | iai_kinect2 4 | 0.0.1 5 | 6 | The iai_kinect2 metapackage 7 | 8 | 9 | Thiemo Wiedemeyer 10 | Apache 2.0 11 | 12 | catkin 13 | 14 | kinect2_calibration 15 | kinect2_viewer 16 | kinect2_bridge 17 | kinect2_registration 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_bridge/data/040942235247/calib_color.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | cameraMatrix: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: d 6 | data: [ 1.0561980914641024e+03, 0., 9.4426761275297940e+02, 0., 7 | 1.0575408977159755e+03, 5.3291545333244630e+02, 0., 0., 1. ] 8 | distortionCoefficients: !!opencv-matrix 9 | rows: 1 10 | cols: 5 11 | dt: d 12 | data: [ 3.2321187813410826e-02, -1.8712745340098196e-02, 13 | 6.2493087507614717e-04, 1.2331710400644979e-03, 14 | -1.8433102733265892e-02 ] 15 | rotation: !!opencv-matrix 16 | rows: 3 17 | cols: 3 18 | dt: d 19 | data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ] 20 | projection: !!opencv-matrix 21 | rows: 4 22 | cols: 4 23 | dt: d 24 | data: [ 1.0599465578241038e+03, 0., 9.5488326677588441e+02, 0., 0., 25 | 1.0539326808799726e+03, 5.2373858291060583e+02, 0., 0., 0., 1., 26 | 0., 0., 0., 0., 1. ] 27 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_bridge/data/040942235247/calib_depth.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | depthShift: -2.4973913350529973e+01 4 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_bridge/data/040942235247/calib_ir.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | cameraMatrix: !!opencv-matrix 4 | rows: 3 5 | cols: 3 6 | dt: d 7 | data: [ 3.6512158525272957e+02, 0., 2.5582210321928576e+02, 0., 8 | 3.6502516931544818e+02, 2.0403714532377998e+02, 0., 0., 1. ] 9 | distortionCoefficients: !!opencv-matrix 10 | rows: 1 11 | cols: 5 12 | dt: d 13 | data: [ 1.0398215585789732e-01, -2.5551569143172653e-01, 14 | 7.5172568930520424e-04, -1.8368725388028954e-04, 15 | 6.1030458198677402e-03 ] 16 | rotation: !!opencv-matrix 17 | rows: 3 18 | cols: 3 19 | dt: d 20 | data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ] 21 | projection: !!opencv-matrix 22 | rows: 4 23 | cols: 4 24 | dt: d 25 | data: [ 3.6512158525272957e+02, 0., 2.5582210321928576e+02, 0., 0., 26 | 3.6502516931544818e+02, 2.0403714532377998e+02, 0., 0., 0., 1., 27 | 0., 0., 0., 0., 1. ] 28 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_bridge/data/040942235247/calib_pose.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | rotation: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: d 6 | data: [ 9.9979883926593693e-01, -1.6871335541758126e-02, 7 | -1.0846153213119176e-02, 1.6891097527572209e-02, 8 | 9.9985583336600126e-01, 1.7330055664057141e-03, 9 | 1.0815351441312036e-02, -1.9158603854556545e-03, 10 | 9.9993967700666042e-01 ] 11 | translation: !!opencv-matrix 12 | rows: 3 13 | cols: 1 14 | dt: d 15 | data: [ -5.2052476112081990e-02, -4.6313865353939110e-04, 16 | 8.8806735554907584e-04 ] 17 | essential: !!opencv-matrix 18 | rows: 3 19 | cols: 3 20 | dt: d 21 | data: [ -2.0009439617717153e-05, -8.8705201686837285e-04, 22 | -4.6464974129998811e-04, 1.4508545338106024e-03, 23 | -1.1470815928716492e-04, 5.2039704036310315e-02, 24 | -4.1617796233292743e-04, -5.2052785649435836e-02, 25 | -9.5230503642644511e-05 ] 26 | fundamental: !!opencv-matrix 27 | rows: 3 28 | cols: 3 29 | dt: d 30 | data: [ -1.1817221592596941e-08, -5.2697406371005770e-07, 31 | 1.1627629703684574e-05, 8.6173835202613450e-07, 32 | -6.8533918097934840e-08, 1.1146863908955499e-02, 33 | -7.0056301516242358e-04, -3.2237824184460216e-02, 1. ] 34 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_bridge/data/196605135147/calib_color.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | cameraMatrix: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: d 6 | data: [ 1.0599465578241038e+03, 0., 9.5488326677588441e+02, 0., 7 | 1.0539326808799726e+03, 5.2373858291060583e+02, 0., 0., 1. ] 8 | distortionCoefficients: !!opencv-matrix 9 | rows: 1 10 | cols: 5 11 | dt: d 12 | data: [ 5.6268441170930321e-02, -7.4199141308694802e-02, 13 | 1.4250797540545752e-03, -1.6951722389720336e-03, 14 | 2.4107681263086548e-02 ] 15 | rotation: !!opencv-matrix 16 | rows: 3 17 | cols: 3 18 | dt: d 19 | data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ] 20 | projection: !!opencv-matrix 21 | rows: 4 22 | cols: 4 23 | dt: d 24 | data: [ 1.0599465578241038e+03, 0., 9.5488326677588441e+02, 0., 0., 25 | 1.0539326808799726e+03, 5.2373858291060583e+02, 0., 0., 0., 1., 26 | 0., 0., 0., 0., 1. ] 27 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_bridge/data/196605135147/calib_depth.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | depthShift: -1.3354238655403018e+01 3 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_bridge/data/196605135147/calib_ir.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | cameraMatrix: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: d 6 | data: [ 3.6694757270064110e+02, 0., 2.4298551682775121e+02, 0., 7 | 3.6479117165721783e+02, 2.0769853575457685e+02, 0., 0., 1. ] 8 | distortionCoefficients: !!opencv-matrix 9 | rows: 1 10 | cols: 5 11 | dt: d 12 | data: [ 9.6558110377123016e-02, -2.8298244017989416e-01, 13 | 1.5889951609432634e-04, -5.0751483524871712e-04, 14 | 1.0523948765859192e-01 ] 15 | rotation: !!opencv-matrix 16 | rows: 3 17 | cols: 3 18 | dt: d 19 | data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ] 20 | projection: !!opencv-matrix 21 | rows: 4 22 | cols: 4 23 | dt: d 24 | data: [ 3.6694757270064110e+02, 0., 2.4298551682775121e+02, 0., 0., 25 | 3.6479117165721783e+02, 2.0769853575457685e+02, 0., 0., 0., 1., 26 | 0., 0., 0., 0., 1. ] 27 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_bridge/data/196605135147/calib_pose.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | rotation: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: d 6 | data: [ 9.9979883926593693e-01, -1.6871335541758126e-02, 7 | -1.0846153213119176e-02, 1.6891097527572209e-02, 8 | 9.9985583336600126e-01, 1.7330055664057141e-03, 9 | 1.0815351441312036e-02, -1.9158603854556545e-03, 10 | 9.9993967700666042e-01 ] 11 | translation: !!opencv-matrix 12 | rows: 3 13 | cols: 1 14 | dt: d 15 | data: [ -5.2052476112081990e-02, -4.6313865353939110e-04, 16 | 8.8806735554907584e-04 ] 17 | essential: !!opencv-matrix 18 | rows: 3 19 | cols: 3 20 | dt: d 21 | data: [ -2.0009439617717153e-05, -8.8705201686837285e-04, 22 | -4.6464974129998811e-04, 1.4508545338106024e-03, 23 | -1.1470815928716492e-04, 5.2039704036310315e-02, 24 | -4.1617796233292743e-04, -5.2052785649435836e-02, 25 | -9.5230503642644511e-05 ] 26 | fundamental: !!opencv-matrix 27 | rows: 3 28 | cols: 3 29 | dt: d 30 | data: [ -1.1817221592596941e-08, -5.2697406371005770e-07, 31 | 1.1627629703684574e-05, 8.6173835202613450e-07, 32 | -6.8533918097934840e-08, 1.1146863908955499e-02, 33 | -7.0056301516242358e-04, -3.2237824184460216e-02, 1. ] 34 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_bridge/data/299150235147/calib_color.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | cameraMatrix: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: d 6 | data: [ 1.0660120360637927e+03, 0., 9.4558752751085558e+02, 0., 7 | 1.0688708399911650e+03, 5.2006994012356529e+02, 0., 0., 1. ] 8 | distortionCoefficients: !!opencv-matrix 9 | rows: 1 10 | cols: 5 11 | dt: d 12 | data: [ 5.9360108008991816e-02, -6.2758287999836640e-02, 13 | -1.5766859436148536e-03, -1.1502971845708829e-03, 14 | 7.7657531491476016e-03 ] 15 | rotation: !!opencv-matrix 16 | rows: 3 17 | cols: 3 18 | dt: d 19 | data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ] 20 | projection: !!opencv-matrix 21 | rows: 4 22 | cols: 4 23 | dt: d 24 | data: [ 1.0705899358803581e+03, 0., 9.4912209493857927e+02, 0., 0., 25 | 1.0734714110656350e+03, 5.1739403678512770e+02, 0., 0., 0., 1., 26 | 0., 0., 0., 0., 1. ] 27 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_bridge/data/299150235147/calib_ir.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | cameraMatrix: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: d 6 | data: [ 3.6638346288148574e+02, 0., 2.5564531890330468e+02, 0., 7 | 3.6714380707081017e+02, 2.0398020160452000e+02, 0., 0., 1. ] 8 | distortionCoefficients: !!opencv-matrix 9 | rows: 1 10 | cols: 5 11 | dt: d 12 | data: [ 1.0952761399998089e-01, -3.1461678804886056e-01, 13 | -1.2613602146463686e-03, -7.1815538262097348e-04, 14 | 1.2565382780717374e-01 ] 15 | rotation: !!opencv-matrix 16 | rows: 3 17 | cols: 3 18 | dt: d 19 | data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ] 20 | projection: !!opencv-matrix 21 | rows: 4 22 | cols: 4 23 | dt: d 24 | data: [ 3.6677544568158339e+02, 0., 2.5556683247282524e+02, 0., 0., 25 | 3.6781383188070873e+02, 2.0379944675671842e+02, 0., 0., 0., 1., 26 | 0., 0., 0., 0., 1. ] 27 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_bridge/data/299150235147/calib_pose.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | rotation: !!opencv-matrix 3 | rows: 3 4 | cols: 3 5 | dt: d 6 | data: [ 9.9999932279092762e-01, 1.1418659978192800e-03, 7 | -2.2485490683788145e-04, -1.1422648685177200e-03, 8 | 9.9999776014165309e-01, -1.7818368744440499e-03, 9 | 2.2281978425420061e-04, 1.7820925116285419e-03, 10 | 9.9999838724751144e-01 ] 11 | translation: !!opencv-matrix 12 | rows: 3 13 | cols: 1 14 | dt: d 15 | data: [ -5.2017152124108991e-02, -2.4045118112262469e-04, 16 | 2.9454469402941368e-04 ] 17 | essential: !!opencv-matrix 18 | rows: 3 19 | cols: 3 20 | dt: d 21 | data: [ 2.8287077589668930e-07, -2.9497254054031309e-04, 22 | -2.3992596273739050e-04, 3.0613494517488655e-04, 23 | 9.3035707847567608e-05, 5.2017002003497723e-02, 24 | 2.9986838371861500e-04, -5.2016761050028761e-02, 25 | 9.2632013130372647e-05 ] 26 | fundamental: !!opencv-matrix 27 | rows: 3 28 | cols: 3 29 | dt: d 30 | data: [ 1.9731570930693606e-10, -2.0533114366326771e-07, 31 | -1.9484772545527813e-05, 2.1297242074431652e-07, 32 | 6.4589181747782925e-08, 1.3190795684430680e-02, 33 | 1.1203310138770501e-04, -3.8438663277953075e-02, 1. ] 34 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_bridge/include/kinect2_bridge/kinect2_definitions.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2014 University of Bremen, Institute for Artificial Intelligence 3 | * Author: Thiemo Wiedemeyer 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #pragma once 19 | #ifndef __KINECT2_DEFINITIONS_H__ 20 | #define __KINECT2_DEFINITIONS_H__ 21 | 22 | #include 23 | 24 | #define K2_DEFAULT_NS "kinect2" 25 | 26 | #define K2_TF_LINK "_link" 27 | #define K2_TF_RGB_OPT_FRAME "_rgb_optical_frame" 28 | #define K2_TF_IR_OPT_FRAME "_ir_optical_frame" 29 | 30 | #define K2_TOPIC_HD "/hd" 31 | #define K2_TOPIC_QHD "/qhd" 32 | #define K2_TOPIC_SD "/sd" 33 | 34 | #define K2_TOPIC_IMAGE_RECT "_rect" 35 | #define K2_TOPIC_IMAGE_COLOR "/image_color" 36 | #define K2_TOPIC_IMAGE_MONO "/image_mono" 37 | #define K2_TOPIC_IMAGE_DEPTH "/image_depth" 38 | #define K2_TOPIC_IMAGE_IR "/image_ir" 39 | 40 | #define K2_TOPIC_COMPRESSED "/compressed" 41 | #define K2_TOPIC_INFO "/camera_info" 42 | 43 | #define K2_CALIB_COLOR "calib_color.yaml" 44 | #define K2_CALIB_IR "calib_ir.yaml" 45 | #define K2_CALIB_POSE "calib_pose.yaml" 46 | #define K2_CALIB_DEPTH "calib_depth.yaml" 47 | 48 | #define K2_CALIB_CAMERA_MATRIX "cameraMatrix" 49 | #define K2_CALIB_DISTORTION "distortionCoefficients" 50 | #define K2_CALIB_ROTATION "rotation" 51 | #define K2_CALIB_PROJECTION "projection" 52 | #define K2_CALIB_TRANSLATION "translation" 53 | #define K2_CALIB_ESSENTIAL "essential" 54 | #define K2_CALIB_FUNDAMENTAL "fundamental" 55 | #define K2_CALIB_DEPTH_SHIFT "depthShift" 56 | 57 | #endif //__KINECT2_DEFINITIONS_H__ 58 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_bridge/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Kinect2Bridge nodelet 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kinect2_bridge 4 | 0.0.1 5 | The kinect2_bridge package 6 | 7 | Thiemo Wiedemeyer 8 | 9 | Apache 2.0 10 | 11 | catkin 12 | 13 | roscpp 14 | rostime 15 | tf 16 | std_msgs 17 | sensor_msgs 18 | message_filters 19 | compressed_depth_image_transport 20 | kinect2_registration 21 | nodelet 22 | cv_bridge 23 | 24 | message_runtime 25 | roscpp 26 | rostime 27 | tf 28 | std_msgs 29 | sensor_msgs 30 | compressed_depth_image_transport 31 | kinect2_registration 32 | nodelet 33 | depth_image_proc 34 | cv_bridge 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_calibration/include/kinect2_calibration/kinect2_calibration_definitions.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2014 University of Bremen, Institute for Artificial Intelligence 3 | * Author: Thiemo Wiedemeyer 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #pragma once 19 | #ifndef __KINECT2_CALIBRATION_DEFINITIONS_H__ 20 | #define __KINECT2_CALIBRATION_DEFINITIONS_H__ 21 | 22 | #define CALIB_FILE_EXT ".png" 23 | #define CALIB_FILE_COLOR "_color" CALIB_FILE_EXT 24 | #define CALIB_FILE_IR "_ir" CALIB_FILE_EXT 25 | #define CALIB_FILE_IR_GREY "_grey_ir" CALIB_FILE_EXT 26 | #define CALIB_FILE_DEPTH "_depth" CALIB_FILE_EXT 27 | 28 | #define CALIB_POINTS_COLOR "_color_points.yaml" 29 | #define CALIB_POINTS_IR "_ir_points.yaml" 30 | 31 | #define CALIB_SYNC "_sync" 32 | #define CALIB_SYNC_COLOR CALIB_SYNC CALIB_FILE_COLOR 33 | #define CALIB_SYNC_IR CALIB_SYNC CALIB_FILE_IR 34 | #define CALIB_SYNC_IR_GREY CALIB_SYNC CALIB_FILE_IR_GREY 35 | 36 | #endif //__KINECT2_CALIBRATION_DEFINITIONS_H__ 37 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_calibration/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kinect2_calibration 4 | 0.0.1 5 | The kinect2_calibration package 6 | 7 | Thiemo Wiedemeyer 8 | 9 | Apache 2.0 10 | 11 | catkin 12 | 13 | roscpp 14 | rostime 15 | std_msgs 16 | sensor_msgs 17 | message_filters 18 | image_transport 19 | compressed_image_transport 20 | compressed_depth_image_transport 21 | kinect2_bridge 22 | cv_bridge 23 | 24 | message_runtime 25 | roscpp 26 | rostime 27 | std_msgs 28 | sensor_msgs 29 | message_filters 30 | image_transport 31 | compressed_image_transport 32 | compressed_depth_image_transport 33 | cv_bridge 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_calibration/patterns/chess5x7x0.03.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/iai_kinect2/kinect2_calibration/patterns/chess5x7x0.03.pdf -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_calibration/patterns/chess7x9x0.025.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/iai_kinect2/kinect2_calibration/patterns/chess7x9x0.025.pdf -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_calibration/patterns/chess9x11x0.02.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/iai_kinect2/kinect2_calibration/patterns/chess9x11x0.02.pdf -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_calibration/scripts/convert_calib_pose_to_urdf_format.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import argparse 5 | import numpy as np 6 | import os 7 | import tempfile 8 | import tf 9 | import yaml 10 | 11 | def read_calib_pose(fname): 12 | tmp = tempfile.TemporaryFile() 13 | # we need modify original yaml file because yaml.load(fname) simply will fail 14 | with open(fname, "r") as f: 15 | reader = f.readlines() 16 | for row in reader: 17 | if row[0] == "%": 18 | # remove first line: "%YAML:1.0" 19 | continue 20 | if row.find("!!") != -1: 21 | # remove "!!opencv-matrix" 22 | row = row[:row.find("!!")] + os.linesep 23 | tmp.write(row) 24 | tmp.seek(0) 25 | data = yaml.load(tmp) 26 | return data 27 | 28 | def calc_xyz_rpy(data): 29 | mat = np.resize(data["rotation"]["data"], (3, 3)) 30 | xyz = data["translation"]["data"] 31 | rpy = tf.transformations.euler_from_matrix(mat) 32 | return xyz, rpy 33 | 34 | def print_urdf(xyz, rpy): 35 | print(""" 36 | 37 | 38 | 39 | 40 | 41 | """.format(x=xyz[0], y=xyz[1], z=xyz[2], 42 | roll=rpy[0], pitch=rpy[1], yaw=rpy[2])) 43 | 44 | if __name__ == "__main__": 45 | parser = argparse.ArgumentParser(description='calculate transform from kinect2_rgb_optical_frame to kinect2_ir_optical_frame') 46 | parser.add_argument('-f', type=str, help='path to calib_pose.yaml', metavar='file', required=True) 47 | args = parser.parse_args() 48 | data = read_calib_pose(args.f) 49 | xyz, rpy = calc_xyz_rpy(data) 50 | print_urdf(xyz, rpy) 51 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_registration/README.md: -------------------------------------------------------------------------------- 1 | # Kinect2 Registration 2 | 3 | ## Maintainer 4 | 5 | - [Thiemo Wiedemeyer](https://ai.uni-bremen.de/team/thiemo_wiedemeyer) <>, [Institute for Artificial Intelligence](http://ai.uni-bremen.de/), University of Bremen 6 | 7 | *Note:* ***Please use the GitHub issues*** *for questions and problems regarding the iai_kinect2 package and its components.* ***Do not write emails.*** 8 | 9 | ## Description 10 | 11 | This is a library for projecting the depth image obtained by Kinect like sensors to a color image. It has a OpenCL implementation for registering the depth image, to reduce CPU load. 12 | 13 | ## Dependencies 14 | 15 | - ROS Hydro/Indigo 16 | - OpenCV 17 | - Eigen (optional, but recommended) 18 | - OpenCL (optional, but recommended) 19 | 20 | At least one of OpenCL or Eigen has to be installed. If OpenCL is not installed the CPU will be used. For optimal performance OpenCL is recommended. 21 | 22 | *for the ROS packages look at the package.xml* 23 | 24 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_registration/cmake/CheckOpenCLICDLoader.cmake: -------------------------------------------------------------------------------- 1 | INCLUDE(CheckCXXSourceCompiles) 2 | INCLUDE(CheckCSourceCompiles) 3 | 4 | SET(CMAKE_REQUIRED_INCLUDES "${MY_DIR}/include/internal" ${OpenCL_INCLUDE_DIRS}) 5 | SET(CMAKE_REQUIRED_LIBRARIES ${OpenCL_LIBRARIES}) 6 | CHECK_C_SOURCE_COMPILES(" 7 | #include 8 | int main() { 9 | clGetPlatformIDs(0, 0, 0); 10 | return 0; 11 | }" OpenCL_C_WORKS) 12 | CHECK_CXX_SOURCE_COMPILES(" 13 | #include 14 | int main() { 15 | cl::Context context; 16 | cl::Platform platform; 17 | cl::Device device; 18 | return 0; 19 | }" OpenCL_CXX_WORKS) 20 | SET(CMAKE_REQUIRED_INCLUDES) 21 | SET(CMAKE_REQUIRED_LIBRARIES) 22 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_registration/cmake/kinect2_registration.cmake.in: -------------------------------------------------------------------------------- 1 | set(DEPTH_REG_CPU "@DEPTH_REG_CPU@") 2 | set(DEPTH_REG_OPENCL "@DEPTH_REG_OPENCL@") 3 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_registration/include/kinect2_registration/kinect2_console.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2015 University of Bremen, Institute for Artificial Intelligence 3 | * Author: Thiemo Wiedemeyer 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #pragma once 19 | #ifndef __KINECT2_CONSOLE_H__ 20 | #define __KINECT2_CONSOLE_H__ 21 | 22 | #include 23 | #include 24 | 25 | // Set this to '0' to disable the extended colored output 26 | #define EXTENDED_OUTPUT 1 27 | 28 | #if EXTENDED_OUTPUT 29 | 30 | #define NO_COLOR "\033[0m" 31 | #define FG_BLACK "\033[30m" 32 | #define FG_RED "\033[31m" 33 | #define FG_GREEN "\033[32m" 34 | #define FG_YELLOW "\033[33m" 35 | #define FG_BLUE "\033[34m" 36 | #define FG_MAGENTA "\033[35m" 37 | #define FG_CYAN "\033[36m" 38 | 39 | const std::string getFunctionName(const std::string &name); 40 | #define OUT_AUX(FUNC_COLOR, MSG_COLOR, STREAM, MSG) STREAM(FUNC_COLOR "[" << getFunctionName(__PRETTY_FUNCTION__) << "] " MSG_COLOR << MSG << NO_COLOR) 41 | 42 | #define OUT_DEBUG(msg) OUT_AUX(FG_BLUE, NO_COLOR, ROS_DEBUG_STREAM, msg) 43 | #define OUT_INFO(msg) OUT_AUX(FG_GREEN, NO_COLOR, ROS_INFO_STREAM, msg) 44 | #define OUT_WARN(msg) OUT_AUX(FG_YELLOW, FG_YELLOW, ROS_WARN_STREAM, msg) 45 | #define OUT_ERROR(msg) OUT_AUX(FG_RED, FG_RED, ROS_ERROR_STREAM, msg) 46 | 47 | #else 48 | 49 | #define NO_COLOR "" 50 | #define FG_BLACK "" 51 | #define FG_RED "" 52 | #define FG_GREEN "" 53 | #define FG_YELLOW "" 54 | #define FG_BLUE "" 55 | #define FG_MAGENTA "" 56 | #define FG_CYAN "" 57 | 58 | #define OUT_DEBUG(msg) ROS_DEBUG_STREAM(msg) 59 | #define OUT_INFO(msg) ROS_INFO_STREAM(msg) 60 | #define OUT_WARN(msg) ROS_WARN_STREAM(msg) 61 | #define OUT_ERROR(msg) ROS_WARN_STREAM(msg) 62 | 63 | #endif 64 | 65 | #endif //__KINECT2_CONSOLE_H__ 66 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_registration/include/kinect2_registration/kinect2_registration.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2014 University of Bremen, Institute for Artificial Intelligence 3 | * Author: Thiemo Wiedemeyer 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #pragma once 19 | #ifndef __KINECT2_REGISTRATION_H__ 20 | #define __KINECT2_REGISTRATION_H__ 21 | 22 | #include 23 | 24 | #include 25 | 26 | class DepthRegistration 27 | { 28 | public: 29 | enum Method 30 | { 31 | DEFAULT = 0, 32 | CPU, 33 | OPENCL 34 | }; 35 | 36 | protected: 37 | cv::Mat cameraMatrixRegistered, cameraMatrixDepth, rotation, translation, mapX, mapY; 38 | cv::Size sizeRegistered, sizeDepth; 39 | float zNear, zFar; 40 | 41 | DepthRegistration(); 42 | 43 | virtual bool init(const int deviceId) = 0; 44 | 45 | public: 46 | virtual ~DepthRegistration(); 47 | 48 | bool init(const cv::Mat &cameraMatrixRegistered, const cv::Size &sizeRegistered, const cv::Mat &cameraMatrixDepth, const cv::Size &sizeDepth, 49 | const cv::Mat &distortionDepth, const cv::Mat &rotation, const cv::Mat &translation, 50 | const float zNear = 0.5f, const float zFar = 12.0f, const int deviceId = -1); 51 | 52 | virtual bool registerDepth(const cv::Mat &depth, cv::Mat ®istered) = 0; 53 | 54 | static DepthRegistration *New(Method method = DEFAULT); 55 | }; 56 | 57 | #endif //__KINECT2_REGISTRATION_H__ 58 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_registration/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kinect2_registration 4 | 0.0.1 5 | The kinect2_registration package 6 | 7 | Thiemo Wiedemeyer 8 | 9 | Apache 2.0 10 | 11 | catkin 12 | 13 | roscpp 14 | cv_bridge 15 | 16 | roscpp 17 | cv_bridge 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_registration/src/depth_registration_cpu.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2014 University of Bremen, Institute for Artificial Intelligence 3 | * Author: Thiemo Wiedemeyer 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #pragma once 19 | #ifndef __DEPTH_REGISTRATION_CPU_H__ 20 | #define __DEPTH_REGISTRATION_CPU_H__ 21 | 22 | #include 23 | 24 | #include 25 | 26 | class DepthRegistrationCPU : public DepthRegistration 27 | { 28 | private: 29 | cv::Mat lookupX, lookupY; 30 | Eigen::Matrix4d proj; 31 | double fx, fy, cx, cy; 32 | 33 | public: 34 | DepthRegistrationCPU(); 35 | 36 | ~DepthRegistrationCPU(); 37 | 38 | bool init(const int deviceId); 39 | 40 | bool registerDepth(const cv::Mat &depth, cv::Mat ®istered); 41 | 42 | private: 43 | void createLookup(); 44 | 45 | uint16_t interpolate(const cv::Mat &in, const float &x, const float &y) const; 46 | 47 | void remapDepth(const cv::Mat &depth, cv::Mat &scaled) const; 48 | void projectDepth(const cv::Mat &scaled, cv::Mat ®istered) const; 49 | }; 50 | 51 | #endif //__DEPTH_REGISTRATION_CPU_H__ 52 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_registration/src/depth_registration_opencl.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2014 University of Bremen, Institute for Artificial Intelligence 3 | * Author: Thiemo Wiedemeyer 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #pragma once 19 | #ifndef __DEPTH_REGISTRATION_OPENCL_H__ 20 | #define __DEPTH_REGISTRATION_OPENCL_H__ 21 | 22 | #include 23 | 24 | class DepthRegistrationOpenCL : public DepthRegistration 25 | { 26 | private: 27 | struct OCLData; 28 | 29 | OCLData *data; 30 | 31 | public: 32 | DepthRegistrationOpenCL(); 33 | 34 | ~DepthRegistrationOpenCL(); 35 | 36 | bool init(const int deviceId); 37 | 38 | bool registerDepth(const cv::Mat &depth, cv::Mat ®istered); 39 | 40 | private: 41 | void generateOptions(std::string &options) const; 42 | 43 | bool readProgram(std::string &source) const; 44 | }; 45 | 46 | #endif //__DEPTH_REGISTRATION_OPENCL_H__ 47 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_viewer/README.md: -------------------------------------------------------------------------------- 1 | # Kinect2 Viewer 2 | 3 | ## Maintainer 4 | 5 | - [Thiemo Wiedemeyer](https://ai.uni-bremen.de/team/thiemo_wiedemeyer) <>, [Institute for Artificial Intelligence](http://ai.uni-bremen.de/), University of Bremen 6 | 7 | *Note:* ***Please use the GitHub issues*** *for questions and problems regarding the iai_kinect2 package and its components.* ***Do not write emails.*** 8 | 9 | ## Description 10 | 11 | This is a simple viewer for the combined color an depth image provided by Kinect like depth sensors. 12 | 13 | It just listens to two ROS topics and displays a the color with the overlayed colored depth image or a registered point cloud. 14 | 15 | ## Dependencies 16 | 17 | - ROS Hydro/Indigo 18 | - OpenCV 19 | - PCL 20 | 21 | *for the ROS packages look at the package.xml* 22 | 23 | ## Usage 24 | 25 | ``` 26 | kinect2_viewer [options] 27 | name: 'any string' equals to the kinect2_bridge topic base name 28 | mode: 'qhd', 'hd', 'sd' or 'ir' 29 | visualization: 'image', 'cloud' or 'both' 30 | options: 31 | 'compressed' use compressed instead of raw topics 32 | 'approx' use approximate time synchronization 33 | ``` 34 | 35 | Example: `rosrun kinect2_viewer kinect2_viewer sd cloud` 36 | 37 | ## Key bindings 38 | 39 | Windows: 40 | - `ESC`, `q`: Quit 41 | - `SPACE`, `s`: Save the current image, cloud in the current directory 42 | 43 | Terminal: 44 | - `CRTL`+`c`: Quit 45 | -------------------------------------------------------------------------------- /src/iai_kinect2/kinect2_viewer/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kinect2_viewer 4 | 0.0.1 5 | The kinect2_viewer package 6 | 7 | Thiemo Wiedemeyer 8 | 9 | Apache 2.0 10 | 11 | catkin 12 | 13 | roscpp 14 | rostime 15 | std_msgs 16 | sensor_msgs 17 | message_filters 18 | image_transport 19 | compressed_image_transport 20 | compressed_depth_image_transport 21 | kinect2_bridge 22 | libpcl-all-dev 23 | cv_bridge 24 | 25 | message_runtime 26 | roscpp 27 | rostime 28 | std_msgs 29 | sensor_msgs 30 | message_filters 31 | image_transport 32 | compressed_image_transport 33 | compressed_depth_image_transport 34 | libpcl-all-dev 35 | libpcl-all 36 | cv_bridge 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /src/itheima_msgs/action/ArmWork.action: -------------------------------------------------------------------------------- 1 | int32 type 2 | --- 3 | string result 4 | --- 5 | int32 state 6 | string desc -------------------------------------------------------------------------------- /src/itheima_msgs/action/CarMove.action: -------------------------------------------------------------------------------- 1 | int32 type 2 | --- 3 | string result 4 | --- 5 | int32 progress -------------------------------------------------------------------------------- /src/itheima_msgs/action/LaserMark.action: -------------------------------------------------------------------------------- 1 | int32 type 2 | string name 3 | int32 id 4 | --- 5 | string result 6 | --- 7 | int32 progress -------------------------------------------------------------------------------- /src/itheima_msgs/msg/AssemblyIR.msg: -------------------------------------------------------------------------------- 1 | bool ir_1 2 | bool ir_2 -------------------------------------------------------------------------------- /src/itheima_msgs/msg/AssemblyLine.msg: -------------------------------------------------------------------------------- 1 | bool line_1 2 | bool line_2 3 | bool line_3 4 | bool line_4 -------------------------------------------------------------------------------- /src/itheima_msgs/msg/BoxPose.msg: -------------------------------------------------------------------------------- 1 | int32[] center 2 | int32[] vect 3 | int32 type 4 | -------------------------------------------------------------------------------- /src/itheima_msgs/msg/Order.msg: -------------------------------------------------------------------------------- 1 | int32 STATE_WAITING = 0 2 | int32 STATE_SCAN_FEEDING = 1 3 | int32 STATE_GRIP_FEEDING = 2 4 | int32 STATE_TRANSPORT = 3 5 | int32 STATE_MARKING = 4 6 | int32 STATE_SCAN_BLANKING = 5 7 | int32 STATE_GRIP_BLANKING = 6 8 | int32 STATE_COMPLETED = 7 9 | 10 | int32 id 11 | int32 state 12 | int32 type 13 | string name 14 | string scan_id -------------------------------------------------------------------------------- /src/itheima_msgs/srv/AssemblyLineCtrl.srv: -------------------------------------------------------------------------------- 1 | uint8 LINE_1 = 1 2 | uint8 LINE_2 = 2 3 | uint8 LINE_3 = 3 4 | uint8 LINE_4 = 4 5 | uint8 LINE_ALL = 5 6 | bool STATE_START = 1 7 | bool STATE_STOP = 0 8 | 9 | uint8 line 10 | bool state 11 | --- 12 | bool result -------------------------------------------------------------------------------- /src/itheima_msgs/srv/GetBoxPoses.srv: -------------------------------------------------------------------------------- 1 | --- 2 | BoxPose[] line_poses 3 | BoxPose[] agv_poses 4 | -------------------------------------------------------------------------------- /src/itheima_msgs/srv/GetLaserBoxLocator.srv: -------------------------------------------------------------------------------- 1 | --- 2 | float32 center_offset_x 3 | float32 center_offset_y 4 | float32 angle_degree -------------------------------------------------------------------------------- /src/joystick_drivers/.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *.swp 3 | -------------------------------------------------------------------------------- /src/joystick_drivers/README.md: -------------------------------------------------------------------------------- 1 | # ROS Joystick Drivers Stack # 2 | 3 | [![CircleCI](https://circleci.com/gh/ros-drivers/joystick_drivers.svg?style=svg)](https://circleci.com/gh/ros-drivers/joystick_drivers) 4 | 5 | A simple set of nodes for supporting various types of joystick inputs and producing ROS messages from the underlying OS messages. 6 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package joy 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.13.0 (2019-06-24) 6 | ------------------- 7 | * Merge pull request `#120 `_ from furushchev/remap 8 | add joy_remap and its sample 9 | * Merge pull request `#128 `_ from ros-drivers/fix/tab_errors 10 | Cleaning up Python indentation. 11 | * Merge pull request `#111 `_ from matt-attack/indigo-devel 12 | Add Basic Force Feedback Support 13 | * Merge pull request `#126 `_ from clalancette/minor-formatting 14 | * Put brackets around ROS\_* macros. 15 | In some circumstances they may be defined to empty, so we need 16 | to have brackets to ensure that they are syntactically valid. 17 | Signed-off-by: Chris Lalancette 18 | * Merge pull request `#122 `_ from lbucklandAS/fix-publish-timestamp 19 | Add timestamp to all joy messages 20 | * Change error messages and set ps3 as default controller 21 | * Better handle device loss 22 | Allow for loss and redetection of device with force feedback 23 | * Add basic force feedback over usb 24 | Addresses `#89 `_ 25 | * Contributors: Chris Lalancette, Furushchev, Joshua Whitley, Lucas Buckland, Matthew, Matthew Bries 26 | 27 | 1.12.0 (2018-06-11) 28 | ------------------- 29 | * Update timestamp when using autorepeat_rate 30 | * Added dev_name parameter to select joystick by name 31 | * Added Readme for joy package with description of new device name parameter 32 | * Fixed numerous outstanding PRs. 33 | * Added sticky buttons 34 | * Changed package xml to format 2 35 | * Fixed issue when the joystick data did not got send until changed. 36 | * Changed messaging to better reflect what the script is doing 37 | * Contributors: Dino Hüllmann, Jonathan Bohren, Joshua Whitley, Miklos Marton, Naoki Mizuno, jprod123, psimona 38 | 39 | 1.11.0 (2017-02-10) 40 | ------------------- 41 | * fixed joy/Cmakelists for osx 42 | * Update dependencies to remove warnings 43 | * Contributors: Marynel Vazquez, Mark D Horn 44 | 45 | 1.10.1 (2015-05-24) 46 | ------------------- 47 | * Remove stray architechture_independent flags 48 | * Contributors: Jonathan Bohren, Scott K Logan 49 | 50 | 1.10.0 (2014-06-26) 51 | ------------------- 52 | * First indigo release 53 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(joy) 3 | 4 | set(CMAKE_CXX_STANDARD 11) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | 7 | # Load catkin and all dependencies required for this package 8 | set(CATKIN_DEPS roscpp diagnostic_updater sensor_msgs roslint) 9 | find_package(catkin REQUIRED ${CATKIN_DEPS}) 10 | 11 | roslint_cpp() 12 | 13 | catkin_package(DEPENDS sensor_msgs) 14 | 15 | # Look for 16 | include(CheckIncludeFiles) 17 | check_include_files(linux/joystick.h HAVE_LINUX_JOYSTICK_H) 18 | 19 | if(CATKIN_ENABLE_TESTING) 20 | roslint_add_test() 21 | endif() 22 | 23 | if(HAVE_LINUX_JOYSTICK_H) 24 | include_directories(msg/cpp ${catkin_INCLUDE_DIRS}) 25 | add_executable(joy_node src/joy_node.cpp) 26 | target_link_libraries(joy_node ${catkin_LIBRARIES}) 27 | 28 | # Install targets 29 | install(TARGETS joy_node 30 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 31 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 32 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 33 | else() 34 | message("Warning: no ; won't build joy node") 35 | endif() 36 | 37 | install(DIRECTORY migration_rules scripts config launch 38 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 39 | USE_SOURCE_PERMISSIONS) 40 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/config/ps4joy.yaml: -------------------------------------------------------------------------------- 1 | mappings: 2 | axes: 3 | - axes[0] 4 | - axes[1] 5 | - axes[2] 6 | - axes[5] 7 | - axes[6] * 2.0 8 | - axes[7] * 2.0 9 | - axes[8] * -2.0 10 | - 0.0 11 | - max(axes[10], 0.0) * -1.0 12 | - min(axes[9], 0.0) 13 | - min(axes[10], 0.0) 14 | - max(axes[9], 0.0) * -1.0 15 | - (axes[3] - 1.0) * 0.5 16 | - (axes[4] - 1.0) * 0.5 17 | - buttons[4] * -1.0 18 | - buttons[5] * -1.0 19 | - buttons[3] * -1.0 20 | - buttons[2] * -1.0 21 | - buttons[1] * -1.0 22 | - buttons[0] * -1.0 23 | - 0.0 24 | - 0.0 25 | - 0.0 26 | - 0.0 27 | - 0.0 28 | - 0.0 29 | - 0.0 30 | buttons: 31 | - buttons[8] 32 | - buttons[10] 33 | - buttons[11] 34 | - buttons[9] 35 | - max(axes[10], 0.0) 36 | - min(axes[9], 0.0) * -1.0 37 | - min(axes[10], 0.0) * -1.0 38 | - max(axes[9], 0.0) 39 | - buttons[6] 40 | - buttons[7] 41 | - buttons[4] 42 | - buttons[5] 43 | - buttons[3] 44 | - buttons[2] 45 | - buttons[1] 46 | - buttons[0] 47 | - buttons[12] 48 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/launch/ps4joy.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/migration_rules/Joy.bmr: -------------------------------------------------------------------------------- 1 | class update_joy_Joy_e3ef016fcdf22397038b36036c66f7c8(MessageUpdateRule): 2 | old_type = "joy/Joy" 3 | old_full_text = """ 4 | float32[] axes 5 | int32[] buttons 6 | """ 7 | 8 | new_type = "sensor_msgs/Joy" 9 | new_full_text = """ 10 | # Reports the state of a joysticks axes and buttons. 11 | Header header # timestamp in the header is the time the data is received from the joystick 12 | float32[] axes # the axes measurements from a joystick 13 | int32[] buttons # the buttons measurements from a joystick 14 | 15 | ================================================================================ 16 | MSG: std_msgs/Header 17 | # Standard metadata for higher-level stamped data types. 18 | # This is generally used to communicate timestamped data 19 | # in a particular coordinate frame. 20 | # 21 | # sequence ID: consecutively increasing ID 22 | uint32 seq 23 | #Two-integer timestamp that is expressed as: 24 | # * stamp.secs: seconds (stamp_secs) since epoch 25 | # * stamp.nsecs: nanoseconds since stamp_secs 26 | # time-handling sugar is provided by the client library 27 | time stamp 28 | #Frame this data is associated with 29 | # 0: no frame 30 | # 1: global frame 31 | string frame_id 32 | """ 33 | 34 | order = 0 35 | migrated_types = [] 36 | 37 | valid = True 38 | 39 | def update(self, old_msg, new_msg): 40 | #No matching field name in old message 41 | new_msg.header = self.get_new_class('Header')() 42 | new_msg.axes = old_msg.axes 43 | new_msg.buttons = old_msg.buttons 44 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/package.xml: -------------------------------------------------------------------------------- 1 | 2 | joy 3 | 1.13.0 4 | BSD 5 | 6 | 7 | ROS driver for a generic Linux joystick. 8 | The joy package contains joy_node, a node that interfaces a 9 | generic Linux joystick to ROS. This node publishes a "Joy" 10 | message, which contains the current state of each one of the 11 | joystick's buttons and axes. 12 | 13 | 14 | Jonathan Bohren 15 | Morgan Quigley 16 | Brian Gerkey 17 | Kevin Watts 18 | Blaise Gassend 19 | 20 | http://www.ros.org/wiki/joy 21 | https://github.com/ros-drivers/joystick_drivers 22 | https://github.com/ros-drivers/joystick_drivers/issues 23 | 24 | catkin 25 | roslint 26 | 27 | diagnostic_updater 28 | joystick 29 | roscpp 30 | sensor_msgs 31 | 32 | rosbag 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/test/saved/Joy.saved: -------------------------------------------------------------------------------- 1 | [joy/Joy]: 2 | float32[] axes 3 | int32[] buttons 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ps3joy 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.13.0 (2019-06-24) 6 | ------------------- 7 | * Merge pull request `#128 `_ from ros-drivers/fix/tab_errors 8 | * Cleaning up Python indentation. 9 | * Merge pull request `#123 `_ from cclauss/modernize-python2-code 10 | * Modernize Python 2 code to get ready for Python 3 11 | * Merge branch 'master' into indigo-devel 12 | * Contributors: Joshua Whitley, Matthew, cclauss 13 | 14 | 1.12.0 (2018-06-11) 15 | ------------------- 16 | * Addressed numerous outstanding PRs. 17 | * Created bluetooth_devices.md 18 | * Created testing guide for ps3joy. 19 | * Create procedure_test.md 20 | * Let ps3joy_node not quit on inactivity-timeout. 21 | * Refine diagnostics message usage in ps3joy_node 22 | * Improve ps3joy_node with rospy.init_node and .is_shutdown 23 | * Remove quit on failed root level check, part one of issue `#53 `_ 24 | * Create README 25 | * Changed package xml to format 2 26 | * Contributors: Alenso Labady, Felix Kolbe, Jonathan Bohren, alab288, jprod123 27 | 28 | 1.11.0 (2017-02-10) 29 | ------------------- 30 | * Update dependencies to remove warnings 31 | * Contributors: Mark D Horn 32 | 33 | 1.10.1 (2015-05-24) 34 | ------------------- 35 | * Remove stray architechture_independent flags 36 | * Contributors: Jonathan Bohren, Scott K Logan 37 | 38 | 1.10.0 (2014-06-26) 39 | ------------------- 40 | * First indigo reelase 41 | * Update ps3joy/package.xml URLs with github user ros to ros-drivers 42 | * Prompt for sudo password when required 43 | * Contributors: Felix Kolbe, Jonathan Bohren, dawonn 44 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ps3joy) 3 | 4 | find_package(PkgConfig REQUIRED) 5 | pkg_search_module(LIBUSB REQUIRED libusb) 6 | 7 | if(LIBUSB_FOUND) 8 | include_directories(SYSTEM ${LIBUSB_INCLUDE_DIRS}) 9 | link_directories(${LIBUSB_LIBRARY_DIRS}) 10 | else() 11 | message( FATAL_ERROR "Failed to find libusb" ) 12 | endif() 13 | 14 | # Load catkin and all dependencies required for this package 15 | set(CATKIN_DEPS diagnostic_msgs sensor_msgs rospy rosgraph roslint) 16 | find_package(catkin REQUIRED COMPONENTS ${CATKIN_DEPS}) 17 | catkin_package(CATKIN_DEPENDS diagnostic_msgs sensor_msgs) 18 | 19 | roslint_python( 20 | scripts/ps3joy.py 21 | scripts/ps3joy_node.py 22 | scripts/ps3joysim.py 23 | ) 24 | 25 | include_directories(${catkin_INCLUDE_DIRS}) 26 | add_executable(sixpair src/sixpair.c) 27 | target_link_libraries(sixpair -lusb ${catkin_LIBRARIES}) 28 | 29 | if(CATKIN_ENABLE_TESTING) 30 | roslint_add_test() 31 | endif() 32 | 33 | # Install targets 34 | install(TARGETS sixpair 35 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 36 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 37 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 38 | ) 39 | 40 | install(DIRECTORY launch 41 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 42 | ) 43 | 44 | install(FILES diagnostics.yaml 45 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 46 | ) 47 | 48 | install(PROGRAMS scripts/ps3joy.py scripts/ps3joy_node.py scripts/ps3joysim.py 49 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 50 | ) 51 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/diagnostics.yaml: -------------------------------------------------------------------------------- 1 | type: AnalyzerGroup 2 | pub_rate: 1.0 # Optional 3 | base_path: '' # Optional, prepended to all diagnostic output 4 | analyzers: 5 | PS3State: 6 | type: diagnostic_aggregator/GenericAnalyzer 7 | path: 'PS3 State' 8 | timeout: 5.0 9 | startswith: ['Battery', 'Charging State', 'Connection', 'ps3_joy'] 10 | remove_prefix: 'ps3_joy' 11 | 12 | 13 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/doc/bluetooth_devices.md: -------------------------------------------------------------------------------- 1 | 2 | ## Bluetooth Device Compatibility 3 | 4 | This driver works with most 2.x Bluetooth adapters.This driver should also work with 3.0 and 4.0 Bluetooth adapters. 5 | Please report other compatible and incompatible Bluetooth adapters through a pull request to this page. 6 | 7 | ### Adapters that are known to work 8 | 9 | * Cambridge Silicon Radio, Ltd Bluetooth Dongle (Bluetooth 4.0) 10 | 11 | ### Adapters that are known not to work 12 | 13 | * Linksys USBBT100 version 2 (Bluetooth 1.1) 14 | * USB device 0a12:0x0001 15 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/doc/troubleshooting.md: -------------------------------------------------------------------------------- 1 | ## Troubleshooting ## 2 | ------------------------- 3 | #### Issues pairing the PS3 joystick #### 4 | When pairing your joystick via bluetooth, you may recieve the following message on 5 | your terminal: 6 | ``` 7 | Current Bluetooth master: 00:15:83:ed:3f:21 8 | Unable to retrieve local bd_addr from `hcitool dev`. 9 | Please enable Bluetooth or specify an address manually. 10 | ``` 11 | This would indicate that your bluetooth is disabled. To enable your bluetooth, try the 12 | following: 13 | 14 | 1. Check the status of your bluetooth by entering the following: 15 | ``` 16 | sudo systemctl status bluetooth 17 | ``` 18 | You may see something like this: 19 | 20 | ``` 21 | ● bluetooth.service - Bluetooth service 22 | Loaded: loaded (/lib/systemd/system/bluetooth.service; disabled; vendor preset: enabled) 23 | Active: inactive (dead) 24 | Docs: man:bluetoothd(8) 25 | ``` 26 | 27 | If you do, that means your bluetooth service is disabled. Turn enable it enter 28 | ``` 29 | sudo systemctl start bluetooth 30 | sudo systemctl status bluetooth 31 | ``` 32 | After running these commands your bluetooth service should be up and running: 33 | 34 | ``` 35 | ● bluetooth.service - Bluetooth service 36 | Loaded: loaded (/lib/systemd/system/bluetooth.service; disabled; vendor preset: enabled) 37 | Active: active (running) since Thu 2017-06-29 16:21:43 EDT; 16s ago 38 | Docs: man:bluetoothd(8) 39 | Main PID: 27362 (bluetoothd) 40 | Status: "Running" 41 | CGroup: /system.slice/bluetooth.service 42 | └─27362 /usr/local/libexec/bluetooth/bluetoothd 43 | ``` 44 | Retry the commands that were mentioned in step 2 for pairing the PS3 joystick. 45 | 46 | 2. Run the following command: 47 | ``` 48 | hciconfig hci0 reset 49 | ``` 50 | followed by: 51 | ``` 52 | sudo bash 53 | rosrun ps3joy sixpair 54 | ``` 55 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/launch/ps3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ps3joy 3 | 1.13.0 4 | BSD 5 | 6 | 7 | Playstation 3 SIXAXIS or DUAL SHOCK 3 joystick driver. 8 | Driver for the Sony PlayStation 3 SIXAXIS or DUAL SHOCK 3 9 | joysticks. In its current state, this driver is not compatible 10 | with the use of other Bluetooth HID devices. The driver listens 11 | for a connection on the HID ports, starts the joystick 12 | streaming data, and passes the data to the Linux uinput device 13 | so that it shows up as a normal joystick. 14 | 15 | 16 | Jonathan Bohren 17 | Blaise Gassend 18 | pascal@pabr.org 19 | Melonee Wise 20 | 21 | http://www.ros.org/wiki/ps3joy 22 | https://github.com/ros-drivers/joystick_drivers 23 | https://github.com/ros-drivers/joystick_drivers/issues 24 | 25 | catkin 26 | 27 | roslint 28 | 29 | bluez 30 | diagnostic_msgs 31 | joystick 32 | libusb-dev 33 | python-bluez 34 | rosgraph 35 | rospy 36 | sensor_msgs 37 | 38 | -------------------------------------------------------------------------------- /src/joystick_drivers/spacenav_node/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package spacenav_node 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.13.0 (2019-06-24) 6 | ------------------- 7 | 8 | 1.12.0 (2018-06-11) 9 | ------------------- 10 | * Adding tested spacenav scaling 11 | * Added README 12 | * Addressed numerous outstanding PRs. 13 | * Changed package xml to format 2 14 | * Reduce the number of scale params in spacenav_node 15 | * Add a scale parameter to each axis in spacenav_node 16 | Do not apply the scale to the joy topic. 17 | * Contributors: Gaël Ecorchard, Jonathan Bohren, jprod123 18 | 19 | 1.11.0 (2017-02-10) 20 | ------------------- 21 | * Update dependencies to remove warnings 22 | * Contributors: Mark D Horn 23 | 24 | 1.10.1 (2015-05-24) 25 | ------------------- 26 | * Add full_scale parameter and apply to offset 27 | * Remove stray architechture_independent flags 28 | * Contributors: Gaël Ecorchard, Jonathan Bohren, Scott K Logan 29 | 30 | 1.10.0 (2014-06-26) 31 | ------------------- 32 | * First indigo release 33 | -------------------------------------------------------------------------------- /src/joystick_drivers/spacenav_node/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(spacenav_node) 3 | 4 | set(CMAKE_CXX_STANDARD 11) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | 7 | # Load catkin and all dependencies required for this package 8 | set(CATKIN_DEPS roscpp geometry_msgs sensor_msgs roslint) 9 | find_package(catkin REQUIRED ${CATKIN_DEPS}) 10 | catkin_package(DEPENDS geometry_msgs sensor_msgs) 11 | 12 | roslint_cpp() 13 | 14 | include_directories(${catkin_INCLUDE_DIRS}) 15 | add_executable(spacenav_node src/spacenav_node.cpp) 16 | target_link_libraries (spacenav_node spnav X11 ${catkin_LIBRARIES}) 17 | 18 | if(CATKIN_ENABLE_TESTING) 19 | roslint_add_test() 20 | endif() 21 | 22 | # Install targets 23 | install(TARGETS spacenav_node 24 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 25 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 26 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 27 | ) 28 | 29 | install(DIRECTORY launch 30 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 31 | ) 32 | -------------------------------------------------------------------------------- /src/joystick_drivers/spacenav_node/README.md: -------------------------------------------------------------------------------- 1 | # Spacenav Nodes # 2 | ## spacenav_node 3 | ##### Published topics 4 | * `spacenav/offset` (geometry_msgs/Vector3) 5 | 6 | Publishes the linear component of the joystick's position. Approximately normalized to a range of -1 to 1. 7 | * `spacenav/rot_offset`(geometry_msgs/Vector3) 8 | 9 | Publishes the angular component of the joystick's position. Approximately normalized to a range of -1 to 1. 10 | * `spacenav/twist` (geometry_msgs/Twist) 11 | 12 | Combines offset and rot_offset into a single message. 13 | * `spacenav/joy` (sensor_msgs/Joy) 14 | 15 | Outputs the spacenav's six degrees of freedom and its buttons as a joystick message. 16 | ##### Parameters 17 | * `zero_when_static` 18 | 19 | 20 | sets values to zero when the spacenav is not moving 21 | * `static_count_threshold` 22 | 23 | 24 | The number of polls needed to be done before the device is considered "static" 25 | * `static_trans_deadband` 26 | 27 | sets the translational deadband 28 | * `static_rot_deadband` 29 | 30 | sets the rotational deadband 31 | * `linear_scale` 32 | 33 | sets the scale of the linear output 34 | 35 | takes a vector (x, y, z) 36 | * `angular_scale` 37 | 38 | sets the scale of the angular output 39 | 40 | takes a vector (x, y, z) 41 | 42 | The `linear_scale` and `angular_scale` parameters take vector arguements. 43 | The easiest way to set these parameters is with a launch file like so: 44 | ``` 45 | 46 | 47 | [.25, .25, .25] 48 | [.5, .5, .5] 49 | 50 | 51 | ``` 52 | 53 | ### Running the spacenav_node ### 54 | 55 | Running the node is straightforward 56 | ``` 57 | $rosrun spacenav_node spacenav_node 58 | ``` 59 | The node is now publishing to the topics listed above. 60 | #### Examples #### 61 | Viewing the output from the spacenav can be done as follows: 62 | ``` 63 | $ rostopic echo /spacenav/joy 64 | ``` 65 | 66 | -------------------------------------------------------------------------------- /src/joystick_drivers/spacenav_node/launch/classic.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/joystick_drivers/spacenav_node/launch/no_deadband.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/joystick_drivers/spacenav_node/launch/static_deadband.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/joystick_drivers/spacenav_node/package.xml: -------------------------------------------------------------------------------- 1 | 2 | spacenav_node 3 | 1.13.0 4 | BSD 5 | 6 | ROS interface to the 3Dconnexion SpaceNavigator 6DOF joystick. 7 | 8 | 9 | http://www.ros.org/wiki/spacenav_node 10 | https://github.com/ros-drivers/joystick_drivers 11 | https://github.com/ros-drivers/joystick_drivers/issues 12 | 13 | Jonathan Bohren 14 | Stuart Glaser 15 | Blaise Gassend 16 | 17 | catkin 18 | 19 | roslint 20 | 21 | geometry_msgs 22 | libspnav-dev 23 | libx11-dev 24 | roscpp 25 | sensor_msgs 26 | 27 | spacenavd 28 | 29 | -------------------------------------------------------------------------------- /src/joystick_drivers/wiimote/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package wiimote 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.13.0 (2019-06-24) 6 | ------------------- 7 | * Merge pull request `#132 `_ from mistoll/clean_exit 8 | * return instead of shutdown 9 | * Merge pull request `#128 `_ from ros-drivers/fix/tab_errors 10 | * Cleaning up Python indentation. 11 | * Merge pull request `#125 `_ from mistoll/check_connection 12 | * Check if wiimote is still connected. 13 | * Merge pull request `#123 `_ from cclauss/modernize-python2-code 14 | * Modernize Python 2 code to get ready for Python 3 15 | * Merge branch 'master' into indigo-devel 16 | * Contributors: Joshua Whitley, Matthew, Michael Stoll, cclauss 17 | 18 | 1.12.0 (2018-06-11) 19 | ------------------- 20 | * Addressed numerous outstanding PRs. 21 | * Fixed issue when nunchuk wasn't connected 22 | * Update README.md 23 | * Added testing proceedures for wiimote 24 | * Resolved div-by-zero error race condition on startup. 25 | * Added testing proceedures for wiimote 26 | * Fixed issue when nunchuk wasn't connected 27 | * Changed package xml to format 2 28 | * Added wiimote testing script 29 | * Add pairing params to wiimote_node 30 | * bluetooth_addr 31 | * pair_timeout 32 | * Contributors: Jonathan Bohren, Matt Vollrath, jprod123, psimona 33 | 34 | 1.11.0 (2017-02-10) 35 | ------------------- 36 | * Sample Teleop Implementation for Wiimote 37 | * C++ Implementation of Wiimote Controller Node 38 | * Add queue_size to remove ROS Warning 39 | * Update dependencies to remove warnings 40 | * Contributors: Mark D Horn 41 | 42 | 1.10.1 (2015-05-24) 43 | ------------------- 44 | 45 | 1.10.0 (2014-06-26) 46 | ------------------- 47 | * First indigo release 48 | -------------------------------------------------------------------------------- /src/joystick_drivers/wiimote/README.md: -------------------------------------------------------------------------------- 1 | # Wiimote Nodes 2 | 3 | Additional Documentation 4 | * [Testing procedure](doc/testing.md) 5 | * [ROS Wiki](http://wiki.ros.org/wiimote) 6 | 7 | Tutorials 8 | * [Teleop with Wiimote](doc/tutorials/teleop.md) 9 | 10 | ## wiimote_node.py 11 | 12 | Original Python version of the wiimote node. 13 | 14 | ## wiimote_node 15 | 16 | The C++ implementation was designed with focus on reduced resource consumption. 17 | 18 | ### Parameters 19 | 20 | * `bluetooth_addr` [str] - Bluetooth address for pairing. Default: `00:00:00:00:00:00` (first device) 21 | * `pair_timeout` [int] - Pair timeout in seconds. `-1` means never timeout. Default: `5` 22 | 23 | ### Differences from Python Implementation 24 | * Both "/wiimote/nunchuk" and "/wiimote/classic" topics are only published 25 | if the Nunchuk or Classic Controller are connected to the wiimote respectively. 26 | * Wiimote data is only polled from the controller if the data is required 27 | to publish for a topic which has at least one subscriber. 28 | * During Calibration the joysticks (Nunchuk & Classic Controller) are polled 29 | to find the current position which is assumed to be the center. Not all joysticks 30 | rest at the theoretical center position. 31 | * Joysticks (Nunchuk & Classic Controller) minimum and maximum values start at 32 | 80% of the theoretical minimum/maximum values and are dynamically updated as 33 | the joystick is used. The full range of values may not be reached on all joysticks 34 | due to physical limitations. Without this adjust, the full range from -1.0 to 1.0 35 | would not be reached on many joysticks. 36 | * Joysticks (Nunchuk & Classic Controller) center deadzone is scaled based on 37 | the granularity of the joystick; python code assumed the same for all. 38 | * Connecting an accessory (Nunchuk, Classic Controller, or MotionPlus) will 39 | automatically start a new calibration sequence. 40 | -------------------------------------------------------------------------------- /src/joystick_drivers/wiimote/include/wiimote/stat_vector_3d.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Three dimensional statistic vector for use with the 3 | * for ROS Node which interfaces with a wiimote control unit. 4 | * Copyright (c) 2016, Intel Corporation. 5 | * 6 | * This program is free software; you can redistribute it and/or modify it 7 | * under the terms and conditions of the GNU General Public License, 8 | * version 2, as published by the Free Software Foundation. 9 | * 10 | * This program is distributed in the hope it will be useful, but WITHOUT 11 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 12 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 13 | * more details. 14 | */ 15 | 16 | /* 17 | * Initial C++ implementation by 18 | * Mark Horn 19 | * 20 | * Revisions: 21 | * 22 | */ 23 | 24 | #pragma once 25 | #ifndef WIIMOTE_STAT_VECTOR_3D_H 26 | #define WIIMOTE_STAT_VECTOR_3D_H 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | // The vector of 3 values collected to generate: 34 | // mean, standard deviation, and variance. 35 | 36 | typedef std::vector TVectorDouble; 37 | 38 | class StatVector3d 39 | { 40 | public: 41 | StatVector3d(); 42 | StatVector3d(int x, int y, int z); 43 | 44 | void clear(); 45 | 46 | int size(); 47 | void addData(int x, int y, int z); 48 | 49 | TVectorDouble getMeanRaw(); 50 | TVectorDouble getMeanScaled(double scale); 51 | TVectorDouble getVarianceRaw(); 52 | TVectorDouble getVarianceScaled(double scale); 53 | TVectorDouble getStandardDeviationRaw(); 54 | TVectorDouble getStandardDeviationScaled(double scale); 55 | 56 | private: 57 | int count_; 58 | std::vector x_; 59 | std::vector y_; 60 | std::vector z_; 61 | }; 62 | 63 | #endif // WIIMOTE_STAT_VECTOR_3D_H 64 | -------------------------------------------------------------------------------- /src/joystick_drivers/wiimote/include/wiimote/teleop_wiimote.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ROS Node for using a wiimote control unit to direct a robot. 3 | * Copyright (c) 2016, Intel Corporation. 4 | * 5 | * This program is free software; you can redistribute it and/or modify it 6 | * under the terms and conditions of the GNU General Public License, 7 | * version 2, as published by the Free Software Foundation. 8 | * 9 | * This program is distributed in the hope it will be useful, but WITHOUT 10 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 11 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 12 | * more details. 13 | */ 14 | 15 | /* 16 | * Initial C++ implementation by 17 | * Mark Horn 18 | * 19 | * Revisions: 20 | * 21 | */ 22 | 23 | #pragma once 24 | #ifndef WIIMOTE_TELEOP_WIIMOTE_H 25 | #define WIIMOTE_TELEOP_WIIMOTE_H 26 | 27 | #include "ros/ros.h" 28 | #include "sensor_msgs/Joy.h" 29 | #include "wiimote/State.h" 30 | 31 | #include 32 | 33 | // Sane defaults based on the TurtleBot 34 | // TurtleBot maximum speed documented at 25.6"/second ~= 0.65024 m/s 35 | #define DEFAULT_MAX_LINEAR_X 0.65024 // m/s 36 | // TurtleBot maximum angular speed is documented at 180 degrees Pi / second 37 | #define DEFAULT_MAX_ANGULAR_Z M_PI // rad/s 38 | 39 | #define DEFAULT_PERCENT_LINEAR_THROTTLE 0.75 40 | #define DEFAULT_PERCENT_ANGULAR_THROTTLE 0.75 41 | 42 | class TeleopWiimote 43 | { 44 | public: 45 | TeleopWiimote(); 46 | 47 | private: 48 | void rumbleFeedback(int useconds); 49 | void setLEDFeedback(double value); 50 | void joyCallback(const sensor_msgs::Joy::ConstPtr& joy); 51 | void wiimoteStateCallback(const wiimote::State::ConstPtr& wiistate); 52 | 53 | double linear_x_max_velocity_; // m/s 54 | double linear_x_min_velocity_; // m/s 55 | double angular_z_max_velocity_; // rad/s 56 | double angular_z_min_velocity_; // rad/s 57 | 58 | double percent_linear_throttle_; // 0.0 - 1.0 (1.0 = 100%) 59 | double percent_angular_throttle_; // 0.0 - 1.0 (1.0 = 100%) 60 | 61 | ros::Publisher vel_pub_; 62 | ros::Publisher joy_pub_; 63 | ros::Subscriber joy_sub_; 64 | ros::Subscriber wiimote_sub_; 65 | 66 | bool dpad_in_use_ = false; 67 | bool njoy_in_use_ = false; 68 | }; 69 | 70 | #endif // WIIMOTE_TELEOP_WIIMOTE_H 71 | -------------------------------------------------------------------------------- /src/joystick_drivers/wiimote/launch/feedback_test_cpp.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 14 | 15 | -------------------------------------------------------------------------------- /src/joystick_drivers/wiimote/launch/feedback_test_py.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 14 | 15 | -------------------------------------------------------------------------------- /src/joystick_drivers/wiimote/launch/turtlesim_cpp.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 13 | 17 | 18 | 25 | 26 | -------------------------------------------------------------------------------- /src/joystick_drivers/wiimote/launch/turtlesim_py.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 13 | 17 | 18 | 23 | 24 | -------------------------------------------------------------------------------- /src/joystick_drivers/wiimote/msg/IrSourceInfo.msg: -------------------------------------------------------------------------------- 1 | # Sensor data pertaining to the Wiimote infrared camera. 2 | # This message contains data for one of the four infrared 3 | # light sources that the camera detects. 4 | # 5 | # Each light is specified with a 2D position and 6 | # a 'source magnitude' (ir_size). If the x dimension 7 | # is set to INVALID_FLOAT, then no light was detected for 8 | # the respective light. The Wiimote handles up to 9 | # four light sources, and the wiimote_node.py software 10 | # is written to that limit as well. 11 | # 12 | # I am unsure what the 'ir_size' values represent. 13 | # They are described as 'source magnitude' in some places. I 14 | # *assume* this is signal amplitude, but it's unclear. 15 | # Note that current lowest level cwiid driver does not 16 | # seem to pass the ir_size value to the cwiid Wiimote.c. 17 | # For now this size will therefore be set INVALID 18 | 19 | float64 x 20 | float64 y 21 | int64 ir_size 22 | -------------------------------------------------------------------------------- /src/joystick_drivers/wiimote/msg/TimedSwitch.msg: -------------------------------------------------------------------------------- 1 | # TimedSwitch allows sender to: 2 | # o turn a switch on, 3 | # o turn a switch off, and 4 | # o repeat an on/off pattern forever or for a 5 | # given number of times. 6 | # Fields (refer to definitions of constants in the definition body): 7 | # o switch_mode: 8 | # ON: turn on (num_cycles and pulse_pattern fields are ignored) 9 | # OFF: turn off (num_cycles and pulse_pattern fields are ignored) 10 | # NO_CHANGE: leave LED in its current state 11 | # REPEAT: repeat an on/off pattern for as long 12 | # as is indicated in the num_cycles field. The 13 | # pattern is defined in the pulse_pattern field. 14 | # 15 | # o num_cycles: 16 | # n>=0: run the pattern that is defined in pulse_pattern 17 | # n times. 18 | # n==FOREVER: run the pattern that is defined in pulse_pattern 19 | # until a new TimedSwitch message is sent. 20 | # 21 | # o pulse_pattern: 22 | # A series of time durations in fractions of a second. The 23 | # first number is the duration for having the switch on. 24 | # The second number is the duration for which the switch 25 | # is off. The third is an 'on' period again, etc. 26 | # A pattern is terminated with the end of the array. 27 | # 28 | # Example: [1,1] specifies an on-off sequence of 1 second. 29 | 30 | int8 ON = 1 31 | int8 OFF = 0 32 | int8 NO_CHANGE = -2 33 | int8 REPEAT = -1 34 | int8 FOREVER = -1 35 | 36 | int8 switch_mode 37 | int32 num_cycles 38 | float32[] pulse_pattern 39 | -------------------------------------------------------------------------------- /src/joystick_drivers/wiimote/package.xml: -------------------------------------------------------------------------------- 1 | 2 | wiimote 3 | 1.13.0 4 | GPL 5 | 6 | The wiimote package allows ROS nodes to communicate with a Nintendo Wiimote 7 | and its related peripherals, including the Nunchuk, Motion Plus, and 8 | (experimentally) the Classic. The package implements a ROS node that uses 9 | Bluetooth to communicate with the Wiimote device, obtaining accelerometer 10 | and gyro data, the state of LEDs, the IR camera, rumble (vibrator), 11 | buttons, joystick, and battery state. The node additionally enables ROS 12 | nodes to control the Wiimote's LEDs and vibration for feedback to the human 13 | Wiimote operator. LEDs and vibration may be switched on and off, or made to 14 | operate according to a timed pattern. 15 | 16 | 17 | http://www.ros.org/wiki/wiimote 18 | https://github.com/ros-drivers/joystick_drivers 19 | https://github.com/ros-drivers/joystick_drivers/issues 20 | 21 | Jonathan Bohren 22 | Andreas Paepcke 23 | Melonee Wise 24 | Mark Horn 25 | 26 | catkin 27 | 28 | cwiid-dev 29 | message_generation 30 | roslint 31 | 32 | geometry_msgs 33 | python-cwiid 34 | python-numpy 35 | roscpp 36 | roslib 37 | rospy 38 | sensor_msgs 39 | std_msgs 40 | std_srvs 41 | 42 | cwiid 43 | message_runtime 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /src/joystick_drivers/wiimote/scripts/wiimote_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env 2 | from __future__ import print_function 3 | from bluetooth import * 4 | 5 | print("Looking for compatible devices") 6 | 7 | nearby_devices = discover_devices(lookup_names=1, duration=10, flush_cache=True) 8 | 9 | for addr, name in nearby_devices: 10 | 11 | if name == 'Nintendo RVL-CNT-01' or name == 'Nintendo RVL-WBC-01': 12 | print('Compatible controller:\n %s - %s' % (addr, name)) 13 | if name == 'Nintendo RVL-CNT-01-TR': 14 | print("Incompatible controller:\n %s - %s" % (addr, name)) 15 | -------------------------------------------------------------------------------- /src/joystick_drivers/wiimote/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['wiimote'], 8 | package_dir={'': 'src'}) 9 | 10 | setup(**d) 11 | -------------------------------------------------------------------------------- /src/joystick_drivers/wiimote/src/wiimote/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/joystick_drivers/wiimote/src/wiimote/__init__.py -------------------------------------------------------------------------------- /src/joystick_drivers/wiimote/src/wiimote/wiimoteExceptions.py: -------------------------------------------------------------------------------- 1 | # ############################################################################### 2 | # 3 | # File: wiimoteExceptions.py 4 | # RCS: $Header: $ 5 | # Description: Exception Classes for Wiimote Controller 6 | # Author: Andreas Paepcke 7 | # Created: Thu Aug 13 09:01:17 2009 8 | # Modified: Mon Aug 17 11:27:02 2009 (Andreas Paepcke) paepcke@anw.willowgarage.com 9 | # Language: Python 10 | # Package: N/A 11 | # Status: Experimental (Do Not Distribute) 12 | # 13 | # ############################################################################### 14 | 15 | 16 | class WiimoteError(Exception): 17 | """Mother of all Wiimote exceptions""" 18 | 19 | errMsg = None 20 | 21 | def __init__(self, theErrMsg): 22 | self.errMsg = theErrMsg 23 | 24 | def __str__(self): 25 | return self.errMsg 26 | 27 | 28 | class WiimoteNotFoundError(WiimoteError): 29 | """Tried to pair but failed.""" 30 | 31 | 32 | class WiimoteEnableError(WiimoteError): 33 | """Found wiimote, but couldn't enable it.""" 34 | 35 | 36 | class CallbackStackMultInstError(WiimoteError): 37 | """Code attempted to create a second callback stack instance.""" 38 | 39 | 40 | class ResumeNonPausedError(WiimoteError): 41 | """Code attempted to resume callbacks without first pausing.""" 42 | 43 | 44 | class CallbackStackEmptyError(WiimoteError): 45 | """Attemp to operate on an empty callback stack.""" 46 | -------------------------------------------------------------------------------- /src/joystick_drivers/wiimote/src/wiimote/wiiutils.py: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # 3 | # File: wiiutils.py 4 | # RCS: $Header: $ 5 | # Description: Various handy utilities 6 | # Author: Andreas Paepcke 7 | # Created: Thu Aug 13 15:11:56 2009 8 | # Modified: Thu Aug 13 15:22:45 2009 (Andreas Paepcke) paepcke@anw.willowgarage.com 9 | # Language: Python 10 | # Package: N/A 11 | # Status: Experimental (Do Not Distribute) 12 | # 13 | # 14 | ################################################################################ 15 | 16 | from __future__ import print_function 17 | from __future__ import absolute_import 18 | import sys 19 | import time 20 | from . import wiimoteConstants as acConst 21 | from math import sqrt 22 | import numpy as np 23 | import random 24 | 25 | 26 | def report(str, debuglevel=acConst._DEBUGLEVEL): 27 | """For error reporting, controlled by debuglevel.""" 28 | if debuglevel > 0: 29 | print(str, file=sys.stderr) 30 | return 31 | 32 | 33 | def log(str, file=None): 34 | if acConst._MONITOR_LEVEL > 0: 35 | print(str, file=sys.stderr) 36 | 37 | 38 | def promptUsr(str): 39 | """Prompting user.""" 40 | print(str, file=sys.stderr) 41 | return 42 | 43 | 44 | def getTimeStamp(): 45 | """Return current time as float of seconds since beginning of Epoch.""" 46 | return time.time() 47 | 48 | 49 | if __name__ == '__main__': 50 | foo = np.array([(4.0, 3.0, 3.0)]) 51 | bar = np.array([(4.0, 3.0, 3.0)]) 52 | 53 | isGreater = np.greater(foo, bar) 54 | isBad = np.greater(foo, bar).any() 55 | test = (foo > bar).any() 56 | 57 | print(repr(isGreater)) 58 | print(isBad) 59 | print(test) 60 | -------------------------------------------------------------------------------- /src/lab_bringup/launch/test_env_assembly.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/lab_bringup/launch/test_env_aubo_ctrl.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 | -------------------------------------------------------------------------------- /src/lab_bringup/launch/test_env_laser_mark.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 | -------------------------------------------------------------------------------- /src/lab_bringup/launch/test_env_slam_move.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/lab_bringup/launch/test_run_aubo_blanding.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/lab_bringup/launch/test_run_aubo_feeding.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/lab_bringup/launch/test_run_laser_mark.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/lab_bringup/launch/test_run_slam_move.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/lab_vision/config/env/locator_offset.json: -------------------------------------------------------------------------------- 1 | {"laser_offset_x": -10.0, "laser_offset_y": 54.0} -------------------------------------------------------------------------------- /src/lab_vision/config/env/mask-BoxLaserLocator.json: -------------------------------------------------------------------------------- 1 | {"h_max": 100, "h_min": 40, "s_max": 255, "v_max": 255, "s_min": 155, "v_min": 18} -------------------------------------------------------------------------------- /src/lab_vision/config/env/mask-LaserRect.json: -------------------------------------------------------------------------------- 1 | {"h_max": 100, "h_min": 40, "s_max": 255, "v_max": 255, "s_min": 189, "v_min": 54} -------------------------------------------------------------------------------- /src/lab_vision/config/env1/locator_offset.json: -------------------------------------------------------------------------------- 1 | {"laser_rect_area": [[626.1441040039062, 723.8411254882812], [455.5872802734375, 710.9689331054688], [480.35614013671875, 382.78167724609375], [650.9129638671875, 395.65386962890625]], "laser_offset_x": -23.0, "laser_offset_y": 37.0} -------------------------------------------------------------------------------- /src/lab_vision/config/env1/mask-BoxLaserLocator.json: -------------------------------------------------------------------------------- 1 | {"h_max": 100, "h_min": 40, "s_max": 255, "v_max": 255, "s_min": 26, "v_min": 58} -------------------------------------------------------------------------------- /src/lab_vision/config/env1/mask-LaserRect.json: -------------------------------------------------------------------------------- 1 | {"h_max": 111, "h_min": 49, "s_max": 255, "v_max": 255, "s_min": 120, "v_min": 49} -------------------------------------------------------------------------------- /src/lab_vision/config/env1/mask-agv.json: -------------------------------------------------------------------------------- 1 | {"h_max": 180, "h_min": 0, "s_max": 48, "v_max": 255, "s_min": 0, "v_min": 191} -------------------------------------------------------------------------------- /src/lab_vision/config/env1/mask-line.json: -------------------------------------------------------------------------------- 1 | {"h_max": 100, "h_min": 40, "s_max": 255, "v_max": 255, "s_min": 59, "v_min": 85} -------------------------------------------------------------------------------- /src/lab_vision/data/usb_camera.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | time: "2020年07月08日 星期三 16时31分02秒" 4 | image_width: 1280 5 | image_height: 960 6 | cameraMatrix: !!opencv-matrix 7 | rows: 3 8 | cols: 3 9 | dt: d 10 | data: [ 1.4212673919432768e+03, 0., 6.6919399774968952e+02, 0., 11 | 1.4200941883011269e+03, 4.9281122990898785e+02, 0., 0., 1. ] 12 | distCoeffs: !!opencv-matrix 13 | rows: 5 14 | cols: 1 15 | dt: d 16 | data: [ -6.9998445942761871e-03, 5.7190053200923199e-02, 17 | 8.1106198326906900e-04, 4.9764755587529382e-03, 18 | -1.7446651936751376e-01 ] 19 | -------------------------------------------------------------------------------- /src/lab_vision/scripts/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/lab_vision/scripts/__init__.py -------------------------------------------------------------------------------- /src/lab_vision/scripts/common/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/lab_vision/scripts/common/__init__.py -------------------------------------------------------------------------------- /src/lab_vision/scripts/common/geometry_util.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # encoding:utf-8 3 | 4 | import numpy as np 5 | from operator import itemgetter 6 | 7 | 8 | def sort_rect(curve): 9 | """ 10 | 对四边形的四个点进行排序 11 | :param curve: 12 | :return: 13 | """ 14 | # 按照x正序排列 15 | rst_x = sorted(curve, key=itemgetter(0)) 16 | # print(repr(rst_x)) 17 | # 取x最小的2个，根据y正序列，作为0，1 18 | rect_left = sorted(rst_x[:2], key=itemgetter(1)) 19 | # 把剩余的2个，根据y倒序列，作为3，4 20 | rect_right = sorted(rst_x[-2:], key=itemgetter(1), reverse=True) 21 | rst = rect_left + rect_right 22 | return rst 23 | 24 | 25 | def calc_vector(rect, refer_vector=np.array([0, 1])): 26 | """ 27 | 根据4个点，计算离参考的方向最近的作为Y向量 28 | :param rect: 四个点 29 | :param refer_vector: 参考向量，默认为Y轴正方向 30 | :return: x和y向量 31 | """ 32 | vector_ab = rect[1] - rect[0] 33 | vector_ad = rect[3] - rect[0] 34 | 35 | norm_ab = np.linalg.norm(vector_ab) 36 | norm_ad = np.linalg.norm(vector_ad) 37 | 38 | norm_refer = np.linalg.norm(refer_vector) 39 | 40 | cosangle_ab = vector_ab.dot(refer_vector) / (norm_ab * norm_refer) 41 | cosangle_ad = vector_ad.dot(refer_vector) / (norm_ad * norm_refer) 42 | 43 | if cosangle_ab > cosangle_ad: 44 | vector_x = vector_ad 45 | vector_y = vector_ab 46 | else: 47 | vector_x = vector_ab 48 | vector_y = vector_ad 49 | 50 | return (vector_x, vector_y) 51 | -------------------------------------------------------------------------------- /src/lab_vision/scripts/common/global_ctl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # encoding:utf-8 3 | 4 | # 调试模式 5 | is_debug_mode = False 6 | 7 | 8 | def update_debug_mode(state): 9 | global is_debug_mode 10 | is_debug_mode = state 11 | -------------------------------------------------------------------------------- /src/lab_vision/scripts/detector/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/lab_vision/scripts/detector/__init__.py -------------------------------------------------------------------------------- /src/lab_vision/scripts/imgs/laser_0.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/lab_vision/scripts/imgs/laser_0.jpg -------------------------------------------------------------------------------- /src/lab_vision/scripts/imgs/laser_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/lab_vision/scripts/imgs/laser_1.jpg -------------------------------------------------------------------------------- /src/lab_vision/scripts/imgs/laser_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/lab_vision/scripts/imgs/laser_2.jpg -------------------------------------------------------------------------------- /src/lab_vision/scripts/imgs/laser_rect1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/lab_vision/scripts/imgs/laser_rect1.jpg -------------------------------------------------------------------------------- /src/lab_vision/scripts/imgs/laser_rect2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/lab_vision/scripts/imgs/laser_rect2.png -------------------------------------------------------------------------------- /src/lab_vision/scripts/imgs/rect2.webm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/lab_vision/scripts/imgs/rect2.webm -------------------------------------------------------------------------------- /src/lab_vision/scripts/imgs/test_aubo2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/lab_vision/scripts/imgs/test_aubo2.png -------------------------------------------------------------------------------- /src/lab_vision/scripts/locator/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/lab_vision/scripts/locator/__init__.py -------------------------------------------------------------------------------- /src/lab_vision/scripts/locator/laser_on_box_locator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # encoding:utf-8 3 | import cv2 4 | 5 | class LaserOnBoxLocator(object): 6 | 7 | def __init__(self): 8 | self.threshold = 230 9 | self.win_name = "" 10 | self.kernal = cv2.getStructuringElement(cv2.MORPH_RECT, (7, 7)) 11 | 12 | def init_track_bar(self, name): 13 | self.win_name = "LaserOnBoxLacator" 14 | cv2.namedWindow(self.win_name, cv2.WINDOW_AUTOSIZE) 15 | cv2.createTrackbar("threshold:", self.win_name, self.threshold, 255, lambda x: self.update_args("threshold", x)) 16 | 17 | def update_args(self, arg_name, value): 18 | if arg_name == "threshold": 19 | self.threshold = value 20 | 21 | def get_win_name(self): 22 | return self.win_name 23 | 24 | def get_trackbar_pos(self, track_bar_name): 25 | return cv2.getTrackbarPos(track_bar_name, self.get_win_name()) 26 | 27 | def detect(self, image): 28 | gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) 29 | ret, binary = cv2.threshold(gray, self.threshold, 255, cv2.THRESH_BINARY) 30 | 31 | # 把掩膜来个先闭后开，去掉噪声 32 | binary = cv2.morphologyEx(binary, cv2.MORPH_OPEN, self.kernal) 33 | 34 | # 计算最小外接圆 35 | _, contours, hierarchy = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) 36 | 37 | cv2.drawContours(image, contours, -1, (0, 0, 255), 2) 38 | # print("contour id", i) 39 | 40 | if self.get_win_name(): 41 | cv2.imshow(self.get_win_name(), image) 42 | 43 | # 绘制外切圆 44 | for contour in contours: 45 | # 计算面积 46 | area = cv2.contourArea(contour) 47 | # 该函数计算曲线长度或闭合轮廓周长。True是否封闭 48 | perimeter = cv2.arcLength(contour, True) 49 | if 300 > area > 100: 50 | # 获取最小的外切圆 51 | (x, y), radius = cv2.minEnclosingCircle(contour) 52 | # cv2.circle(binary, (int(x), int(y)), int(radius), (255, 255, 0), 2) 53 | # print("轮廓面积：{} 周长：{:.3f} 圆心：{} 半径：{:.3f}".format(area, perimeter, (x, y), radius)) 54 | return (x, y), radius 55 | 56 | return None 57 | -------------------------------------------------------------------------------- /src/lab_vision/scripts/main_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # encoding:utf-8 3 | 4 | import cv2 5 | from detector.detector_main import DetectorMain 6 | import common.global_ctl as g_ctl 7 | """ 8 | 启动Action服务，等待调用者发起请求 9 | 开启盒子坐标发布者 10 | 11 | 从相机读取数据 12 | 发布盒子位姿信息 13 | """ 14 | if __name__ == '__main__': 15 | g_ctl.update_debug_mode(True) 16 | print "debug_mode: ", g_ctl.is_debug_mode 17 | 18 | detector = DetectorMain("../", "env") 19 | 20 | # 输入 21 | pic = cv2.imread("./imgs/test_aubo2.png", cv2.IMREAD_UNCHANGED) 22 | 23 | rst_lst = detector.start_find(pic) 24 | 25 | # 输出 26 | print(rst_lst) 27 | 28 | while True: 29 | action = cv2.waitKey(30) & 0xFF 30 | if action == ord("q") or action == 27: 31 | break 32 | elif action == 32: 33 | detector.save_params() 34 | 35 | cv2.destroyAllWindows() -------------------------------------------------------------------------------- /src/lab_vision/scripts/test/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/lab_vision/scripts/test/__init__.py -------------------------------------------------------------------------------- /src/lab_vision/scripts/test/shrink_curve.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | #encoding: utf-8 3 | 4 | import cv2 5 | import numpy as np 6 | from common.curve_tools import shrink_polygon 7 | 8 | if __name__ == '__main__': 9 | img = np.zeros((480, 640, 3), np.uint8) 10 | 11 | target_area = np.array([ 12 | [30, 100], 13 | [230, 100], 14 | [210, 180], 15 | [10, 180], 16 | ]) 17 | 18 | cv2.drawContours(img, [target_area], 0, (0, 0, 255), 2) 19 | 20 | new_area = shrink_polygon(target_area, 0.8) 21 | new_area = np.int0(new_area) 22 | print new_area 23 | cv2.drawContours(img, [new_area], 0, (0, 255, 255), 2) 24 | 25 | new_area = shrink_polygon(new_area, 1.5) 26 | new_area = np.int0(new_area) 27 | cv2.drawContours(img, [new_area], 0, (255, 255, 255), 2) 28 | 29 | cv2.imshow("image", img) 30 | 31 | cv2.waitKey(0) 32 | 33 | cv2.destroyAllWindows() 34 | -------------------------------------------------------------------------------- /src/lab_vision/scripts/test/test.py: -------------------------------------------------------------------------------- 1 | for i in range(3): 2 | print i 3 | 4 | print "-" * 10 5 | 6 | -------------------------------------------------------------------------------- /src/lab_vision/scripts/test/test_camera.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # encoding:utf-8 3 | 4 | import numpy as np 5 | import cv2 6 | import glob 7 | 8 | 9 | def draw(img, corners, imgpts): 10 | imgpts = np.int32(imgpts).reshape(-1,2) 11 | 12 | # 将地板绘制成绿色，参数2为多个轮廓的列表，故需要多套一层[] 13 | img = cv2.drawContours(img, [imgpts[:4]],-1,(0,255,0),-1) 14 | 15 | print("-------------------") 16 | # 柱子绘制成蓝色, 参数必须是tuple类型 17 | for i,j in zip(range(4),range(4,8)): 18 | print("{}.{} -> {}.{}".format(i, tuple(imgpts[i]), j,tuple(imgpts[j]))) 19 | img = cv2.line(img, tuple(imgpts[i]), tuple(imgpts[j]),(255,0,0),3) 20 | 21 | # 顶部框用红色 22 | img = cv2.drawContours(img, [imgpts[4:]],-1,(0,0,255),3) 23 | 24 | return img 25 | 26 | 27 | if __name__ == '__main__': 28 | fs = cv2.FileStorage("../../data/usb_camera.yml", cv2.FILE_STORAGE_READ) 29 | mtx = fs.getNode("cameraMatrix").mat() 30 | dist = fs.getNode("distCoeffs").mat() 31 | 32 | print(mtx) 33 | print(dist) 34 | fs.release() 35 | 36 | # 构建对象矩阵， Z=0 37 | objp = np.zeros((6 * 9, 3), np.float32) 38 | objp[:, :2] = np.mgrid[0:6, 0:9].T.reshape(-1, 2) 39 | """ 40 | [ 41 | [0,0],[1,0],[2,0] ... 42 | [0,1],[1,1],[2,2] ... 43 | ... 44 | ] 45 | """ 46 | 47 | # 3D立方体的8个角点 48 | # axis = np.float32([[3, 0, 0], [0, 3, 0], [0, 0, -3]]).reshape(-1, 3) 49 | axis = np.float32([[0, 0, 0], [0, 3, 0], [3, 3, 0], [3, 0, 0], 50 | [0, 0, -3], [0, 3, -3], [3, 3, -3], [3, 0, -3]]) 51 | criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) 52 | 53 | index = 0 54 | capture = cv2.VideoCapture(2) 55 | while True: 56 | _, frame = capture.read() 57 | img = cv2.flip(frame, 1) 58 | gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 59 | ret, corners = cv2.findChessboardCorners(gray, (6, 9), None) 60 | 61 | if ret: 62 | corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) 63 | 64 | # 查找旋转向量和平移向量 65 | retval, rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, mtx, dist) 66 | 67 | # 将3D点投影到图像平面 68 | imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist) 69 | 70 | img = draw(img, corners2, imgpts) 71 | cv2.imshow('img', img) 72 | 73 | k = cv2.waitKey(100) & 0xff 74 | if k == ord('s'): 75 | cv2.imwrite('image_pose_{}.png'.format(index), img) 76 | index += 1 77 | elif k == 27 or k == ord('q') or k == ord('Q'): 78 | break 79 | 80 | cv2.destroyAllWindows() -------------------------------------------------------------------------------- /src/laser_mark/scripts/test_laser_mark.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # coding:utf-8 3 | 4 | import rospy 5 | import actionlib 6 | from actionlib import GoalStatus 7 | from actionlib import ClientGoalHandle 8 | from itheima_msgs.msg import LaserMarkAction, LaserMarkGoal, LaserMarkFeedback, LaserMarkResult 9 | 10 | 11 | def transition_cb(goal_handle): 12 | rospy.loginfo("---------------transition cb---------------") 13 | 14 | 15 | def feedback_cb(goal_handle, feedback): 16 | rospy.loginfo("---------------feedback cb---------------") 17 | rospy.loginfo(feedback) 18 | 19 | 20 | if __name__ == '__main__': 21 | # 创建节点 22 | rospy.init_node("test_laser_mark", anonymous=True) 23 | 24 | # 创建客户端 25 | client = actionlib.ActionClient("/laser/mark", LaserMarkAction) 26 | # 等待连接服务器 27 | client.wait_for_server() 28 | 29 | # 发送请求 30 | goal = LaserMarkGoal() 31 | goal.name = "你好Hello" 32 | goal.type = 4 33 | goal.id = 1 34 | goal_handle = client.send_goal(goal, transition_cb=transition_cb, feedback_cb=feedback_cb) 35 | 36 | client.wait_for_server(rospy.Duration(10)) 37 | -------------------------------------------------------------------------------- /src/order_manager/scripts/order_tip1.ogg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/order_manager/scripts/order_tip1.ogg -------------------------------------------------------------------------------- /src/order_manager/scripts/order_tip2.ogg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/order_manager/scripts/order_tip2.ogg -------------------------------------------------------------------------------- /src/order_manager/scripts/order_tip_node.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # coding: utf-8 3 | 4 | import rospy 5 | import httplib 6 | import json 7 | import os 8 | from playsound import playsound 9 | 10 | 11 | current_dir = os.path.abspath(os.path.dirname(__file__)) 12 | 13 | 14 | def update_status_audit(id): 15 | # 0: 新建订单 16 | # 1: 等待审核 17 | # 2: 审核通过，等待生产 18 | # 3: 正在生产 19 | # 4: 生产完成 20 | order_update_url = "/order/update" 21 | order_update_body = "status=1&id={}".format(id) 22 | conn = httplib.HTTPConnection(host, port) 23 | conn.request("POST", url=order_update_url, body=order_update_body, 24 | headers={"Content-Type": "application/x-www-form-urlencoded"}) 25 | conn.getresponse() 26 | conn.close() 27 | 28 | 29 | if __name__ == '__main__': 30 | # 创建node 31 | node_name = "order_tip_node" 32 | rospy.init_node(node_name) 33 | 34 | host = rospy.get_param("~service_host", "192.168.1.100") 35 | port = rospy.get_param("~service_port", 3000) 36 | 37 | order_list_url = "/order/list" 38 | order_list_body = 'status=0' 39 | 40 | # engine = pyttsx.init() 41 | # voice = engine.getProperty('voice') 42 | # print voice 43 | 44 | rate = rospy.Rate(0.2) 45 | while not rospy.is_shutdown(): 46 | 47 | try: 48 | # 获取新建订单，提醒订单审核 49 | conn = httplib.HTTPConnection(host, port) 50 | conn.request("POST", url=order_list_url, body=order_list_body, 51 | headers={"Content-Type": "application/x-www-form-urlencoded"}) 52 | 53 | response = conn.getresponse() 54 | # print response.status 55 | # print response.read() 56 | data = response.read() 57 | data = json.loads(data) 58 | print data 59 | if len(data) == 0: 60 | rate.sleep() 61 | continue 62 | 63 | if len(data) == 1: 64 | playsound(current_dir + "/order_tip1.ogg") 65 | else: 66 | playsound(current_dir + "/order_tip2.ogg") 67 | 68 | for item in data: 69 | update_status_audit(item["id"]) 70 | 71 | conn.close() 72 | except Exception as e: 73 | print e 74 | 75 | rate.sleep() 76 | -------------------------------------------------------------------------------- /src/slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # toplevel CMakeLists.txt for a catkin workspace 2 | # catkin/cmake/toplevel.cmake 3 | 4 | cmake_minimum_required(VERSION 3.0.2) 5 | 6 | set(CATKIN_TOPLEVEL TRUE) 7 | 8 | # search for catkin within the workspace 9 | set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}") 10 | execute_process(COMMAND ${_cmd} 11 | RESULT_VARIABLE _res 12 | OUTPUT_VARIABLE _out 13 | ERROR_VARIABLE _err 14 | OUTPUT_STRIP_TRAILING_WHITESPACE 15 | ERROR_STRIP_TRAILING_WHITESPACE 16 | ) 17 | if(NOT _res EQUAL 0 AND NOT _res EQUAL 2) 18 | # searching fot catkin resulted in an error 19 | string(REPLACE ";" " " _cmd_str "${_cmd}") 20 | message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}") 21 | endif() 22 | 23 | # include catkin from workspace or via find_package() 24 | if(_res EQUAL 0) 25 | set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake") 26 | # include all.cmake without add_subdirectory to let it operate in same scope 27 | include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE) 28 | add_subdirectory("${_out}") 29 | 30 | else() 31 | # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument 32 | # or CMAKE_PREFIX_PATH from the environment 33 | if(NOT DEFINED CMAKE_PREFIX_PATH) 34 | if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "") 35 | if(NOT WIN32) 36 | string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) 37 | else() 38 | set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) 39 | endif() 40 | endif() 41 | endif() 42 | 43 | # list of catkin workspaces 44 | set(catkin_search_path "") 45 | foreach(path ${CMAKE_PREFIX_PATH}) 46 | if(EXISTS "${path}/.catkin") 47 | list(FIND catkin_search_path ${path} _index) 48 | if(_index EQUAL -1) 49 | list(APPEND catkin_search_path ${path}) 50 | endif() 51 | endif() 52 | endforeach() 53 | 54 | # search for catkin in all workspaces 55 | set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE) 56 | find_package(catkin QUIET 57 | NO_POLICY_SCOPE 58 | PATHS ${catkin_search_path} 59 | NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) 60 | unset(CATKIN_TOPLEVEL_FIND_PACKAGE) 61 | 62 | if(NOT catkin_FOUND) 63 | message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.") 64 | endif() 65 | endif() 66 | 67 | catkin_workspace() 68 | -------------------------------------------------------------------------------- /src/slam/heima_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(heima_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS message_generation std_msgs geometry_msgs) 5 | 6 | add_message_files( 7 | DIRECTORY msg 8 | FILES 9 | Velocities.msg 10 | PID.msg 11 | Imu.msg 12 | Battery.msg 13 | DHT22.msg 14 | Servo.msg 15 | Infrared.msg 16 | Ultrasonic.msg 17 | 18 | ) 19 | 20 | generate_messages(DEPENDENCIES std_msgs geometry_msgs) 21 | 22 | catkin_package(CATKIN_DEPENDS message_runtime std_msgs geometry_msgs) 23 | -------------------------------------------------------------------------------- /src/slam/heima_msgs/msg/Battery.msg: -------------------------------------------------------------------------------- 1 | float32 battery 2 | -------------------------------------------------------------------------------- /src/slam/heima_msgs/msg/DHT22.msg: -------------------------------------------------------------------------------- 1 | float32 Temperature 2 | float32 Humidity 3 | -------------------------------------------------------------------------------- /src/slam/heima_msgs/msg/Imu.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Vector3 linear_acceleration 2 | geometry_msgs/Vector3 angular_velocity 3 | geometry_msgs/Vector3 magnetic_field -------------------------------------------------------------------------------- /src/slam/heima_msgs/msg/Infrared.msg: -------------------------------------------------------------------------------- 1 | float32 front 2 | float32 rear 3 | 4 | -------------------------------------------------------------------------------- /src/slam/heima_msgs/msg/PID.msg: -------------------------------------------------------------------------------- 1 | float32 p 2 | float32 d 3 | float32 i 4 | -------------------------------------------------------------------------------- /src/slam/heima_msgs/msg/Servo.msg: -------------------------------------------------------------------------------- 1 | int32 Servo1 2 | int32 Servo2 3 | -------------------------------------------------------------------------------- /src/slam/heima_msgs/msg/Ultrasonic.msg: -------------------------------------------------------------------------------- 1 | float32 front 2 | float32 left 3 | float32 right 4 | -------------------------------------------------------------------------------- /src/slam/heima_msgs/msg/Velocities.msg: -------------------------------------------------------------------------------- 1 | 2 | float32 linear_x 3 | float32 linear_y 4 | float32 angular_z 5 | -------------------------------------------------------------------------------- /src/slam/heima_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | heima_msgs 4 | 0.0.1 5 | The heima_msgs package 6 | kevin 7 | BSD 8 | catkin 9 | message_generation 10 | std_msgs 11 | geometry_msgs 12 | 13 | message_runtime 14 | std_msgs 15 | geometry_msgs 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/slam/heima_serial/include/BlockingQueue.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef HEIMA_SERIAL_BLOCKINGQUEUE_H 3 | #define HEIMA_SERIAL_BLOCKINGQUEUE_H 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | template 11 | class BlockingQueue { 12 | public: 13 | using MutexLockGuard = std::lock_guard; 14 | 15 | BlockingQueue() 16 | : _mutex(), 17 | _notEmpty(), 18 | _queue() 19 | { 20 | } 21 | 22 | BlockingQueue(const BlockingQueue &) = delete; 23 | BlockingQueue& operator=(const BlockingQueue &) = delete; 24 | 25 | void put(const T &x) 26 | { 27 | { 28 | MutexLockGuard lock(_mutex); 29 | _queue.push_back(x); 30 | } 31 | _notEmpty.notify_one(); 32 | } 33 | 34 | void put(T &&x) 35 | { 36 | { 37 | MutexLockGuard lock(_mutex); 38 | _queue.push_back(std::move(x)); 39 | } 40 | _notEmpty.notify_one(); 41 | } 42 | 43 | T take() 44 | { 45 | std::unique_lock lock(_mutex); 46 | _notEmpty.wait(lock, [this]{ return !this->_queue.empty(); }); 47 | assert(!_queue.empty()); 48 | 49 | T front(std::move(_queue.front())); 50 | _queue.pop_front(); 51 | 52 | return front; 53 | } 54 | 55 | size_t size() const 56 | { 57 | MutexLockGuard lock(_mutex); 58 | return _queue.size(); 59 | } 60 | 61 | private: 62 | mutable std::mutex _mutex; 63 | std::condition_variable _notEmpty; 64 | std::deque _queue; 65 | }; 66 | 67 | 68 | #endif //HEIMA_SERIAL_BLOCKINGQUEUE_H 69 | -------------------------------------------------------------------------------- /src/slam/heima_serial/include/Kinematics.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Kaijun on 2020/6/21. 3 | // 4 | 5 | #ifndef HEIMA3ROBOT_KINEMATICS_H 6 | #define HEIMA3ROBOT_KINEMATICS_H 7 | 8 | #include 9 | 10 | class Kinematics { 11 | 12 | public: 13 | struct RPM{ 14 | int motorLeftRpm; 15 | int leftSignalNum; 16 | uint8_t leftSignalDirect; 17 | int motorRightRpm; 18 | int rightSignalNum; 19 | uint8_t rightSignalDirect; 20 | }; 21 | 22 | struct Velocity{ 23 | double linear_velocity; 24 | double angular_velocity; 25 | }; 26 | 27 | private: 28 | double wheel_diameter; 29 | double half_width; 30 | double wheel_circle; 31 | 32 | public: 33 | /** 34 | * 根据线速度和角速度计算左右轮子的转速 35 | * @param wheel_diameter 轮子的直径 36 | * @param half_width 小车左右轮子中心距离的一半 37 | */ 38 | 39 | Kinematics(double wheel_diameter,double half_width); 40 | Velocity getVelocityByNum(double l_num,double r_num); 41 | RPM calculateRPM(double linear_x,double angular_z); 42 | Velocity getVelocity(double rpm_l,double rmp_r); 43 | 44 | }; 45 | 46 | 47 | #endif //HEIMA3ROBOT_KINEMATICS_H 48 | -------------------------------------------------------------------------------- /src/slam/heima_serial/include/heima_protocol.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Kaijun on 2020/5/30. 3 | // 4 | 5 | #ifndef ROSSERIAL_DEMO_USER_PROTOCOL_H 6 | #define ROSSERIAL_DEMO_USER_PROTOCOL_H 7 | 8 | #include 9 | 10 | enum _ptl_type 11 | { 12 | VAL_START = 0, 13 | VAL_POSE, 14 | VAL_VEL, 15 | VAL_PID, 16 | VAL_IMU, 17 | VAL_END 18 | }; 19 | #define _SERIAL_SYN_CODE_START 0xFA 20 | // 1 字节对齐 21 | #pragma pack(1) 22 | typedef struct _serial_data 23 | { 24 | uint8_t syn; 25 | uint8_t type; 26 | struct { 27 | struct{ 28 | float liner[3]; 29 | float angular[3]; 30 | }vel; 31 | struct{ 32 | bool rot_ok,acc_ok,mag_ok; 33 | double rot[3],acc[3],mag[3]; 34 | }imu; 35 | float pid[3]; 36 | }dat; 37 | uint8_t syn_CR; 38 | uint8_t syn_LF; 39 | }serialData; 40 | #pragma pack(); 41 | 42 | 43 | // 定义接收数据帧格式 44 | #define SERIAL_SIGN_FLAG1 0xff 45 | #define SERIAL_SIGN_FLAG2 0xfe 46 | #pragma pack(1) 47 | typedef struct 48 | { 49 | double x_vel; 50 | double y_vel; 51 | double z_vel; 52 | 53 | double x_acc; 54 | double y_acc; 55 | double z_acc; 56 | 57 | double x_gyro; 58 | double y_gyro; 59 | double z_gyro; 60 | 61 | }SerialRxData; 62 | #pragma pack(); 63 | 64 | 65 | #pragma pack(1) 66 | typedef struct 67 | { 68 | uint8_t syn1; 69 | uint8_t syn2; 70 | uint8_t a_vel; 71 | uint8_t b_vel; 72 | uint8_t a_direct; 73 | uint8_t b_direct; 74 | uint8_t flag0 = 0; 75 | uint8_t flag1 = 0; 76 | uint8_t flag2 = 0; 77 | uint8_t flag3 = 0; 78 | }SerialTxData; 79 | #pragma pack(); 80 | 81 | 82 | 83 | #endif //ROSSERIAL_DEMO_USER_PROTOCOL_H 84 | -------------------------------------------------------------------------------- /src/slam/heima_serial/include/heima_serial_node.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by itcast on 7/6/20. 3 | // 4 | 5 | #ifndef HEIMA_SERIAL_HEIMA_SERIAL_NODE_H 6 | #define HEIMA_SERIAL_HEIMA_SERIAL_NODE_H 7 | 8 | using namespace std; 9 | #define SEND_DATA_CHECK 1 //标志位，发送端做校验位 10 | #define READ_DATA_CHECK 0 //标志位，接收端做校验位 11 | #define FRAME_HEADER 0X7B //帧头，和下位机一致 12 | #define FRAME_TAIL 0X7D //帧尾 13 | #define RECEIVE_DATA_SIZE 24//下位机发过来的数据的长度 14 | #define SEND_DATA_SIZE 11//ROS发给下位机的数据的长度 考虑到时效应短尽短 15 | #define PI 3.1415926f//圆周率 16 | //这个和陀螺仪设置的量程有关的 转化为度每秒是/65.5 转为弧度每秒/57.3 其子65.5看MPU6050手册，STM32底层FS_SEL=1 17 | #define GYROSCOPE_RATIO 0.00026644f // 1/65.5/57.30=0.00026644 陀螺仪原始数据换成弧度单位 18 | //这个和陀螺仪设置的量程有关的 转化为度每秒是/65.5 转为弧度每秒/57.3 其子65.5看MPU6050手册，STM32底层FS_SEL=1 19 | #define ACCEl_RATIO 16384.0f // 量程±2g，重力加速度定义为1g等于9.8米每平方秒。 20 | extern sensor_msgs::Imu Mpu6050; 21 | 22 | // 速度/位置结构体 23 | typedef struct __Vel_Pos_Data_ 24 | { 25 | float X; 26 | float Y; 27 | float Z; 28 | }Vel_Pos_Data; 29 | 30 | 31 | typedef struct __MPU6050_DATA_ 32 | { 33 | short accele_x_data; 34 | short accele_y_data; 35 | short accele_z_data; 36 | short gyros_x_data; 37 | short gyros_y_data; 38 | short gyros_z_data; 39 | 40 | }MPU6050_DATA; 41 | 42 | //DATE：2020-5-31 43 | 44 | typedef struct _SEND_DATA_ 45 | { 46 | uint8_t tx[SEND_DATA_SIZE]; 47 | float X_speed; 48 | float Y_speed; 49 | float Z_speed; 50 | unsigned char Frame_Tail; //1个字节 帧尾 校验位 51 | 52 | }SEND_DATA; 53 | //DATE：2020-5-31 54 | 55 | typedef struct _RECEIVE_DATA_ 56 | { 57 | uint8_t rx[RECEIVE_DATA_SIZE]; 58 | uint8_t Flag_Stop; 59 | unsigned char Frame_Header; //1个字节 帧头 60 | float X_speed; 61 | float Y_speed; 62 | float Z_speed; 63 | float Power_Voltage; 64 | unsigned char Frame_Tail;//1个字节 帧尾 校验位 65 | }RECEIVE_DATA; 66 | 67 | 68 | 69 | 70 | 71 | #endif //HEIMA_SERIAL_HEIMA_SERIAL_NODE_H 72 | -------------------------------------------------------------------------------- /src/slam/heima_serial/launch/serial_bridge.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/slam/heimarobot/include/heima_base.h: -------------------------------------------------------------------------------- 1 | #ifndef RIKI_BASE_H 2 | #define RIKI_BASE_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | class HeimaBase 9 | { 10 | public: 11 | HeimaBase(); 12 | void velCallback(const heima_msgs::Velocities& vel); 13 | 14 | 15 | private: 16 | ros::NodeHandle nh_; 17 | ros::Publisher odom_publisher_; 18 | ros::Subscriber velocity_subscriber_; 19 | tf::TransformBroadcaster odom_broadcaster_; 20 | 21 | float linear_scale_; 22 | float steering_angle_; 23 | float linear_velocity_x_; 24 | float linear_velocity_y_; 25 | float angular_velocity_z_; 26 | ros::Time last_vel_time_; 27 | float vel_dt_; 28 | float x_pos_; 29 | float y_pos_; 30 | float heading_; 31 | }; 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /src/slam/heimarobot/launch/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 | 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 | -------------------------------------------------------------------------------- /src/slam/heimarobot/launch/lidar/deltalidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | -------------------------------------------------------------------------------- /src/slam/heimarobot/launch/lidar/rplidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /src/slam/heimarobot/launch/lidar/ydlidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /src/slam/heimarobot/launch/lidar_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/slam/heimarobot/launch/lidar_slam_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /src/slam/heimarobot/launch/navigate.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/slam/heimarobot/launch/navigate_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /src/slam/heimarobot/maps/house.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/slam/heimarobot/maps/house.pgm -------------------------------------------------------------------------------- /src/slam/heimarobot/maps/house.yaml: -------------------------------------------------------------------------------- 1 | image: house.pgm 2 | resolution: 0.050000 3 | origin: [-13.800000, -13.800000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /src/slam/heimarobot/maps/map.sh: -------------------------------------------------------------------------------- 1 | rosrun map_server map_saver -f house 2 | -------------------------------------------------------------------------------- /src/slam/heimarobot/param/box_filter.yaml: -------------------------------------------------------------------------------- 1 | scan_filter_chain: 2 | - name: box 3 | type: laser_filters/LaserScanBoxFilter 4 | params: 5 | box_frame: laser 6 | min_x: -0.02 7 | max_x: 0.40 8 | min_y: -0.28 9 | max_y: 0.28 10 | min_z: -0.20 11 | max_z: 0.20 12 | 13 | -------------------------------------------------------------------------------- /src/slam/heimarobot/param/ekf/robot_localization.yaml: -------------------------------------------------------------------------------- 1 | frequency: 40 2 | 3 | two_d_mode: true 4 | diagnostics_agg: true 5 | 6 | #x , y , z, 7 | #roll , pitch , yaw, 8 | #vx , vy , vz, 9 | #vroll , vpitch, vyaw, 10 | #ax , ay , az 11 | 12 | odom0: /raw_odom 13 | odom0_config: [false, false, false, 14 | false, false, false, 15 | true, true, false, 16 | false, false, true, 17 | false, false, false] 18 | 19 | odom0_differential: true 20 | odom0_relative: false 21 | 22 | imu0: /imu/data 23 | 24 | # NOTE: If you find that your robot has x drift, 25 | # the most likely candidate is the x'' (acceleration) fr$ 26 | # Just set it to false! (It's the first entry on the las$ 27 | imu0_config: [false, false, false, 28 | false, false, true, 29 | false, false, false, 30 | false, false, true, 31 | false, false, false] 32 | 33 | imu0_differential: true 34 | imu0_relative: false 35 | 36 | odom_frame: odom 37 | base_link_frame: base_footprint 38 | world_frame: odom 39 | 40 | -------------------------------------------------------------------------------- /src/slam/heimarobot/param/imu/imu_calib.yaml: -------------------------------------------------------------------------------- 1 | bias: 2 | - 66.96578957299793 3 | - 35.94031769965611 4 | - 175.9547672064946 5 | SM: 6 | - 69.64039744318617 7 | - 17.25490671933836 8 | - 67.84925522374903 9 | - -50.5410734075456 10 | - -89.11223351740486 11 | - 31.13633875156774 12 | - 3.982039071485659 13 | - 87.66051574719046 14 | - 176.9947872889987 -------------------------------------------------------------------------------- /src/slam/heimarobot/param/navigation/2wd/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | max_vel_trans: 0.6 #x方向最大线速度绝对值，单位:米/秒 3 | min_vel_trans: 0.01 4 | max_vel_x: 0.35 5 | min_vel_x: -0.35 6 | max_vel_y: 0.0 7 | min_vel_y: 0.0 8 | max_vel_theta: 0.35 #机器人的最大旋转角速度的绝对值，单位为 rad/s 9 | min_vel_theta: -0.35 10 | acc_lim_x: 1.25 11 | acc_lim_y: 0.0 12 | acc_lim_theta: 5 13 | acc_lim_trans: 1.25 14 | 15 | prune_plan: false 16 | 17 | xy_goal_tolerance: 0.15 18 | yaw_goal_tolerance: 0.15 19 | latch_xy_goal_tolerance: false 20 | trans_stopped_vel: 0.1 21 | theta_stopped_vel: 0.1 22 | sim_time: 4.0 # 前向模拟轨迹的时间，单位为s(seconds) 23 | sim_granularity: 0.1 24 | angular_sim_granularity: 0.1 25 | path_distance_bias: 34.0 26 | goal_distance_bias: 24.0 27 | occdist_scale: 0.05 28 | twirling_scale: 0.0 29 | stop_time_buffer: 0.5 30 | oscillation_reset_dist: 0.05 31 | oscillation_reset_angle: 0.2 32 | forward_point_distance: 0.3 33 | scaling_speed: 0.25 34 | max_scaling_factor: 0.2 35 | vx_samples: 20 36 | vy_samples: 0 37 | vth_samples: 40 38 | 39 | use_dwa: true 40 | restore_defaults: false 41 | -------------------------------------------------------------------------------- /src/slam/heimarobot/param/navigation/2wd/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 2.5 2 | raytrace_range: 3.0 3 | footprint: [[0.14, -0.30], [-0.46, -0.30], [-0.46, 0.30], [0.14, 0.30]] 4 | inflation_radius: 0.30 5 | transform_tolerance: 0.5 6 | 7 | observation_sources: scan_filtered 8 | scan_filtered: 9 | data_type: LaserScan 10 | topic: scan_filtered 11 | marking: true 12 | clearing: true 13 | 14 | 15 | 16 | #obstacle_layer: 17 | # enabled: true #使能障碍层 18 | # max_obstacle_height: 2.0 19 | # min_obstacle_height: 0.0 20 | # #origin_z: 0.0 21 | # #z_resolution: 0.2 22 | # #z_voxels: 2 23 | # #unknown_threshold: 15 24 | # #mark_threshold: 0 25 | # combination_method: 1 26 | # track_unknown_space: true #true needed for disabling global path planning through unknown space 27 | # obstacle_range: 2.5 #这些参数设置了代价地图中的障碍物信息的阈值。 "obstacle_range" 参数确定最大范围传感器读数 28 | # #这将导致障碍物被放入代价地图中。在这里，我们把它设置在2.5米，这意味着机器人只会更新其地图包含距离移动基座2.5米以内的障碍物的信息。 29 | # raytrace_range: 3.0 #“raytrace_range”参数确定了用于清除指定范围外的空间。将其设置为3.0米， 30 | # # 这意味着机器人将尝试清除3米外的空间，在代价地图中清除3米外的障碍物。 31 | # #origin_z: 0.0 32 | # #z_resolution: 0.2 33 | # #z_voxels: 2 34 | # publish_voxel_map: false 35 | # observation_sources: scan_filtered 36 | # scan_filtered: 37 | # data_type: LaserScan 38 | # topic: scan_filtered 39 | # marking: true 40 | # clearing: true 41 | # expected_update_rate: 0 42 | # 43 | # 44 | ##cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns 45 | #inflation_layer: 46 | # enabled: true #使能膨胀层 47 | # cost_scaling_factor: 5.0 # exponential rate at which the obstacle cost drops off (default: 10) 48 | # inflation_radius: 0.4 # 机器人膨胀半径，比如设置为0.3,意味着规划的路径距离0.3米以上，这个参数理论上越大越安全 49 | # #但是会导致无法穿过狭窄的地方 50 | # 51 | #static_layer: 52 | # enabled: true 53 | # map_topic: "/map" 54 | # 55 | ## map_type: costmap 56 | -------------------------------------------------------------------------------- /src/slam/heimarobot/param/navigation/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_footprint 4 | update_frequency: 3.0 5 | publish_frequency: 3.0 6 | transform_tolerance: 0.5 7 | static_map: true 8 | 9 | #global_costmap: 10 | # global_frame: map 11 | # robot_base_frame: base_footprint 12 | # update_frequency: 0.8 #before: 5.0 13 | # publish_frequency: 0.5 #before 0.5 14 | # #static_map: true 15 | # transform_tolerance: 0.5 16 | # cost_scaling_factor: 10.0 17 | # inflation_radius: 0.55 18 | # 19 | # plugins: 20 | # - {name: static_layer, type: "costmap_2d::StaticLayer"} 21 | # - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 22 | # - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 23 | 24 | -------------------------------------------------------------------------------- /src/slam/heimarobot/param/navigation/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: base_footprint 4 | 5 | update_frequency: 3.0 6 | publish_frequency: 3.0 7 | transform_tolerance: 0.5 8 | 9 | static_map: false 10 | rolling_window: true 11 | width: 3 12 | height: 3 13 | resolution: 0.05 14 | 15 | meter_scoring: true 16 | #local_costmap: 17 | # global_frame: odom 18 | # robot_base_frame: base_footprint 19 | # update_frequency: 5.0 #before 5.0 20 | # publish_frequency: 2.0 #before 2.0 21 | # static_map: false 22 | # rolling_window: true 23 | # width: 2.5 24 | # height: 2.5 25 | # resolution: 0.05 #increase to for higher res 0.025 26 | # transform_tolerance: 0.5 27 | # cost_scaling_factor: 5 28 | # inflation_radius: 0.55 29 | # 30 | # plugins: 31 | # - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 32 | # - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 33 | -------------------------------------------------------------------------------- /src/slam/heimarobot/param/navigation/move_base.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/slam/heimarobot/param/navigation/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | base_global_planner: global_planner/GlobalPlanner 2 | base_local_planner: dwa_local_planner/DWAPlannerROS 3 | # 4 | #shutdown_costmaps: false 5 | # 6 | #controller_frequency: 5.0 #before 5.0 7 | #controller_patience: 3.0 8 | # 9 | #planner_frequency: 0.5 10 | #planner_patience: 5.0 11 | # 12 | #oscillation_timeout: 10.0 13 | #oscillation_distance: 0.2 14 | # 15 | #conservative_reset_dist: 0.1 #distance from an obstacle at which it will unstuck itself 16 | # 17 | #cost_factor: 1.0 18 | #neutral_cost: 55 19 | #lethal_cost: 253 20 | controller_frequency: 3.0 21 | planner_patience: 5.0 22 | controller_patience: 15.0 23 | conservative_reset_dist: 3.0 24 | planner_frequency: 2.0 25 | oscillation_timeout: 10.0 26 | oscillation_distance: 0.2 27 | -------------------------------------------------------------------------------- /src/slam/heimarobot/param/navigation/slam_gmapping.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 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /src/slam/heimarobot/script/common/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/slam/heimarobot/script/common/__init__.py -------------------------------------------------------------------------------- /src/slam/heimarobot/script/common/transform_tools.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | import numpy as np 4 | import math 5 | 6 | def isRotationMatrix(R): 7 | Rt = np.transpose(R) 8 | shouldBeIdentity = np.dot(Rt, R) 9 | I = np.identity(3, dtype=R.dtype) 10 | n = np.linalg.norm(I - shouldBeIdentity) 11 | return n < 1e-6 12 | 13 | 14 | def rotationMatrixToEulerAngles(R): 15 | assert (isRotationMatrix(R)) 16 | 17 | sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0]) 18 | 19 | singular = sy < 1e-6 20 | 21 | if not singular: 22 | x = math.atan2(R[2, 1], R[2, 2]) 23 | y = math.atan2(-R[2, 0], sy) 24 | z = math.atan2(R[1, 0], R[0, 0]) 25 | else: 26 | x = math.atan2(-R[1, 2], R[1, 1]) 27 | y = math.atan2(-R[2, 0], sy) 28 | z = 0 29 | 30 | return np.array([x, y, z]) 31 | 32 | 33 | def eulerAnglesToRotationMatrix(theta): 34 | R_x = np.array([[1, 0, 0], 35 | [0, math.cos(theta[0]), -math.sin(theta[0])], 36 | [0, math.sin(theta[0]), math.cos(theta[0])] 37 | ]) 38 | 39 | R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])], 40 | [0, 1, 0], 41 | [-math.sin(theta[1]), 0, math.cos(theta[1])] 42 | ]) 43 | 44 | R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0], 45 | [math.sin(theta[2]), math.cos(theta[2]), 0], 46 | [0, 0, 1] 47 | ]) 48 | 49 | R = np.dot(R_z, np.dot(R_y, R_x)) 50 | 51 | return R 52 | -------------------------------------------------------------------------------- /src/slam/heimarobot/script/test.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/slam/heimarobot/script/test.jpg -------------------------------------------------------------------------------- /src/slam/heimarobot/script/usb_camera.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | time: "2020年07月13日 星期一 14时34分45秒" 4 | image_width: 1280 5 | image_height: 720 6 | cameraMatrix: !!opencv-matrix 7 | rows: 3 8 | cols: 3 9 | dt: d 10 | data: [ 1.4292452281580436e+03, 0., 6.3779945691363878e+02, 0., 11 | 1.4299496653334040e+03, 4.0885870902509140e+02, 0., 0., 1. ] 12 | distCoeffs: !!opencv-matrix 13 | rows: 5 14 | cols: 1 15 | dt: d 16 | data: [ -1.7216306760505651e-01, 3.8721873727076400e-01, 17 | 8.2316134233809070e-04, -4.6363980158779859e-04, 18 | -1.6428868286626035e+00 ] 19 | -------------------------------------------------------------------------------- /src/slam/heimarobot/src/heima_base_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "heima_base.h" 3 | 4 | int main(int argc, char** argv ) 5 | { 6 | ros::init(argc, argv, "heima_base_node"); 7 | HeimaBase heima; 8 | ros::spin(); 9 | return 0; 10 | } 11 | -------------------------------------------------------------------------------- /src/slam/heimarobot_nav/.gitignore: -------------------------------------------------------------------------------- 1 | *.out 2 | *.swp 3 | *.pyc 4 | tags 5 | -------------------------------------------------------------------------------- /src/slam/heimarobot_nav/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(heimarobot_nav) 4 | 5 | find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure) 6 | 7 | catkin_python_setup() 8 | 9 | generate_dynamic_reconfigure_options(cfg/CalibrateAngular.cfg cfg/CalibrateLinear.cfg cfg/TestPath.cfg) 10 | 11 | catkin_package(CATKIN_DEPENDS dynamic_reconfigure) 12 | -------------------------------------------------------------------------------- /src/slam/heimarobot_nav/cfg/CalibrateAngular.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = "heimarobot_nav" 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | gen.add("test_angle", double_t, 0, "Test Angle", 360.0, 0.0, 360.0) 10 | 11 | gen.add("speed", double_t, 0, "Angular speed in radians per second", 0.25, -1.5, 1.5) 12 | 13 | gen.add("tolerance", double_t, 0, "Error tolerance to goal angle in degrees", 1.0, 1.0, 10.0) 14 | 15 | gen.add("odom_angular_scale_correction", double_t, 0, "Angular correction factor", 1.0, 0.0, 3.0) 16 | 17 | gen.add("start_test", bool_t, 0, "Check to start the test", False) 18 | 19 | 20 | exit(gen.generate(PACKAGE, "calibrate_angular", "CalibrateAngular")) 21 | -------------------------------------------------------------------------------- /src/slam/heimarobot_nav/cfg/CalibrateLinear.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE = "heimarobot_nav" 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | gen.add("test_distance", double_t, 0, "Test distance in meters", 1.0, 0.0, 2.0) 10 | 11 | gen.add("speed", double_t, 0, "Robot speed in meters per second", 0.15, 0.0, 0.3) 12 | 13 | gen.add("tolerance", double_t, 0, "Error tolerance to goal distance in meters", 0.01, 0.0, 0.1) 14 | 15 | gen.add("odom_linear_scale_correction", double_t, 0, "Linear correction factor", 1.0, 0.0, 3.0) 16 | 17 | gen.add("start_test", bool_t, 0, "Check to start the test", False) 18 | 19 | 20 | exit(gen.generate(PACKAGE, "calibrate_linear", "CalibrateLinear")) 21 | -------------------------------------------------------------------------------- /src/slam/heimarobot_nav/cfg/TestPath.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE = "heimarobot_nav" 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | gen.add("test_distance", double_t, 0, "Test distance in meters", 1.0, 0.0, 2.0) 10 | 11 | gen.add("test_angle", double_t, 0, "Test Angle",360.0, 0.0, 360.0) 12 | 13 | gen.add("tolerance", double_t, 0, "Error tolerance to goal distance in meters", 0.01, 0.0, 0.1) 14 | 15 | gen.add("odom_linear_scale_correction", double_t, 0, "Linear correction factor", 1.0, 0.0, 3.0) 16 | gen.add("odom_angular_scale_correction", double_t, 0, "Angular correction factor", 1.0, 0.0, 3.0) 17 | 18 | gen.add("start_distance", bool_t, 0, "Check to start the test", False) 19 | gen.add("start_angle", bool_t, 0, "Check to start the test", False) 20 | 21 | 22 | exit(gen.generate(PACKAGE, "move_path", "TestPath")) 23 | -------------------------------------------------------------------------------- /src/slam/heimarobot_nav/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['heimarobot_nav'], 9 | package_dir={'': 'src'}) 10 | 11 | setup(**setup_args) 12 | -------------------------------------------------------------------------------- /src/slam/heimarobot_nav/src/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/slam/heimarobot_nav/src/__init__.py -------------------------------------------------------------------------------- /src/slam/heimarobot_nav/src/heimarobot_nav/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/slam/heimarobot_nav/src/heimarobot_nav/__init__.py -------------------------------------------------------------------------------- /src/slam/heimarobot_nav/src/heimarobot_nav/cfg/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/slam/heimarobot_nav/src/heimarobot_nav/cfg/__init__.py -------------------------------------------------------------------------------- /src/slam/heimarobot_nav/src/heimarobot_nav/transform_utils.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ A couple of handy conversion utilities taken from the turtlebot_node.py script found in the 4 | turtlebot_node ROS package at: 5 | 6 | http://www.ros.org/wiki/turtlebot_node 7 | 8 | """ 9 | 10 | import PyKDL 11 | from math import pi 12 | 13 | def quat_to_angle(quat): 14 | rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w) 15 | return rot.GetRPY()[2] 16 | 17 | def normalize_angle(angle): 18 | res = angle 19 | while res > pi: 20 | res -= 2.0 * pi 21 | while res < -pi: 22 | res += 2.0 * pi 23 | return res 24 | -------------------------------------------------------------------------------- /src/slam/imu_calib/.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.user 3 | -------------------------------------------------------------------------------- /src/slam/imu_calib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(imu_calib) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | cmake_modules 6 | roscpp 7 | sensor_msgs 8 | heima_msgs 9 | ) 10 | 11 | find_package(Eigen REQUIRED) 12 | pkg_check_modules(YAML yaml-cpp) 13 | 14 | ################################### 15 | ## catkin specific configuration ## 16 | ################################### 17 | 18 | catkin_package( 19 | INCLUDE_DIRS include 20 | LIBRARIES accel_calib 21 | CATKIN_DEPENDS cmake_modules roscpp sensor_msgs heima_msgs 22 | DEPENDS Eigen yaml-cpp 23 | ) 24 | 25 | ########### 26 | ## Build ## 27 | ########### 28 | 29 | include_directories(include) 30 | include_directories( 31 | ${catkin_INCLUDE_DIRS} 32 | ${Eigen_INCLUDE_DIRS} 33 | ${YAML_INCLUDEDIR} 34 | ) 35 | 36 | # accel_calib library 37 | add_library(accel_calib 38 | src/accel_calib/accel_calib.cpp 39 | ) 40 | target_link_libraries(accel_calib 41 | ${YAML_LIBRARIES} 42 | ) 43 | 44 | # do_calib node 45 | add_executable(do_calib src/do_calib_node.cpp src/do_calib.cpp) 46 | add_dependencies(do_calib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 47 | target_link_libraries(do_calib 48 | accel_calib 49 | ${catkin_LIBRARIES} 50 | ) 51 | 52 | # apply_calib node 53 | add_executable(apply_calib src/apply_calib_node.cpp src/apply_calib.cpp) 54 | add_dependencies(apply_calib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 55 | target_link_libraries(apply_calib 56 | accel_calib 57 | ${catkin_LIBRARIES} 58 | ) 59 | 60 | ############# 61 | ## Install ## 62 | ############# 63 | 64 | # executables and libraries 65 | install(TARGETS accel_calib do_calib apply_calib 66 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 67 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 68 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 69 | ) 70 | 71 | # header files 72 | install(DIRECTORY include/${PROJECT_NAME}/ 73 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 74 | FILES_MATCHING PATTERN "*.h" 75 | PATTERN ".svn" EXCLUDE 76 | ) 77 | -------------------------------------------------------------------------------- /src/slam/imu_calib/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Daniel Koch 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, 5 | are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | 3. Neither the name of the copyright holder nor the names of its contributors 15 | may be used to endorse or promote products derived from this software 16 | without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 22 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 25 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /src/slam/imu_calib/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | imu_calib 4 | 0.0.0 5 | Package for computing and applying IMU calibrations 6 | 7 | Daniel Koch 8 | Daniel Koch 9 | 10 | BSD 11 | 12 | http://wiki.ros.org/imu_calib 13 | https://github.com/dpkoch/imu_calib.git 14 | https://github.com/dpkoch/imu_calib/issues 15 | 16 | catkin 17 | 18 | cmake_modules 19 | eigen 20 | roscpp 21 | sensor_msgs 22 | yaml-cpp 23 | heima_msgs 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/slam/imu_calib/src/apply_calib_node.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, Daniel Koch 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /** 36 | * \file apply_calib_node.cpp 37 | * \author Daniel Koch 43 | 44 | #include "imu_calib/apply_calib.h" 45 | 46 | int main(int argc, char** argv) 47 | { 48 | ros::init(argc, argv, "apply_calib"); 49 | 50 | imu_calib::ApplyCalib calib; 51 | ros::spin(); 52 | 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /src/slam/imu_calib/src/do_calib_node.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, Daniel Koch 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /** 36 | * \file do_calib_node.cpp 37 | * \author Daniel Koch 38 | * 39 | * Node performs accelerometer calibration and writes parameters to data file 40 | */ 41 | 42 | #include 43 | 44 | #include "imu_calib/do_calib.h" 45 | 46 | int main(int argc, char** argv) 47 | { 48 | ros::init(argc, argv, "do_calib"); 49 | 50 | imu_calib::DoCalib calib; 51 | while (ros::ok() && calib.running()) 52 | { 53 | ros::spinOnce(); 54 | } 55 | 56 | return 0; 57 | } 58 | -------------------------------------------------------------------------------- /src/slam_ctrl/scripts/common/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/slam_ctrl/scripts/common/__init__.py -------------------------------------------------------------------------------- /src/slam_ctrl/scripts/common/transform_tools.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | import numpy as np 4 | import math 5 | 6 | def isRotationMatrix(R): 7 | Rt = np.transpose(R) 8 | shouldBeIdentity = np.dot(Rt, R) 9 | I = np.identity(3, dtype=R.dtype) 10 | n = np.linalg.norm(I - shouldBeIdentity) 11 | return n < 1e-6 12 | 13 | 14 | def rotationMatrixToEulerAngles(R): 15 | assert (isRotationMatrix(R)) 16 | 17 | sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0]) 18 | 19 | singular = sy < 1e-6 20 | 21 | if not singular: 22 | x = math.atan2(R[2, 1], R[2, 2]) 23 | y = math.atan2(-R[2, 0], sy) 24 | z = math.atan2(R[1, 0], R[0, 0]) 25 | else: 26 | x = math.atan2(-R[1, 2], R[1, 1]) 27 | y = math.atan2(-R[2, 0], sy) 28 | z = 0 29 | 30 | return np.array([x, y, z]) 31 | 32 | 33 | def eulerAnglesToRotationMatrix(theta): 34 | R_x = np.array([[1, 0, 0], 35 | [0, math.cos(theta[0]), -math.sin(theta[0])], 36 | [0, math.sin(theta[0]), math.cos(theta[0])] 37 | ]) 38 | 39 | R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])], 40 | [0, 1, 0], 41 | [-math.sin(theta[1]), 0, math.cos(theta[1])] 42 | ]) 43 | 44 | R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0], 45 | [math.sin(theta[2]), math.cos(theta[2]), 0], 46 | [0, 0, 1] 47 | ]) 48 | 49 | R = np.dot(R_z, np.dot(R_y, R_x)) 50 | 51 | return R 52 | -------------------------------------------------------------------------------- /src/slam_ctrl/scripts/test.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/slam_ctrl/scripts/test.jpg -------------------------------------------------------------------------------- /src/slam_ctrl/scripts/usb_camera.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | time: "2020年07月13日 星期一 14时34分45秒" 4 | image_width: 1280 5 | image_height: 720 6 | cameraMatrix: !!opencv-matrix 7 | rows: 3 8 | cols: 3 9 | dt: d 10 | data: [ 1.4292452281580436e+03, 0., 6.3779945691363878e+02, 0., 11 | 1.4299496653334040e+03, 4.0885870902509140e+02, 0., 0., 1. ] 12 | distCoeffs: !!opencv-matrix 13 | rows: 5 14 | cols: 1 15 | dt: d 16 | data: [ -1.7216306760505651e-01, 3.8721873727076400e-01, 17 | 8.2316134233809070e-04, -4.6363980158779859e-04, 18 | -1.6428868286626035e+00 ] 19 | -------------------------------------------------------------------------------- /src/ui_sync/scripts/ui_sync_node.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # coding: utf-8 3 | 4 | import rospy 5 | from Connector import Connector 6 | 7 | from std_msgs.msg import Float32MultiArray 8 | from itheima_msgs.msg import AssemblyLine, AssemblyIR 9 | import time 10 | 11 | 12 | def aubo_joints_callback(msg): 13 | if not isinstance(msg, Float32MultiArray): 14 | return 15 | 16 | connector.send_aubo_joints(msg.data) 17 | 18 | 19 | def assembly_line_callback(msg): 20 | if not isinstance(msg, AssemblyLine): 21 | return 22 | lines = [msg.line_1, msg.line_2, msg.line_3, msg.line_4] 23 | connector.send_assembly_line(lines) 24 | 25 | 26 | def assembly_ir_callback(msg): 27 | if not isinstance(msg, AssemblyIR): 28 | return 29 | irs = [msg.ir_1, msg.ir_2] 30 | connector.send_assembly_ir(irs) 31 | 32 | 33 | def conn_cb(is_connected): 34 | if not is_connected: 35 | connector.connect(callback=conn_cb) 36 | time.sleep(3) 37 | 38 | 39 | if __name__ == '__main__': 40 | # 创建node 41 | node_name = "ui_sync_node" 42 | rospy.init_node(node_name) 43 | 44 | ui_host = rospy.get_param("~ui_host", "192.168.1.100") 45 | ui_port = rospy.get_param("~ui_port", 10008) 46 | 47 | connector = Connector(ip=ui_host, port=ui_port) 48 | connector.connect(callback=conn_cb) 49 | 50 | # aubo joints 51 | rospy.Subscriber("/aubo/joints", Float32MultiArray, aubo_joints_callback) 52 | 53 | # assembly line 54 | rospy.Subscriber("/assembly/line_state", AssemblyLine, assembly_line_callback) 55 | 56 | # assembly ir 57 | rospy.Subscriber("/assembly/ir_state", AssemblyIR, assembly_ir_callback) 58 | 59 | rospy.spin() 60 | -------------------------------------------------------------------------------- /src/usb_cam/.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: trusty 3 | language: generic 4 | compiler: 5 | - gcc 6 | 7 | branches: 8 | only: 9 | - master 10 | - develop 11 | 12 | install: 13 | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' 14 | - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 15 | - sudo apt-get update -qq 16 | - sudo apt-get install python-catkin-pkg python-rosdep ros-indigo-catkin -qq 17 | - sudo rosdep init 18 | - rosdep update 19 | - mkdir -p /tmp/ws/src 20 | - ln -s `pwd` /tmp/ws/src/package 21 | - cd /tmp/ws 22 | - rosdep install --from-paths src --ignore-src --rosdistro indigo -y 23 | 24 | script: 25 | - source /opt/ros/indigo/setup.bash 26 | - catkin_make 27 | - catkin_make install 28 | -------------------------------------------------------------------------------- /src/usb_cam/AUTHORS.md: -------------------------------------------------------------------------------- 1 | Original Authors 2 | ---------------- 3 | 4 | * [Benjamin Pitzer] (benjamin.pitzer@bosch.com) 5 | 6 | Contributors 7 | ------------ 8 | 9 | * [Russell Toris](http://users.wpi.edu/~rctoris/) (rctoris@wpi.edu) 10 | -------------------------------------------------------------------------------- /src/usb_cam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(usb_cam) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS image_transport roscpp std_msgs std_srvs sensor_msgs camera_info_manager) 8 | 9 | ## pkg-config libraries 10 | find_package(PkgConfig REQUIRED) 11 | pkg_check_modules(avcodec libavcodec REQUIRED) 12 | pkg_check_modules(swscale libswscale REQUIRED) 13 | 14 | ################################################### 15 | ## Declare things to be passed to other projects ## 16 | ################################################### 17 | 18 | ## LIBRARIES: libraries you create in this project that dependent projects also need 19 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 20 | ## DEPENDS: system dependencies of this project that dependent projects also need 21 | catkin_package( 22 | INCLUDE_DIRS include 23 | LIBRARIES ${PROJECT_NAME} 24 | ) 25 | 26 | ########### 27 | ## Build ## 28 | ########### 29 | 30 | include_directories(include 31 | ${catkin_INCLUDE_DIRS} 32 | ${avcodec_INCLUDE_DIRS} 33 | ${swscale_INCLUDE_DIRS} 34 | ) 35 | 36 | ## Build the USB camera library 37 | add_library(${PROJECT_NAME} src/usb_cam.cpp) 38 | target_link_libraries(${PROJECT_NAME} 39 | ${avcodec_LIBRARIES} 40 | ${swscale_LIBRARIES} 41 | ${catkin_LIBRARIES} 42 | ) 43 | 44 | ## Declare a cpp executable 45 | add_executable(${PROJECT_NAME}_node nodes/usb_cam_node.cpp) 46 | target_link_libraries(${PROJECT_NAME}_node 47 | ${PROJECT_NAME} 48 | ${avcodec_LIBRARIES} 49 | ${swscale_LIBRARIES} 50 | ${catkin_LIBRARIES} 51 | ) 52 | 53 | ############# 54 | ## Install ## 55 | ############# 56 | 57 | ## Mark executables and/or libraries for installation 58 | install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME} 59 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 60 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | ) 62 | 63 | ## Copy launch files 64 | install(DIRECTORY launch/ 65 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 66 | FILES_MATCHING PATTERN "*.launch" 67 | ) 68 | 69 | install(DIRECTORY include/${PROJECT_NAME}/ 70 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 71 | FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 72 | ) 73 | -------------------------------------------------------------------------------- /src/usb_cam/LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2014, Robert Bosch LLC. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above 13 | copyright notice, this list of conditions and the following 14 | disclaimer in the documentation and/or other materials provided 15 | with the distribution. 16 | * Neither the name of the Robert Bosch LLC. nor the names of its 17 | contributors may be used to endorse or promote products derived 18 | from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | -------------------------------------------------------------------------------- /src/usb_cam/README.md: -------------------------------------------------------------------------------- 1 | usb_cam [![Build Status](https://api.travis-ci.org/bosch-ros-pkg/usb_cam.png)](https://travis-ci.org/bosch-ros-pkg/usb_cam) 2 | ======= 3 | 4 | #### A ROS Driver for V4L USB Cameras 5 | This package is based off of V4L devices specifically instead of just UVC. 6 | 7 | For full documentation, see [the ROS wiki](http://ros.org/wiki/usb_cam). 8 | 9 | [Doxygen](http://docs.ros.org/indigo/api/usb_cam/html/) files can be found on the ROS wiki. 10 | 11 | ### License 12 | usb_cam is released with a BSD license. For full terms and conditions, see the [LICENSE](LICENSE) file. 13 | 14 | ### Authors 15 | See the [AUTHORS](AUTHORS.md) file for a full list of contributors. 16 | -------------------------------------------------------------------------------- /src/usb_cam/data/usb_camera.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | time: "2020年07月08日 星期三 16时31分02秒" 4 | image_width: 1280 5 | image_height: 960 6 | cameraMatrix: !!opencv-matrix 7 | rows: 3 8 | cols: 3 9 | dt: d 10 | data: [ 1.4212673919432768e+03, 0., 6.6919399774968952e+02, 0., 11 | 1.4200941883011269e+03, 4.9281122990898785e+02, 0., 0., 1. ] 12 | distCoeffs: !!opencv-matrix 13 | rows: 5 14 | cols: 1 15 | dt: d 16 | data: [ -6.9998445942761871e-03, 5.7190053200923199e-02, 17 | 8.1106198326906900e-04, 4.9764755587529382e-03, 18 | -1.7446651936751376e-01 ] 19 | -------------------------------------------------------------------------------- /src/usb_cam/launch/usb_cam-test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /src/usb_cam/launch/uvc_cam-test.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 | -------------------------------------------------------------------------------- /src/usb_cam/launch/uvc_client.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/usb_cam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | usb_cam 3 | 0.3.6 4 | A ROS Driver for V4L USB Cameras 5 | 6 | Russell Toris 7 | ROS Orphaned Package Maintainers 8 | Benjamin Pitzer 9 | 10 | BSD 11 | 12 | http://wiki.ros.org/usb_cam 13 | https://github.com/bosch-ros-pkg/usb_cam/issues 14 | https://github.com/bosch-ros-pkg/usb_cam 15 | 16 | catkin 17 | 18 | image_transport 19 | roscpp 20 | std_msgs 21 | std_srvs 22 | sensor_msgs 23 | ffmpeg 24 | camera_info_manager 25 | 26 | image_transport 27 | roscpp 28 | std_msgs 29 | std_srvs 30 | sensor_msgs 31 | ffmpeg 32 | camera_info_manager 33 | v4l-utils 34 | 35 | -------------------------------------------------------------------------------- /src/usb_cam/src/LICENSE: -------------------------------------------------------------------------------- 1 | Video for Linux Two API Specification 2 | Revision 0.24 3 | Michael H Schimek 4 | Bill Dirks 5 | Hans Verkuil 6 | Martin Rubli 7 | 8 | Copyright © 1999, 2000, 2001, 2002, 2003, 2004, 2005, 2006, 2007, 2008 Bill Dirks, Michael H. Schimek, Hans Verkuil, Martin Rubli 9 | 10 | This document is copyrighted © 1999-2008 by Bill Dirks, Michael H. Schimek, Hans Verkuil and Martin Rubli. 11 | 12 | Permission is granted to copy, distribute and/or modify this document under the terms of the GNU Free Documentation License, Version 1.1 or any later version published by the Free Software Foundation; with no Invariant Sections, with no Front-Cover Texts, and with no Back-Cover Texts. A copy of the license is included in the appendix entitled "GNU Free Documentation License". 13 | 14 | Programming examples can be used and distributed without restrictions. 15 | -------------------------------------------------------------------------------- /src/usb_cam/src/usb_reset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/itheima1/ros_mini_industry/11ea00fef606dd8f31812224bcf05440a75b0f63/src/usb_cam/src/usb_reset -------------------------------------------------------------------------------- /src/usb_cam/src/usb_reset.c: -------------------------------------------------------------------------------- 1 | /* usbreset -- send a USB port reset to a USB device */ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | int main(int argc, char **argv) { 11 | 12 | const char *filename; 13 | int fd; 14 | int rc; 15 | if (argc != 2) { 16 | fprintf(stderr, "Usage: usbreset device-filename"); 17 | return 1; 18 | } 19 | filename = argv[1]; 20 | 21 | fd = open(filename, O_WRONLY); 22 | 23 | if (fd < 0) { 24 | perror("Error opening output file"); 25 | return 1; 26 | } 27 | printf("Resetting USB device %s", filename); 28 | 29 | rc = ioctl(fd, USBDEVFS_RESET, 0); 30 | if (rc < 0) { 31 | perror("Error in ioctl"); 32 | return 1; 33 | } 34 | printf("Reset successful"); 35 | close(fd); 36 | 37 | return 0; 38 | } 39 | --------------------------------------------------------------------------------