├── README.md ├── examples ├── turtlebot3_manipulation │ ├── sigverse_turtlebot3_manipulation_moveit_config │ │ ├── config │ │ │ ├── kinematics.yaml │ │ │ ├── sensors_3d.yaml │ │ │ ├── moveit_controllers.yaml │ │ │ ├── ompl_planning.yaml │ │ │ ├── ompl_planning_for_jazzy.yaml │ │ │ ├── joint_limits.yaml │ │ │ └── turtlebot3_manipulation.srdf │ │ ├── CMakeLists.txt │ │ └── package.xml │ ├── THIRD_PARTY_LICENSES.txt │ ├── sigverse_turtlebot3_manipulation_description │ │ ├── CMakeLists.txt │ │ ├── package.xml │ │ ├── urdf │ │ │ ├── common_properties.urdf │ │ │ └── turtlebot3_manipulation.urdf.xacro │ │ └── ros2_control │ │ │ └── turtlebot3_manipulation_system.ros2_control.xacro │ └── sigverse_turtlebot3_manipulation_hardware │ │ ├── plugin.xml │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ ├── include │ │ └── sigverse_turtlebot3_manipulation_hardware │ │ │ └── sigverse_turtlebot3_system.hpp │ │ └── src │ │ └── sigverse_turtlebot3_system.cpp ├── tiago │ ├── README.md │ ├── package.xml │ ├── CMakeLists.txt │ └── launch │ │ ├── teleop_key_launch.xml │ │ ├── teleop_key_rviz_launch.xml │ │ └── tiago.rviz ├── hsr │ ├── teleop_key │ │ ├── README.md │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ └── launch │ │ │ ├── teleop_key_launch.xml │ │ │ ├── teleop_key_rviz_launch.xml │ │ │ └── hsr.rviz │ └── hsr_joint_state_2_joint_trajectory │ │ ├── launch │ │ ├── display_launch.xml │ │ └── hsr_joint_state_2_joint_trajectory_launch.xml │ │ ├── README.md │ │ ├── CMakeLists.txt │ │ ├── package.xml │ │ └── src │ │ └── hsr_joint_state_2_joint_trajectory.cpp └── turtlebot3 │ ├── config │ └── turtlebot3_controller_manager.yaml │ ├── README.md │ ├── package.xml │ ├── launch │ ├── teleop_key_launch.xml │ ├── slam_launch.xml │ ├── recognize_pointed_direction_launch.xml │ ├── grasping_auto_launch.py │ ├── recognize_pointed_direction.rviz │ ├── slam.rviz │ └── grasping_auto_launch.rviz │ ├── CMakeLists.txt │ └── src │ ├── grasping_auto.hpp │ ├── recognize_pointed_direction.cpp │ ├── turtlebot3_teleop_key.cpp │ └── grasping_auto.cpp ├── sigverse_ros_bridge ├── launch │ └── sigverse_ros_bridge_launch.xml ├── package.xml ├── README.md ├── CMakeLists.txt └── src │ ├── sigverse_ros_bridge.hpp │ └── sigverse_ros_bridge.cpp └── .gitignore /README.md: -------------------------------------------------------------------------------- 1 | # sigverse_ros_package 2 | 3 | Please refer to the Wiki for details: 4 | https://github.com/SIGVerse/sigverse_unity_project/wiki 5 | 6 | This project uses ROS 2 Humble. 7 | For the older version that used ROS 1, please refer to the ros1 branch. 8 | 9 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout: 0.05 5 | position_only_ik: True 6 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/THIRD_PARTY_LICENSES.txt: -------------------------------------------------------------------------------- 1 | This project includes code copied from the TurtleBot3 project: 2 | https://github.com/ROBOTIS-GIT/turtlebot3 3 | https://github.com/ROBOTIS-GIT/turtlebot3_manipulation 4 | 5 | License: Apache License 2.0 6 | License text: https://www.apache.org/licenses/LICENSE-2.0 7 | 8 | -------------------------------------------------------------------------------- /examples/tiago/README.md: -------------------------------------------------------------------------------- 1 | # sigverse_tiago_teleop_key 2 | 3 | Keyboard teleoperation for TIAGo. 4 | 5 | ## How To Use 6 | 7 | ```bash 8 | $ ros2 launch sigverse_tiago_teleop_key teleop_key_launch.xml 9 | ``` 10 | 11 | With RViz2 12 | ```bash 13 | $ ros2 launch sigverse_tiago_teleop_key teleop_key_rviz_launch.xml 14 | ``` 15 | 16 | -------------------------------------------------------------------------------- /examples/hsr/teleop_key/README.md: -------------------------------------------------------------------------------- 1 | # sigverse_hsr_teleop_key 2 | 3 | Keyboard teleoperation for Toyota HSR. 4 | 5 | ## How To Use 6 | 7 | ```bash 8 | $ ros2 launch sigverse_hsr_teleop_key teleop_key_launch.xml 9 | ``` 10 | 11 | With RViz2 12 | ```bash 13 | $ ros2 launch sigverse_hsr_teleop_key teleop_key_rviz_launch.xml 14 | ``` 15 | 16 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(sigverse_turtlebot3_manipulation_description) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | install(DIRECTORY urdf ros2_control 7 | DESTINATION share/${PROJECT_NAME} 8 | ) 9 | 10 | ament_package() 11 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_hardware/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(sigverse_turtlebot3_manipulation_moveit_config) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) 7 | #install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) 8 | 9 | ament_package() 10 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_moveit_config/config/sensors_3d.yaml: -------------------------------------------------------------------------------- 1 | # Dummy depth-based Octomap updater 2 | sensors: 3 | - camera_depth_image 4 | camera_depth_image: 5 | sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater 6 | image_topic: /camera_dummy/depth/image_raw 7 | queue_size: 5 8 | near_clipping_plane_distance: 0.1 9 | far_clipping_plane_distance: 12.0 10 | shadow_threshold: 0.2 11 | padding_scale: 1.0 12 | max_update_rate: 1.0 13 | # filtered_cloud_topic: /camera/filtered_points 14 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sigverse_turtlebot3_manipulation_moveit_config 4 | 0.1.0 5 | TurtleBot3 MoveIt config for SIGVerse (config-only package) 6 | inamura laboratory 7 | SIGVerse License 8 | 9 | ament_cmake 10 | 11 | 12 | ament_cmake 13 | 14 | 15 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_moveit_config/config/moveit_controllers.yaml: -------------------------------------------------------------------------------- 1 | trajectory_execution: 2 | allowed_execution_duration_scaling: 1.2 3 | allowed_goal_duration_margin: 0.5 4 | allowed_start_tolerance: 0.01 5 | moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager 6 | moveit_simple_controller_manager: 7 | controller_names: 8 | - arm_controller 9 | arm_controller: 10 | type: FollowJointTrajectory 11 | action_ns: follow_joint_trajectory 12 | default: true 13 | joints: 14 | - joint1 15 | - joint2 16 | - joint3 17 | - joint4 18 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_hardware/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sigverse_turtlebot3_manipulation_hardware 4 | 0.1.0 5 | TurtleBot3 hardware interface for SIGVerse 6 | inamura laboratory 7 | SIGVerse License 8 | 9 | ament_cmake 10 | hardware_interface 11 | pluginlib 12 | rclcpp 13 | realtime_tools 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sigverse_turtlebot3_manipulation_description 4 | 0.1.0 5 | TurtlebBt3 manipulation description for SIGVerse 6 | inamura laboratory 7 | SIGVerse License 8 | 9 | ament_cmake 10 | xacro 11 | turtlebot3_manipulation_description 12 | sigverse_turtlebot3_manipulation_hardware 13 | 14 | 15 | ament_cmake 16 | 17 | 18 | -------------------------------------------------------------------------------- /examples/turtlebot3/config/turtlebot3_controller_manager.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 90 4 | arm_controller: 5 | type: joint_trajectory_controller/JointTrajectoryController 6 | 7 | arm_controller: 8 | ros__parameters: 9 | joints: 10 | - joint1 11 | - joint2 12 | - joint3 13 | - joint4 14 | command_interfaces: 15 | - position 16 | state_interfaces: 17 | - position 18 | state_publish_rate: 50.0 # Defaults to 50 19 | action_monitor_rate: 20.0 # Defaults to 20 20 | allow_partial_joints_goal: false # Defaults to false 21 | open_loop_control: true 22 | constraints: 23 | stopped_velocity_tolerance: 0.01 # Defaults to 0.01 24 | goal_time: 0.0 # Defaults to 0.0 (start immediately) 25 | -------------------------------------------------------------------------------- /examples/turtlebot3/README.md: -------------------------------------------------------------------------------- 1 | # sigverse_turtlebot3_open_manipulator 2 | 3 | This package includes sample ROS nodes for TurtleBot3 equipped with an arm. 4 | 5 | ## Prerequisites 6 | 7 | ### SLAM 8 | - **slam-toolbox** 9 | 10 | ### recognize_pointed_direction 11 | - **yolo_ros** 12 | 13 | ### grasping_auto 14 | - **yolo_ros** 15 | - **moveit** 16 | 17 | ## How To Use 18 | 19 | teleop_key 20 | ```bash 21 | $ ros2 launch sigverse_turtlebot3 teleop_key_launch.xml 22 | ``` 23 | 24 | SLAM 25 | ```bash 26 | $ ros2 launch sigverse_turtlebot3 slam.launch 27 | ``` 28 | 29 | recognize_pointed_direction 30 | ```bash 31 | $ ros2 launch sigverse_turtlebot3 recognize_pointed_direction.launch 32 | ``` 33 | 34 | grasping_auto 35 | ```bash 36 | $ ros2 launch sigverse_turtlebot3 grasping_auto_launch.xml 37 | ``` 38 | 39 | -------------------------------------------------------------------------------- /examples/hsr/hsr_joint_state_2_joint_trajectory/launch/display_launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /sigverse_ros_bridge/launch/sigverse_ros_bridge_launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /sigverse_ros_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sigverse_ros_bridge 4 | 0.2.0 5 | The sigverse_ros_bridge package 6 | inamura laboratory 7 | SIGVerse License 8 | 9 | ament_cmake 10 | 11 | rclcpp 12 | geometry_msgs 13 | std_msgs 14 | sensor_msgs 15 | 16 | rclcpp 17 | geometry_msgs 18 | std_msgs 19 | sensor_msgs 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /examples/tiago/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sigverse_tiago_teleop_key 4 | 2.0.0 5 | Sample package to operate TIAGo with keyboard 6 | inamura laboratory 7 | SIGVerse License 8 | 9 | ament_cmake 10 | 11 | rclcpp 12 | std_msgs 13 | geometry_msgs 14 | sensor_msgs 15 | trajectory_msgs 16 | tf2_geometry_msgs 17 | 18 | rviz2 19 | rosbridge_server 20 | sigverse_ros_bridge 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /examples/hsr/hsr_joint_state_2_joint_trajectory/README.md: -------------------------------------------------------------------------------- 1 | # hsr_joint_state_2_joint_trajectory 2 | 3 | This package is to manipulate HSR via joint_state_publisher. 4 | 5 | ## Prerequisites 6 | 7 | - **xacro** 8 | - **joint_state_publisher_gui** 9 | - **hsrb_description** 10 | - **hsrb_meshes** 11 | 12 | For reference 13 | ```bash 14 | $ sudo apt install ros-$ROS_DISTRO-xacro 15 | $ sudo apt install ros-$ROS_DISTRO-joint-state-publisher-gui 16 | $ cd ~/ros2_ws/src 17 | $ git clone --single-branch --branch humble https://github.com/hsr-project/hsrb_description.git 18 | $ git clone --single-branch --branch humble https://github.com/hsr-project/hsrb_meshes.git 19 | $ cd ~/ros2_ws 20 | $ colcon build --symlink-install 21 | $ source ~/ros2_ws/install/setup.bash 22 | ``` 23 | 24 | ## How To Use 25 | 26 | ```bash 27 | $ ros2 launch hsr_joint_state_2_joint_trajectory hsr_joint_state_2_joint_trajectory_launch.xml 28 | ``` 29 | -------------------------------------------------------------------------------- /examples/hsr/teleop_key/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sigverse_hsr_teleop_key 4 | 2.0.0 5 | Sample package to operate Toyota HSR with keyboard 6 | inamura laboratory 7 | SIGVerse License 8 | 9 | ament_cmake 10 | 11 | rclcpp 12 | geometry_msgs 13 | std_msgs 14 | sensor_msgs 15 | trajectory_msgs 16 | tf2_geometry_msgs 17 | 20 | 21 | rviz2 22 | rosbridge_server 23 | sigverse_ros_bridge 24 | 27 | 28 | ament_cmake 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /examples/hsr/hsr_joint_state_2_joint_trajectory/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(hsr_joint_state_2_joint_trajectory) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rclcpp REQUIRED) 15 | find_package(std_msgs REQUIRED) 16 | find_package(sensor_msgs REQUIRED) 17 | find_package(trajectory_msgs REQUIRED) 18 | 19 | add_executable(hsr_joint_state_2_joint_trajectory src/hsr_joint_state_2_joint_trajectory.cpp) 20 | ament_target_dependencies(hsr_joint_state_2_joint_trajectory rclcpp std_msgs sensor_msgs trajectory_msgs) 21 | 22 | install( 23 | TARGETS hsr_joint_state_2_joint_trajectory 24 | DESTINATION lib/${PROJECT_NAME} 25 | ) 26 | 27 | install( 28 | DIRECTORY launch 29 | DESTINATION share/${PROJECT_NAME} 30 | ) 31 | 32 | ament_package() 33 | 34 | -------------------------------------------------------------------------------- /examples/tiago/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(sigverse_tiago_teleop_key) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rclcpp REQUIRED) 15 | find_package(std_msgs REQUIRED) 16 | find_package(geometry_msgs REQUIRED) 17 | find_package(sensor_msgs REQUIRED) 18 | find_package(trajectory_msgs REQUIRED) 19 | find_package(tf2_ros REQUIRED) 20 | find_package(tf2_geometry_msgs REQUIRED) 21 | 22 | add_executable(tiago_teleop_key src/tiago_teleop_key.cpp) 23 | 24 | ament_target_dependencies(tiago_teleop_key rclcpp std_msgs geometry_msgs sensor_msgs trajectory_msgs tf2_ros tf2_geometry_msgs) 25 | 26 | install( 27 | TARGETS tiago_teleop_key 28 | DESTINATION lib/${PROJECT_NAME} 29 | ) 30 | 31 | install( 32 | DIRECTORY launch 33 | DESTINATION share/${PROJECT_NAME} 34 | ) 35 | 36 | ament_package() 37 | 38 | 39 | -------------------------------------------------------------------------------- /examples/hsr/teleop_key/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(sigverse_hsr_teleop_key) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rclcpp REQUIRED) 15 | find_package(std_msgs REQUIRED) 16 | find_package(geometry_msgs REQUIRED) 17 | find_package(sensor_msgs REQUIRED) 18 | find_package(trajectory_msgs REQUIRED) 19 | find_package(tf2_ros REQUIRED) 20 | find_package(tf2_geometry_msgs REQUIRED) 21 | # find_package(tmc_suction REQUIRED) 22 | 23 | add_executable(hsr_teleop_key src/hsr_teleop_key.cpp) 24 | 25 | ament_target_dependencies(hsr_teleop_key rclcpp std_msgs geometry_msgs sensor_msgs trajectory_msgs tf2_ros tf2_geometry_msgs) 26 | 27 | install( 28 | TARGETS hsr_teleop_key 29 | DESTINATION lib/${PROJECT_NAME} 30 | ) 31 | 32 | install( 33 | DIRECTORY launch 34 | DESTINATION share/${PROJECT_NAME} 35 | ) 36 | 37 | ament_package() 38 | 39 | -------------------------------------------------------------------------------- /examples/hsr/hsr_joint_state_2_joint_trajectory/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hsr_joint_state_2_joint_trajectory 4 | 2.0.0 5 | The hsr_joint_state_2_joint_trajectory package 6 | inamura laboratory 7 | SIGVerse License 8 | 9 | ament_cmake 10 | 11 | rclcpp 12 | geometry_msgs 13 | std_msgs 14 | sensor_msgs 15 | trajectory_msgs 16 | tf2_geometry_msgs 17 | 18 | xacro 19 | joint_state_publisher_gui 20 | robot_state_publisher 21 | hsrb_description 22 | hsrb_meshes 23 | rviz2 24 | rosbridge_server 25 | sigverse_ros_bridge 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_description/urdf/common_properties.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | -------------------------------------------------------------------------------- /examples/turtlebot3/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sigverse_turtlebot3 4 | 2.0.0 5 | Sample package for TurtleBot3 with OpenManipulator Chain 6 | inamura laboratory 7 | SIGVerse License 8 | 9 | ament_cmake 10 | 11 | rclcpp 12 | std_msgs 13 | geometry_msgs 14 | sensor_msgs 15 | trajectory_msgs 16 | builtin_interfaces 17 | tf2_ros 18 | yolo_msgs 19 | visualization_msgs 20 | moveit_ros_planning_interface 21 | 22 | sigverse_turtlebot3_manipulation_hardware 23 | sigverse_turtlebot3_manipulation_description 24 | sigverse_turtlebot3_manipulation_moveit_config 25 | rosbridge_server 26 | sigverse_ros_bridge 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /sigverse_ros_bridge/README.md: -------------------------------------------------------------------------------- 1 | ## Setup 2 | 3 | ### Install Mongo C Driver 4 | 5 | ```bash: 6 | $ cd ~/Downloads 7 | $ wget https://github.com/mongodb/mongo-c-driver/releases/download/1.4.2/mongo-c-driver-1.4.2.tar.gz 8 | $ tar zxvf mongo-c-driver-1.4.2.tar.gz 9 | $ cd mongo-c-driver-1.4.2 10 | $ ./configure 11 | $ make 12 | $ sudo make install 13 | ``` 14 | 15 | ### Install Mongo C++ Driver 16 | 17 | ```bash: 18 | $ cd ~/Downloads 19 | $ wget https://github.com/mongodb/mongo-cxx-driver/archive/r3.0.3.tar.gz 20 | $ tar zxvf r3.0.3.tar.gz 21 | $ cd mongo-cxx-driver-r3.0.3/build 22 | $ cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr/local -DLIBMONGOC_DIR=/usr/local -DLIBBSON_DIR=/usr/local .. 23 | $ sudo make EP_mnmlstc_core 24 | $ make 25 | $ sudo make install 26 | ``` 27 | 28 | ### Install SIGVerse ROS Bridge 29 | 30 | ```bash: 31 | $ cd ~/catkin_ws/src 32 | $ git clone https://github.com/SIGVerse/sigverse_ros_package.git 33 | $ cd .. 34 | $ catkin_make 35 | ``` 36 | 37 | 38 | ## How to use 39 | 40 | ```bash: 41 | $ rosrun sigverse_ros_bridge sigverse_ros_bridge 42 | ``` 43 | 44 | Default port number is 50001. 45 | If you need change the port number, please pass the argument. 46 | 47 | ```bash 48 | $ rosrun sigverse_ros_bridge sigverse_ros_bridge 12345 49 | ``` 50 | 51 | 52 | -------------------------------------------------------------------------------- /sigverse_ros_bridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(sigverse_ros_bridge) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rclcpp REQUIRED) 15 | find_package(std_msgs REQUIRED) 16 | find_package(geometry_msgs REQUIRED) 17 | find_package(sensor_msgs REQUIRED) 18 | find_package(tf2_ros REQUIRED) 19 | 20 | find_package(Boost REQUIRED COMPONENTS 21 | thread 22 | ) 23 | 24 | include_directories( 25 | include 26 | /usr/local/include 27 | /usr/local/include/bsoncxx/v_noabi 28 | ) 29 | 30 | link_directories(/usr/local/lib) 31 | 32 | add_executable(sigverse_ros_bridge src/sigverse_ros_bridge.cpp) 33 | 34 | target_link_libraries(sigverse_ros_bridge 35 | mongocxx 36 | bsoncxx 37 | ) 38 | 39 | ament_target_dependencies(sigverse_ros_bridge rclcpp std_msgs geometry_msgs sensor_msgs tf2_ros) 40 | 41 | install( 42 | TARGETS sigverse_ros_bridge 43 | DESTINATION lib/${PROJECT_NAME} 44 | ) 45 | 46 | install( 47 | DIRECTORY launch 48 | DESTINATION share/${PROJECT_NAME} 49 | ) 50 | 51 | ament_package() 52 | 53 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_moveit_config/config/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | planning_plugin: ompl_interface/OMPLPlanner 2 | request_adapters: >- 3 | default_planner_request_adapters/FixWorkspaceBounds 4 | default_planner_request_adapters/FixStartStateBounds 5 | default_planner_request_adapters/FixStartStateCollision 6 | default_planner_request_adapters/FixStartStatePathConstraints 7 | default_planner_request_adapters/ResolveConstraintFrames 8 | default_planner_request_adapters/AddTimeOptimalParameterization 9 | 10 | planner_configs: 11 | RRTConnectkConfigDefault: 12 | type: geometric::RRTConnect 13 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 14 | RRTstarkConfigDefault: 15 | type: geometric::RRTstar 16 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 17 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 18 | delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 19 | PRMstarkConfigDefault: 20 | type: geometric::PRMstar 21 | 22 | arm: 23 | planner_configs: 24 | - RRTConnectkConfigDefault 25 | - RRTstarkConfigDefault 26 | - PRMstarkConfigDefault 27 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_hardware/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(sigverse_turtlebot3_manipulation_hardware) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(hardware_interface REQUIRED) 15 | find_package(pluginlib REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(realtime_tools REQUIRED) 18 | 19 | add_library(sigverse_turtlebot3_system SHARED src/sigverse_turtlebot3_system.cpp) 20 | ament_target_dependencies(sigverse_turtlebot3_system hardware_interface pluginlib rclcpp realtime_tools) 21 | 22 | target_include_directories(sigverse_turtlebot3_system PUBLIC include) 23 | pluginlib_export_plugin_description_file(hardware_interface plugin.xml) 24 | 25 | install(TARGETS sigverse_turtlebot3_system 26 | ARCHIVE DESTINATION lib 27 | LIBRARY DESTINATION lib 28 | RUNTIME DESTINATION bin) 29 | 30 | install(FILES plugin.xml DESTINATION share/${PROJECT_NAME}) 31 | 32 | ament_export_include_directories(include) 33 | ament_export_libraries(sigverse_turtlebot3_system) 34 | ament_package() 35 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_moveit_config/config/ompl_planning_for_jazzy.yaml: -------------------------------------------------------------------------------- 1 | planning_plugins: 2 | - ompl_interface/OMPLPlanner 3 | 4 | request_adapters: 5 | - default_planning_request_adapters/ValidateWorkspaceBounds 6 | - default_planning_request_adapters/CheckStartStateBounds 7 | - default_planning_request_adapters/CheckStartStateCollision 8 | - default_planning_request_adapters/ResolveConstraintFrames 9 | 10 | response_adapters: 11 | - default_planning_response_adapters/AddTimeOptimalParameterization 12 | 13 | planner_configs: 14 | RRTConnectkConfigDefault: 15 | type: geometric::RRTConnect 16 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 17 | RRTstarkConfigDefault: 18 | type: geometric::RRTstar 19 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 20 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 21 | delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 22 | PRMstarkConfigDefault: 23 | type: geometric::PRMstar 24 | 25 | arm: 26 | planner_configs: 27 | - RRTConnectkConfigDefault 28 | - RRTstarkConfigDefault 29 | - PRMstarkConfigDefault 30 | -------------------------------------------------------------------------------- /examples/turtlebot3/launch/teleop_key_launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /examples/hsr/hsr_joint_state_2_joint_trajectory/launch/hsr_joint_state_2_joint_trajectory_launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_description/urdf/turtlebot3_manipulation.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /examples/tiago/launch/teleop_key_launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /examples/turtlebot3/launch/slam_launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | 3 | # For beginners, we downscale velocity and acceleration limits. 4 | # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. 5 | default_velocity_scaling_factor: 0.1 6 | default_acceleration_scaling_factor: 0.1 7 | 8 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 9 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 10 | joint_limits: 11 | # gripper_left_joint: 12 | # has_velocity_limits: true 13 | # max_velocity: 4.7999999999999998 14 | # has_acceleration_limits: false 15 | # max_acceleration: 0 16 | # gripper_right_joint: 17 | # has_velocity_limits: true 18 | # max_velocity: 4.7999999999999998 19 | # has_acceleration_limits: false 20 | # max_acceleration: 0 21 | joint1: 22 | has_velocity_limits: true 23 | max_velocity: 4.7999999999999998 24 | has_acceleration_limits: true 25 | max_acceleration: 5.0 26 | joint2: 27 | has_velocity_limits: true 28 | max_velocity: 4.7999999999999998 29 | has_acceleration_limits: true 30 | max_acceleration: 5.0 31 | joint3: 32 | has_velocity_limits: true 33 | max_velocity: 4.7999999999999998 34 | has_acceleration_limits: true 35 | max_acceleration: 5.0 36 | joint4: 37 | has_velocity_limits: true 38 | max_velocity: 4.7999999999999998 39 | has_acceleration_limits: true 40 | max_acceleration: 5.0 41 | -------------------------------------------------------------------------------- /examples/tiago/launch/teleop_key_rviz_launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /examples/turtlebot3/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(sigverse_turtlebot3) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | set(CURSES_NEED_WIDE TRUE) 10 | 11 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 12 | add_compile_options(-Wall -Wextra -Wpedantic) 13 | endif() 14 | 15 | find_package(Curses REQUIRED) 16 | find_package(ament_cmake REQUIRED) 17 | find_package(rclcpp REQUIRED) 18 | find_package(std_msgs REQUIRED) 19 | find_package(geometry_msgs REQUIRED) 20 | find_package(sensor_msgs REQUIRED) 21 | find_package(trajectory_msgs REQUIRED) 22 | find_package(builtin_interfaces REQUIRED) 23 | find_package(tf2_ros REQUIRED) 24 | find_package(yolo_msgs REQUIRED) 25 | find_package(visualization_msgs REQUIRED) 26 | find_package(moveit_ros_planning_interface REQUIRED) 27 | 28 | # teleop_key 29 | add_executable(turtlebot3_teleop_key src/turtlebot3_teleop_key.cpp) 30 | ament_target_dependencies(turtlebot3_teleop_key rclcpp std_msgs geometry_msgs sensor_msgs trajectory_msgs builtin_interfaces tf2_ros) 31 | 32 | # recognize_pointed_direction 33 | add_executable(recognize_pointed_direction src/recognize_pointed_direction.cpp) 34 | ament_target_dependencies(recognize_pointed_direction rclcpp std_msgs geometry_msgs sensor_msgs trajectory_msgs builtin_interfaces tf2_ros yolo_msgs visualization_msgs) 35 | 36 | # grasping_auto 37 | add_executable(grasping_auto src/grasping_auto.cpp) 38 | ament_target_dependencies(grasping_auto rclcpp std_msgs geometry_msgs sensor_msgs trajectory_msgs builtin_interfaces tf2_ros yolo_msgs visualization_msgs moveit_ros_planning_interface) 39 | target_link_libraries(grasping_auto ${CURSES_LIBRARIES}) 40 | target_include_directories(grasping_auto PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src ${CURSES_INCLUDE_DIR}) 41 | 42 | install( 43 | TARGETS turtlebot3_teleop_key 44 | recognize_pointed_direction 45 | grasping_auto 46 | DESTINATION lib/${PROJECT_NAME} 47 | ) 48 | 49 | install( 50 | DIRECTORY launch config 51 | DESTINATION share/${PROJECT_NAME} 52 | ) 53 | 54 | ament_package() 55 | 56 | -------------------------------------------------------------------------------- /examples/hsr/teleop_key/launch/teleop_key_launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /examples/hsr/teleop_key/launch/teleop_key_rviz_launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /examples/turtlebot3/launch/recognize_pointed_direction_launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /sigverse_ros_bridge/src/sigverse_ros_bridge.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SIGVERSE_ROS_BRIDGE_HPP 2 | #define SIGVERSE_ROS_BRIDGE_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "rclcpp/rclcpp.hpp" 19 | #include "std_msgs/msg/string.hpp" 20 | #include "geometry_msgs/msg/twist.hpp" 21 | #include "geometry_msgs/msg/transform_stamped.hpp" 22 | #include "sensor_msgs/msg/camera_info.hpp" 23 | #include "sensor_msgs/msg/image.hpp" 24 | #include "sensor_msgs/msg/laser_scan.hpp" 25 | #include "tf2_ros/transform_broadcaster.h" 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | #define TYPE_TWIST "geometry_msgs/msg/Twist" 43 | #define TYPE_CAMERA_INFO "sensor_msgs/msg/CameraInfo" 44 | #define TYPE_IMAGE "sensor_msgs/msg/Image" 45 | #define TYPE_LASER_SCAN "sensor_msgs/msg/LaserScan" 46 | #define TYPE_TIME_SYNC "sigverse/TimeSync" 47 | #define TYPE_TF_LIST "sigverse/TfList" 48 | 49 | #define BUFFER_SIZE 25*1024*1024 //100MB 50 | 51 | #define DEFAULT_PORT 50001 52 | #define DEFAULT_SYNC_TIME_MAX_NUM 1 53 | 54 | class SIGVerseROSBridge 55 | { 56 | private: 57 | static pid_t get_tid(void); 58 | 59 | static bool check_receivable (int fd ); 60 | 61 | static void set_vector_double(std::vector &destVec, const bsoncxx::array::view &arrayView); 62 | static void set_vector_float (std::vector &destVec, const bsoncxx::array::view &arrayView); 63 | 64 | template < size_t ArrayNum > 65 | static void set_array_double(std::array &vec, const bsoncxx::array::view &arrayView); 66 | 67 | template < size_t ArrayNum > 68 | static void set_array_double(boost::array &vec, const bsoncxx::array::view &arrayView); 69 | 70 | static void *receiving_thread(void *param); 71 | 72 | static int syncTimeCnt; 73 | static int syncTimeMaxNum; 74 | 75 | public: 76 | int run(int argc, char **argv); 77 | }; 78 | 79 | #endif // SIGVERSE_ROS_BRIDGE_HPP 80 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_hardware/include/sigverse_turtlebot3_manipulation_hardware/sigverse_turtlebot3_system.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SIGVERSE_TURTLEBOT3_MANIPULATION_HARDWARE__SIGVERSE_TURTLEBOT3_SYSTEM_HPP_ 2 | #define SIGVERSE_TURTLEBOT3_MANIPULATION_HARDWARE__SIGVERSE_TURTLEBOT3_SYSTEM_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "hardware_interface/handle.hpp" 11 | #include "hardware_interface/hardware_info.hpp" 12 | #include "hardware_interface/system_interface.hpp" 13 | #include "hardware_interface/types/hardware_interface_return_values.hpp" 14 | #include "rclcpp/clock.hpp" 15 | #include "rclcpp/duration.hpp" 16 | #include "rclcpp/macros.hpp" 17 | #include "rclcpp/time.hpp" 18 | #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" 19 | #include "rclcpp_lifecycle/state.hpp" 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace sigverse 27 | { 28 | class SIGVerseTurtleBot3System : public hardware_interface::SystemInterface 29 | { 30 | public: 31 | hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; 32 | std::vector export_state_interfaces() override; 33 | std::vector export_command_interfaces() override; 34 | hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; 35 | hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; 36 | hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; 37 | hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; 38 | 39 | private: 40 | const double kJointCommandDelta = 0.001; // 0.001 rad = 0.057 degrees 41 | // const double kJointUpdateInterval = 0.1; // sec 42 | 43 | bool has_joint_command_changed() const noexcept; 44 | 45 | std::unordered_map joint_position_map_; 46 | std::unordered_map joint_position_command_map_; 47 | std::unordered_map last_sent_joint_pos_cmd_map_; 48 | 49 | realtime_tools::RealtimeBuffer> joint_states_buffer_; 50 | 51 | rclcpp::Node::SharedPtr node_; 52 | rclcpp::Subscription::SharedPtr joint_state_sub_; 53 | std::shared_ptr> realtime_joint_trajectory_pub_; 54 | std::shared_ptr> joint_trajectory_pub_; 55 | 56 | std::unique_ptr thread_executor_; 57 | std::thread spin_thread_; 58 | 59 | rclcpp::Time last_sent_joint_pos_time_{0,0}; 60 | 61 | double joint_trajectory_pub_interval_; 62 | }; 63 | } // namespace sigverse 64 | #endif // SIGVERSE_TURTLEBOT3_MANIPULATION_HARDWARE__SIGVERSE_TURTLEBOT3_SYSTEM_HPP_ 65 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Compiled Object files 3 | *.slo 4 | *.lo 5 | *.o 6 | *.d 7 | 8 | # Compiled Dynamic libraries 9 | *.so 10 | *.dylib 11 | 12 | # Compiled Static libraries 13 | *.lai 14 | *.la 15 | *.a 16 | 17 | # Eclipse 18 | .settings/ 19 | .cproject 20 | .project 21 | 22 | # CLion 23 | .idea 24 | 25 | # etc 26 | *.dat 27 | 28 | 29 | 30 | ## Ignore Visual Studio temporary files, build results, and 31 | ## files generated by popular Visual Studio add-ons. 32 | 33 | # User-specific files 34 | *.suo 35 | *.user 36 | *.sln.docstates 37 | 38 | # Build results 39 | [Dd]ebug/ 40 | [Dd]ebugPublic/ 41 | [Rr]elease/ 42 | x64/ 43 | build/ 44 | bld/ 45 | [Bb]in/ 46 | [Oo]bj/ 47 | 48 | # Roslyn cache directories 49 | *.ide/ 50 | 51 | # MSTest test Results 52 | [Tt]est[Rr]esult*/ 53 | [Bb]uild[Ll]og.* 54 | 55 | #NUNIT 56 | *.VisualState.xml 57 | TestResult.xml 58 | 59 | # Build Results of an ATL Project 60 | [Dd]ebugPS/ 61 | [Rr]eleasePS/ 62 | dlldata.c 63 | 64 | *_i.c 65 | *_p.c 66 | *_i.h 67 | *.ilk 68 | *.meta 69 | *.obj 70 | *.pch 71 | *.pdb 72 | *.pgc 73 | *.pgd 74 | *.rsp 75 | *.sbr 76 | *.tlb 77 | *.tli 78 | *.tlh 79 | *.tmp 80 | *.tmp_proj 81 | *.log 82 | *.vspscc 83 | *.vssscc 84 | .builds 85 | *.pidb 86 | *.svclog 87 | *.scc 88 | 89 | # Chutzpah Test files 90 | _Chutzpah* 91 | 92 | # Visual C++ cache files 93 | ipch/ 94 | *.aps 95 | *.ncb 96 | *.opensdf 97 | *.sdf 98 | *.cachefile 99 | 100 | # Visual Studio profiler 101 | *.psess 102 | *.vsp 103 | *.vspx 104 | 105 | # TFS 2012 Local Workspace 106 | $tf/ 107 | 108 | # Guidance Automation Toolkit 109 | *.gpState 110 | 111 | # ReSharper is a .NET coding add-in 112 | _ReSharper*/ 113 | *.[Rr]e[Ss]harper 114 | *.DotSettings.user 115 | 116 | # JustCode is a .NET coding addin-in 117 | .JustCode 118 | 119 | # TeamCity is a build add-in 120 | _TeamCity* 121 | 122 | # DotCover is a Code Coverage Tool 123 | *.dotCover 124 | 125 | # NCrunch 126 | _NCrunch_* 127 | .*crunch*.local.xml 128 | 129 | # MightyMoose 130 | *.mm.* 131 | AutoTest.Net/ 132 | 133 | # Web workbench (sass) 134 | .sass-cache/ 135 | 136 | # Installshield output folder 137 | [Ee]xpress/ 138 | 139 | # DocProject is a documentation generator add-in 140 | DocProject/buildhelp/ 141 | DocProject/Help/*.HxT 142 | DocProject/Help/*.HxC 143 | DocProject/Help/*.hhc 144 | DocProject/Help/*.hhk 145 | DocProject/Help/*.hhp 146 | DocProject/Help/Html2 147 | DocProject/Help/html 148 | 149 | # Click-Once directory 150 | publish/ 151 | 152 | # Publish Web Output 153 | *.[Pp]ublish.xml 154 | *.azurePubxml 155 | ## TODO: Comment the next line if you want to checkin your 156 | ## web deploy settings but do note that will include unencrypted 157 | ## passwords 158 | #*.pubxml 159 | 160 | # NuGet Packages Directory 161 | packages/* 162 | ## TODO: If the tool you use requires repositories.config 163 | ## uncomment the next line 164 | #!packages/repositories.config 165 | 166 | # Enable "build/" folder in the NuGet Packages folder since 167 | # NuGet packages use it for MSBuild targets. 168 | # This line needs to be after the ignore of the build folder 169 | # (and the packages folder if the line above has been uncommented) 170 | !packages/build/ 171 | 172 | # Windows Azure Build Output 173 | csx/ 174 | *.build.csdef 175 | 176 | # Windows Store app package directory 177 | AppPackages/ 178 | 179 | # Others 180 | sql/ 181 | *.Cache 182 | ClientBin/ 183 | [Ss]tyle[Cc]op.* 184 | ~$* 185 | *~ 186 | *.dbmdl 187 | *.dbproj.schemaview 188 | *.pfx 189 | *.publishsettings 190 | node_modules/ 191 | 192 | # RIA/Silverlight projects 193 | Generated_Code/ 194 | 195 | # Backup & report files from converting an old project file 196 | # to a newer Visual Studio version. Backup files are not needed, 197 | # because we have git ;-) 198 | _UpgradeReport_Files/ 199 | Backup*/ 200 | UpgradeLog*.XML 201 | UpgradeLog*.htm 202 | 203 | # SQL Server files 204 | *.mdf 205 | *.ldf 206 | 207 | # Business Intelligence projects 208 | *.rdl.data 209 | *.bim.layout 210 | *.bim_*.settings 211 | 212 | # Microsoft Fakes 213 | FakesAssemblies/ 214 | 215 | # LightSwitch generated files 216 | GeneratedArtifacts/ 217 | _Pvt_Extensions/ 218 | ModelManifest.xml 219 | 220 | # VS Code 221 | **/.vscode/ 222 | 223 | -------------------------------------------------------------------------------- /examples/turtlebot3/src/grasping_auto.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SIGVERSE_TB3_GRASPING_AUTO_HPP 2 | #define SIGVERSE_TB3_GRASPING_AUTO_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include // Include ncurses after MoveIt headers to avoid macro conflicts 26 | 27 | using namespace std::chrono; 28 | 29 | class SIGVerseTb3GraspingAuto 30 | { 31 | public: 32 | enum GraspingStage 33 | { 34 | MoveArm = 0, 35 | WaitMoveArm, 36 | Grasp, 37 | WaitGrasp, 38 | UpArm, 39 | WaitUpArm, 40 | }; 41 | 42 | private: 43 | const std::string LINK1_NAME = "link1"; 44 | const std::string LINK3_NAME = "link3"; 45 | const std::string LINK4_NAME = "link4"; 46 | const std::string LINK5_NAME = "link5"; 47 | const std::string CAMERA_DEPTH_OPTICAL_FRAME_NAME = "zedm_left_camera_optical_frame"; 48 | 49 | const std::string TARGET_NAME = "target"; 50 | 51 | const std::string JOINT1_NAME = "joint1"; 52 | const std::string JOINT2_NAME = "joint2"; 53 | const std::string JOINT3_NAME = "joint3"; 54 | const std::string JOINT4_NAME = "joint4"; 55 | 56 | const std::string GRIP_JOINT_NAME = "gripper_left_joint"; 57 | const std::string GRIP_JOINT_SUB_NAME = "gripper_right_joint"; 58 | 59 | const double LINEAR_VEL = 0.2; 60 | const double ANGULAR_VEL = 0.4; 61 | const double JOINT_MIN = -2.83; 62 | const double JOINT_MAX = +2.83; 63 | const double GRIP_MIN = -0.01; 64 | const double GRIP_MAX = +0.035; 65 | 66 | const std::string GRASPING_TARGET1_NAME = "clock"; 67 | const std::string GRASPING_TARGET2_NAME = "cup"; 68 | const std::string GRASPING_TARGET3_NAME = "apple"; 69 | const std::string GRASPING_TARGET4_NAME = "teddy bear"; 70 | const std::string GRASPING_TARGET5_NAME = "person"; 71 | 72 | const double PROBABILITY_THRESHOLD = 0.3; 73 | 74 | const int OBJECTS_INFO_UPDATING_INTERVAL = 500; //[ms] 75 | 76 | const int MAX_OBJECTS_NUM = 10; 77 | const int WINDOW_HEADER_HEIGHT = 10; 78 | 79 | public: 80 | SIGVerseTb3GraspingAuto(); 81 | 82 | void key_loop(int argc, char** argv); 83 | 84 | private: 85 | void yolo_detection_callback(const yolo_msgs::msg::DetectionArray::SharedPtr detection_array); 86 | void joint_state_callback (const sensor_msgs::msg::JointState::SharedPtr joint_state); 87 | 88 | void move_base(const double linear_x, const double angular_z); 89 | void move_arm(const std::string &name, const double position, const double current_pos); 90 | void move_gripper(const double position, const double current_pos); 91 | void stop_joints(const int duration_sec); 92 | bool find_grasping_target(geometry_msgs::msg::Point &target_pos, const std::string &target_name); 93 | bool move_arm_toward_object(const std::string &target_name); 94 | 95 | void initialize_posture(); 96 | 97 | template static T clamp(const T val, const T min, const T max); 98 | static int calc_trajectory_duration(const double val, const double current_val); 99 | static bool is_waiting(time_point &latest_stage_time, const int wait_duration_milli); 100 | static void go_next(time_point &latest_stage_time, GraspingStage &stage); 101 | 102 | std::string get_detected_objects_list(); 103 | void publish_debug_markers(const std::string& frame_id, const geometry_msgs::msg::Point& target_pos); 104 | void show_help(); 105 | void display_message_in_window(const std::string& text); 106 | void update_window_layout(); 107 | void init_window(); 108 | void resize_window(); 109 | void shutdown_window(); 110 | 111 | rclcpp::Node::SharedPtr node_; 112 | 113 | std::shared_ptr tf_buffer_; 114 | std::shared_ptr tf_listener_; 115 | 116 | rclcpp::Publisher::SharedPtr pub_base_twist_; 117 | rclcpp::Publisher::SharedPtr pub_joint_trajectory_; 118 | rclcpp::Publisher::SharedPtr pub_debug_markers_; 119 | 120 | std::optional yolo_objects_; 121 | mutable std::mutex yolo_mutex_; // Guard access if multi-threaded executor is used 122 | 123 | // Current positions that is updated by JointState 124 | double joint1_pos_, joint2_pos_, joint3_pos_, joint4_pos_, grip_joint_pos_; 125 | 126 | // Window settings 127 | WINDOW* win_header_ = nullptr; 128 | static std::atomic need_redraw_window_; // set from existing SIGWINCH handler 129 | int header_height_ = WINDOW_HEADER_HEIGHT; 130 | }; 131 | 132 | 133 | SIGVerseTb3GraspingAuto::SIGVerseTb3GraspingAuto() 134 | { 135 | joint1_pos_ = 0.0; 136 | joint2_pos_ = 0.0; 137 | joint3_pos_ = 0.0; 138 | joint4_pos_ = 0.0; 139 | grip_joint_pos_ = 0.0; 140 | } 141 | 142 | 143 | template 144 | T SIGVerseTb3GraspingAuto::clamp(const T val, const T min, const T max) 145 | { 146 | return std::min(std::max(min, val), max); 147 | } 148 | 149 | 150 | int SIGVerseTb3GraspingAuto::calc_trajectory_duration(const double val, const double current_val) 151 | { 152 | return std::max((int)(std::abs(val - current_val) / 0.5), 1); 153 | } 154 | 155 | 156 | bool SIGVerseTb3GraspingAuto::is_waiting(time_point &latest_stage_time, const int wait_duration_milli) 157 | { 158 | auto duration = std::chrono::duration_cast(system_clock::now() - latest_stage_time).count(); 159 | 160 | if(duration < wait_duration_milli){ return true; } 161 | 162 | return false; 163 | } 164 | 165 | 166 | void SIGVerseTb3GraspingAuto::go_next(time_point &latest_stage_time, GraspingStage &stage) 167 | { 168 | latest_stage_time = system_clock::now(); 169 | 170 | stage = static_cast(static_cast(stage)+1); 171 | } 172 | 173 | 174 | #endif // SIGVERSE_TB3_GRASPING_AUTO_HPP 175 | -------------------------------------------------------------------------------- /examples/hsr/hsr_joint_state_2_joint_trajectory/src/hsr_joint_state_2_joint_trajectory.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class HsrJointState2JointTrajectory 13 | { 14 | public: 15 | 16 | HsrJointState2JointTrajectory(); 17 | int run(); 18 | 19 | private: 20 | void joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr joint_state); 21 | bool is_contain_joint_names(std::vector joint_names); 22 | double get_current_joint_states_angle(std::string joint_name); 23 | void move_arm(); 24 | void move_head(); 25 | void move_gripper(); 26 | 27 | sensor_msgs::msg::JointState current_joint_states_; 28 | 29 | std::vector arm_joint_names_; 30 | std::vector head_joint_names_; 31 | std::vector gripper_joint_names_; 32 | 33 | rclcpp::Node::SharedPtr node_; 34 | rclcpp::Subscription::SharedPtr sub_joint_state_; 35 | rclcpp::Publisher::SharedPtr pub_arm_trajectory_; 36 | rclcpp::Publisher::SharedPtr pub_head_trajectory_; 37 | rclcpp::Publisher::SharedPtr pub_gripper_trajectory_; 38 | }; 39 | 40 | 41 | HsrJointState2JointTrajectory::HsrJointState2JointTrajectory() 42 | { 43 | arm_joint_names_.push_back("arm_lift_joint"); 44 | arm_joint_names_.push_back("arm_flex_joint"); 45 | arm_joint_names_.push_back("arm_roll_joint"); 46 | arm_joint_names_.push_back("wrist_flex_joint"); 47 | arm_joint_names_.push_back("wrist_roll_joint"); 48 | head_joint_names_.push_back("head_pan_joint"); 49 | head_joint_names_.push_back("head_tilt_joint"); 50 | gripper_joint_names_.push_back("hand_motor_joint"); 51 | 52 | node_ = rclcpp::Node::make_shared("hsr_joint_state_2_joint_trajectory"); 53 | 54 | sub_joint_state_ = node_->create_subscription("/joint_states", 10, std::bind(&HsrJointState2JointTrajectory::joint_state_callback, this, std::placeholders::_1)); 55 | pub_arm_trajectory_ = node_->create_publisher("/hsrb/arm_trajectory_controller/command", 10); 56 | pub_head_trajectory_ = node_->create_publisher("/hsrb/head_trajectory_controller/command", 10); 57 | pub_gripper_trajectory_ = node_->create_publisher("/hsrb/gripper_controller/command", 10); 58 | } 59 | 60 | 61 | void HsrJointState2JointTrajectory::joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr joint_state) 62 | { 63 | current_joint_states_ = *joint_state; 64 | } 65 | 66 | 67 | bool HsrJointState2JointTrajectory::is_contain_joint_names(std::vector joint_names) 68 | { 69 | if(current_joint_states_.name.empty()){ return false; } 70 | 71 | for(size_t i = 0; i < joint_names.size(); i++) 72 | { 73 | std::vector::iterator it = std::find(current_joint_states_.name.begin(), current_joint_states_.name.end(), joint_names[i]); 74 | if(it == current_joint_states_.name.end()){ return false; } 75 | } 76 | 77 | return true; 78 | } 79 | 80 | 81 | double HsrJointState2JointTrajectory::get_current_joint_states_angle(std::string joint_name) 82 | { 83 | std::vector::iterator it = std::find(current_joint_states_.name.begin(), current_joint_states_.name.end(), joint_name); 84 | int index = std::distance(current_joint_states_.name.begin(), it); 85 | return current_joint_states_.position[index]; 86 | } 87 | 88 | 89 | void HsrJointState2JointTrajectory::move_arm() 90 | { 91 | if(is_contain_joint_names(arm_joint_names_) == false){ return; } 92 | 93 | std::vector goal_position; 94 | 95 | for(size_t i = 0; i < arm_joint_names_.size(); i++){ 96 | goal_position.push_back(get_current_joint_states_angle(arm_joint_names_[i])); 97 | } 98 | 99 | trajectory_msgs::msg::JointTrajectoryPoint arm_joint_point; 100 | arm_joint_point.positions = goal_position; 101 | arm_joint_point.time_from_start = rclcpp::Duration::from_seconds(0.5); 102 | 103 | trajectory_msgs::msg::JointTrajectory joint_trajectory; 104 | joint_trajectory.joint_names = arm_joint_names_; 105 | joint_trajectory.points.push_back(arm_joint_point); 106 | 107 | pub_arm_trajectory_->publish(joint_trajectory); 108 | } 109 | 110 | 111 | void HsrJointState2JointTrajectory::move_head() 112 | { 113 | if(is_contain_joint_names(head_joint_names_) == false){ return; } 114 | 115 | std::vector goal_position; 116 | 117 | for(size_t i = 0; i < head_joint_names_.size(); i++){ 118 | goal_position.push_back(get_current_joint_states_angle(head_joint_names_[i])); 119 | } 120 | 121 | trajectory_msgs::msg::JointTrajectoryPoint head_joint_point; 122 | head_joint_point.positions = goal_position; 123 | head_joint_point.time_from_start = rclcpp::Duration::from_seconds(0.5); 124 | 125 | trajectory_msgs::msg::JointTrajectory joint_trajectory; 126 | joint_trajectory.joint_names = head_joint_names_; 127 | joint_trajectory.points.push_back(head_joint_point); 128 | 129 | pub_head_trajectory_->publish(joint_trajectory); 130 | } 131 | 132 | 133 | void HsrJointState2JointTrajectory::move_gripper() 134 | { 135 | if(is_contain_joint_names(gripper_joint_names_) == false){ return; } 136 | 137 | std::vector goal_position; 138 | 139 | for(size_t i = 0; i < gripper_joint_names_.size(); i++){ 140 | goal_position.push_back(get_current_joint_states_angle(gripper_joint_names_[i])); 141 | } 142 | 143 | trajectory_msgs::msg::JointTrajectoryPoint gripper_joint_point; 144 | gripper_joint_point.positions = goal_position; 145 | gripper_joint_point.time_from_start = rclcpp::Duration::from_seconds(0.5); 146 | 147 | trajectory_msgs::msg::JointTrajectory joint_trajectory; 148 | joint_trajectory.joint_names = gripper_joint_names_; 149 | joint_trajectory.points.push_back(gripper_joint_point); 150 | 151 | pub_gripper_trajectory_->publish(joint_trajectory); 152 | } 153 | 154 | 155 | int HsrJointState2JointTrajectory::run() 156 | { 157 | rclcpp::Rate loop_rate(10); 158 | 159 | while (rclcpp::ok()) 160 | { 161 | move_arm(); 162 | move_head(); 163 | move_gripper(); 164 | 165 | rclcpp::spin_some(node_); 166 | loop_rate.sleep(); 167 | } 168 | 169 | return 0; 170 | } 171 | 172 | 173 | int main(int argc, char** argv) 174 | { 175 | rclcpp::init(argc, argv); 176 | 177 | HsrJointState2JointTrajectory hsr_joint_state_2_joint_trajectory; 178 | return hsr_joint_state_2_joint_trajectory.run(); 179 | } 180 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_description/ros2_control/turtlebot3_manipulation_system.ros2_control.xacro: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | sigverse_turtlebot3_manipulation_hardware/SIGVerseTurtleBot3System 14 | /tb3/joint_state 15 | /tb3/joint_trajectory 16 | 0.1 17 | 18 | 19 | 93 | 94 | 95 | ${-pi*0.9} 96 | ${pi*0.9} 97 | 98 | 99 | 0.0 100 | 101 | 106 | 107 | 108 | 109 | ${-pi*0.57} 110 | ${pi*0.5} 111 | 112 | 113 | 0.0 114 | 115 | 120 | 121 | 122 | 123 | ${-pi*0.3} 124 | ${pi*0.44} 125 | 126 | 127 | 0.0 128 | 129 | 134 | 135 | 136 | 137 | ${-pi*0.57} 138 | ${pi*0.65} 139 | 140 | 141 | 0.0 142 | 143 | 148 | 149 | 177 | 178 | 179 | 180 | 181 | 182 | -------------------------------------------------------------------------------- /examples/turtlebot3/launch/grasping_auto_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import yaml 3 | from pathlib import Path 4 | from ament_index_python.packages import get_package_share_directory 5 | from launch import LaunchDescription 6 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 7 | from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution, EnvironmentVariable 8 | from launch.launch_description_sources import PythonLaunchDescriptionSource 9 | from launch_xml.launch_description_sources import XMLLaunchDescriptionSource 10 | from launch_ros.actions import Node 11 | from launch_ros.substitutions import FindPackageShare 12 | from moveit_configs_utils import MoveItConfigsBuilder 13 | from launch.logging import get_logger 14 | 15 | def load_yaml(pkg: str, relpath: str): 16 | path = os.path.join(get_package_share_directory(pkg), relpath) 17 | with open(path, "r") as file: 18 | return yaml.safe_load(file) 19 | 20 | def generate_launch_description(): 21 | declare_sigverse_port = DeclareLaunchArgument("sigverse_ros_bridge_port", default_value="50001") 22 | declare_rosbridge_port = DeclareLaunchArgument("ros_bridge_port", default_value="9090") 23 | declare_camera_ns = DeclareLaunchArgument("camera_ns", default_value="/camera") 24 | declare_controllers_file = DeclareLaunchArgument( 25 | "controllers_file", 26 | default_value=PathJoinSubstitution([ 27 | FindPackageShare("sigverse_turtlebot3"), 28 | "config", "turtlebot3_controller_manager.yaml" 29 | ]) 30 | ) 31 | 32 | description_dir = get_package_share_directory("sigverse_turtlebot3_manipulation_description") 33 | urdf_path = str(Path(description_dir, "urdf", "turtlebot3_manipulation.urdf.xacro")) 34 | 35 | # robot_state_publisher_node = Node( 36 | # package="robot_state_publisher", 37 | # executable="robot_state_publisher", 38 | # output="screen", 39 | # parameters=[{ 40 | # "robot_description": Command(["xacro ", urdf_path]) 41 | # }] 42 | # ) 43 | 44 | controller_manager_node = Node( 45 | package="controller_manager", 46 | executable="ros2_control_node", 47 | # name="controller_manager", 48 | output="screen", 49 | # prefix='gnome-terminal --title="controller_manager" --', 50 | parameters=[ 51 | {"robot_description": Command(["xacro ", urdf_path])}, 52 | LaunchConfiguration("controllers_file") 53 | ] 54 | ) 55 | 56 | spawn_arm_controller = Node( 57 | package="controller_manager", 58 | executable="spawner", 59 | # name="spawn_arm_controller", 60 | arguments=["arm_controller", "--controller-manager", "/controller_manager"], 61 | output="screen" 62 | ) 63 | 64 | moveit_config = ( 65 | MoveItConfigsBuilder("sigverse_turtlebot3_manipulation") 66 | .robot_description (file_path=urdf_path) 67 | .robot_description_semantic (file_path="config/turtlebot3_manipulation.srdf") 68 | .robot_description_kinematics(file_path="config/kinematics.yaml") 69 | .joint_limits (file_path="config/joint_limits.yaml") 70 | .planning_scene_monitor(publish_robot_description=True, publish_robot_description_semantic=True) 71 | .trajectory_execution (file_path="config/moveit_controllers.yaml") 72 | .planning_pipelines (pipelines=["ompl"], default_planning_pipeline="ompl") 73 | .sensors_3d (file_path="config/sensors_3d.yaml") 74 | .to_moveit_configs() 75 | ) 76 | 77 | move_group_node = Node( 78 | package="moveit_ros_move_group", 79 | executable="move_group", 80 | # name="move_group", 81 | output="screen", 82 | parameters=[moveit_config.to_dict()], 83 | remappings=[("/joint_states", "/tb3/joint_state")], 84 | ) 85 | 86 | yolo_launch = IncludeLaunchDescription( 87 | PythonLaunchDescriptionSource([ 88 | PathJoinSubstitution([FindPackageShare('yolo_bringup'), 'launch', 'yolov11.launch.py']) 89 | ]), 90 | launch_arguments={ 91 | "namespace": "yolo_objects", 92 | "model": PathJoinSubstitution([EnvironmentVariable("HOME"), "yolo_weights", "yolo11n.pt"]), 93 | "device": "cpu", 94 | "threshold": "0.5", 95 | "imgsz_height": "376", 96 | "imgsz_width": "672", 97 | "input_image_topic": PathJoinSubstitution([LaunchConfiguration("camera_ns"), "rgb", "image_raw"]), 98 | "input_depth_topic": PathJoinSubstitution([LaunchConfiguration("camera_ns"), "depth", "image_raw"]), 99 | "input_depth_info_topic": PathJoinSubstitution([LaunchConfiguration("camera_ns"), "depth", "camera_info"]), 100 | "target_frame": "base_link", 101 | "depth_image_units_divisor": "1", 102 | "use_3d": "True", 103 | "use_debug": "True", 104 | "maximum_detection_threshold": "0.03" 105 | }.items() 106 | ) 107 | 108 | moveit_config_pkg = "sigverse_turtlebot3_manipulation_moveit_config" 109 | srdf_content = (Path(get_package_share_directory(moveit_config_pkg)).joinpath("config/turtlebot3_manipulation.srdf")).read_text() 110 | kinematics_dict = load_yaml(moveit_config_pkg, "config/kinematics.yaml") 111 | 112 | # logger = get_logger("dump") 113 | # logger.info("kinematics_dict:\n" + yaml.safe_dump(kinematics_dict, sort_keys=False)) 114 | 115 | grasping_auto_node = Node( 116 | package="sigverse_turtlebot3", 117 | executable="grasping_auto", 118 | output="screen", 119 | prefix='gnome-terminal --title="TurtleBot3 teleop key" --geometry=60x20 --', 120 | parameters=[ 121 | {"robot_description": Command(["xacro ", urdf_path])}, 122 | {"robot_description_semantic": srdf_content}, 123 | {"robot_description_kinematics": kinematics_dict}, 124 | ] 125 | ) 126 | 127 | sigverse_bridge_node = Node( 128 | package="sigverse_ros_bridge", 129 | executable="sigverse_ros_bridge", 130 | output="screen", 131 | # prefix='gnome-terminal --title="SIGVerse Rosbridge" --', 132 | arguments=[LaunchConfiguration("sigverse_ros_bridge_port")] 133 | ) 134 | 135 | rosbridge_launch = IncludeLaunchDescription( 136 | XMLLaunchDescriptionSource([ 137 | PathJoinSubstitution([FindPackageShare('rosbridge_server'), 'launch', 'rosbridge_websocket_launch.xml']) 138 | ]), 139 | launch_arguments={ 140 | "port": LaunchConfiguration("ros_bridge_port"), 141 | "default_call_service_timeout": "5.0", 142 | "call_services_in_new_thread": "true", 143 | "send_action_goals_in_new_thread": "true" 144 | }.items() 145 | ) 146 | 147 | rviz_node = Node( 148 | package="rviz2", 149 | executable="rviz2", 150 | # name="rviz2", 151 | output="screen", 152 | arguments=["-d", PathJoinSubstitution([ 153 | FindPackageShare("sigverse_turtlebot3"), "launch", "grasping_auto_launch.rviz" 154 | ])] 155 | ) 156 | 157 | return LaunchDescription([ 158 | declare_sigverse_port, 159 | declare_rosbridge_port, 160 | declare_camera_ns, 161 | declare_controllers_file, 162 | 163 | # robot_state_publisher_node, 164 | controller_manager_node, 165 | spawn_arm_controller, 166 | move_group_node, 167 | yolo_launch, 168 | grasping_auto_node, 169 | sigverse_bridge_node, 170 | rosbridge_launch, 171 | rviz_node 172 | ]) 173 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_moveit_config/config/turtlebot3_manipulation.srdf: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | -------------------------------------------------------------------------------- /examples/turtlebot3/launch/recognize_pointed_direction.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 70 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Grid1/Offset1 8 | - /TF1/Frames1 9 | - /DepthCloud1/Auto Size1 10 | - /Image1/Topic1 11 | - /MarkerArray1 12 | Splitter Ratio: 0.5 13 | Tree Height: 159 14 | - Class: rviz_common/Selection 15 | Name: Selection 16 | - Class: rviz_common/Tool Properties 17 | Expanded: 18 | - /2D Goal Pose1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz_common/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz_common/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz_default_plugins/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.029999999329447746 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: -0.5 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Class: rviz_default_plugins/TF 54 | Enabled: true 55 | Frame Timeout: 15 56 | Frames: 57 | All Enabled: true 58 | Marker Scale: 1 59 | Name: TF 60 | Show Arrows: true 61 | Show Axes: true 62 | Show Names: false 63 | Tree: 64 | {} 65 | Update Interval: 0 66 | Value: true 67 | - Alpha: 1 68 | Auto Size: 69 | Auto Size Factor: 1 70 | Value: true 71 | Autocompute Intensity Bounds: true 72 | Autocompute Value Bounds: 73 | Max Value: 10 74 | Min Value: -10 75 | Value: true 76 | Axis: Z 77 | Channel Name: intensity 78 | Class: rviz_default_plugins/DepthCloud 79 | Color: 255; 255; 255 80 | Color Image Topic: "" 81 | Color Transformer: RGB8 82 | Color Transport Hint: raw 83 | Decay Time: 0 84 | Depth Map Topic: /camera/depth/image_raw 85 | Depth Map Transport Hint: raw 86 | Enabled: false 87 | Invert Rainbow: false 88 | Max Color: 255; 255; 255 89 | Max Intensity: 4096 90 | Min Color: 0; 0; 0 91 | Min Intensity: 0 92 | Name: DepthCloud 93 | Occlusion Compensation: 94 | Occlusion Time-Out: 30 95 | Value: false 96 | Position Transformer: XYZ 97 | Queue Size: 1 98 | Reliability Policy: Best effort 99 | Selectable: true 100 | Size (Pixels): 1 101 | Style: Points 102 | Topic Filter: true 103 | Use Fixed Frame: true 104 | Use rainbow: true 105 | Value: false 106 | - Class: rviz_default_plugins/Image 107 | Enabled: true 108 | Max Value: 1 109 | Median window: 5 110 | Min Value: 0 111 | Name: Image 112 | Normalize Range: true 113 | Topic: 114 | Depth: 5 115 | Durability Policy: Volatile 116 | History Policy: Keep Last 117 | Reliability Policy: Reliable 118 | Value: /yolo_human_pose/dbg_image 119 | Value: true 120 | - Class: rviz_default_plugins/MarkerArray 121 | Enabled: true 122 | Name: MarkerArray 123 | Namespaces: 124 | {} 125 | Topic: 126 | Depth: 5 127 | Durability Policy: Volatile 128 | History Policy: Keep Last 129 | Reliability Policy: Reliable 130 | Value: /tb3/debug_markers 131 | Value: true 132 | Enabled: true 133 | Global Options: 134 | Background Color: 48; 48; 48 135 | Fixed Frame: base_link 136 | Frame Rate: 30 137 | Name: root 138 | Tools: 139 | - Class: rviz_default_plugins/Interact 140 | Hide Inactive Objects: true 141 | - Class: rviz_default_plugins/MoveCamera 142 | - Class: rviz_default_plugins/Select 143 | - Class: rviz_default_plugins/FocusCamera 144 | - Class: rviz_default_plugins/Measure 145 | Line color: 128; 128; 0 146 | - Class: rviz_default_plugins/SetInitialPose 147 | Covariance x: 0.25 148 | Covariance y: 0.25 149 | Covariance yaw: 0.06853891909122467 150 | Topic: 151 | Depth: 5 152 | Durability Policy: Volatile 153 | History Policy: Keep Last 154 | Reliability Policy: Reliable 155 | Value: /initialpose 156 | - Class: rviz_default_plugins/SetGoal 157 | Topic: 158 | Depth: 5 159 | Durability Policy: Volatile 160 | History Policy: Keep Last 161 | Reliability Policy: Reliable 162 | Value: /goal_pose 163 | - Class: rviz_default_plugins/PublishPoint 164 | Single click: true 165 | Topic: 166 | Depth: 5 167 | Durability Policy: Volatile 168 | History Policy: Keep Last 169 | Reliability Policy: Reliable 170 | Value: /clicked_point 171 | Transformation: 172 | Current: 173 | Class: rviz_default_plugins/TF 174 | Value: true 175 | Views: 176 | Current: 177 | Class: rviz_default_plugins/Orbit 178 | Distance: 3.3334977626800537 179 | Enable Stereo Rendering: 180 | Stereo Eye Separation: 0.05999999865889549 181 | Stereo Focal Distance: 1 182 | Swap Stereo Eyes: false 183 | Value: false 184 | Focal Point: 185 | X: 0.916344940662384 186 | Y: 0.23534490168094635 187 | Z: -0.1286897212266922 188 | Focal Shape Fixed Size: true 189 | Focal Shape Size: 0.05000000074505806 190 | Invert Z Axis: false 191 | Name: Current View 192 | Near Clip Distance: 0.009999999776482582 193 | Pitch: 0.7003976106643677 194 | Target Frame: 195 | Value: Orbit (rviz) 196 | Yaw: 2.51855731010437 197 | Saved: ~ 198 | Window Geometry: 199 | Displays: 200 | collapsed: false 201 | Height: 786 202 | Hide Left Dock: false 203 | Hide Right Dock: false 204 | Image: 205 | collapsed: false 206 | QMainWindow State: 000000ff00000000fd00000004000000000000025e00000274fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000122000000c900fffffffb0000000a0049006d00610067006501000001650000014c0000002800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b00000236fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000236000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005810000003efc0100000002fb0000000800540069006d0065010000000000000581000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000031d0000027400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 207 | Selection: 208 | collapsed: false 209 | Time: 210 | collapsed: false 211 | Tool Properties: 212 | collapsed: false 213 | Views: 214 | collapsed: false 215 | Width: 1409 216 | X: 164 217 | Y: 80 218 | -------------------------------------------------------------------------------- /examples/turtlebot3/launch/slam.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Map1 10 | Splitter Ratio: 0.5 11 | Tree Height: 427 12 | - Class: rviz_common/Selection 13 | Name: Selection 14 | - Class: rviz_common/Tool Properties 15 | Expanded: 16 | - /2D Goal Pose1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz_common/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz_common/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz_default_plugins/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.029999999329447746 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Class: rviz_default_plugins/TF 52 | Enabled: true 53 | Frame Timeout: 15 54 | Frames: 55 | All Enabled: true 56 | base_footprint: 57 | Value: true 58 | base_link: 59 | Value: true 60 | base_scan: 61 | Value: true 62 | caster_back_left_link: 63 | Value: true 64 | caster_back_right_link: 65 | Value: true 66 | map: 67 | Value: true 68 | odom: 69 | Value: true 70 | wheel_left_link: 71 | Value: true 72 | wheel_right_link: 73 | Value: true 74 | zedm_base_link: 75 | Value: true 76 | zedm_camera_center: 77 | Value: true 78 | zedm_left_camera_frame: 79 | Value: true 80 | zedm_left_camera_optical_frame: 81 | Value: true 82 | zedm_right_camera_frame: 83 | Value: true 84 | zedm_right_camera_optical_frame: 85 | Value: true 86 | Marker Scale: 1 87 | Name: TF 88 | Show Arrows: true 89 | Show Axes: true 90 | Show Names: false 91 | Tree: 92 | map: 93 | odom: 94 | base_footprint: 95 | base_link: 96 | base_scan: 97 | {} 98 | caster_back_left_link: 99 | {} 100 | caster_back_right_link: 101 | {} 102 | wheel_left_link: 103 | {} 104 | wheel_right_link: 105 | {} 106 | zedm_base_link: 107 | zedm_camera_center: 108 | zedm_left_camera_frame: 109 | zedm_left_camera_optical_frame: 110 | {} 111 | zedm_right_camera_frame: 112 | zedm_right_camera_optical_frame: 113 | {} 114 | Update Interval: 0 115 | Value: true 116 | - Alpha: 0.699999988079071 117 | Class: rviz_default_plugins/Map 118 | Color Scheme: map 119 | Draw Behind: false 120 | Enabled: true 121 | Name: Map 122 | Topic: 123 | Depth: 5 124 | Durability Policy: Volatile 125 | Filter size: 10 126 | History Policy: Keep Last 127 | Reliability Policy: Reliable 128 | Value: /map 129 | Update Topic: 130 | Depth: 5 131 | Durability Policy: Volatile 132 | History Policy: Keep Last 133 | Reliability Policy: Reliable 134 | Value: /map_updates 135 | Use Timestamp: false 136 | Value: true 137 | Enabled: true 138 | Global Options: 139 | Background Color: 48; 48; 48 140 | Fixed Frame: map 141 | Frame Rate: 30 142 | Name: root 143 | Tools: 144 | - Class: rviz_default_plugins/Interact 145 | Hide Inactive Objects: true 146 | - Class: rviz_default_plugins/MoveCamera 147 | - Class: rviz_default_plugins/Select 148 | - Class: rviz_default_plugins/FocusCamera 149 | - Class: rviz_default_plugins/Measure 150 | Line color: 128; 128; 0 151 | - Class: rviz_default_plugins/SetInitialPose 152 | Covariance x: 0.25 153 | Covariance y: 0.25 154 | Covariance yaw: 0.06853891909122467 155 | Topic: 156 | Depth: 5 157 | Durability Policy: Volatile 158 | History Policy: Keep Last 159 | Reliability Policy: Reliable 160 | Value: /initialpose 161 | - Class: rviz_default_plugins/SetGoal 162 | Topic: 163 | Depth: 5 164 | Durability Policy: Volatile 165 | History Policy: Keep Last 166 | Reliability Policy: Reliable 167 | Value: /goal_pose 168 | - Class: rviz_default_plugins/PublishPoint 169 | Single click: true 170 | Topic: 171 | Depth: 5 172 | Durability Policy: Volatile 173 | History Policy: Keep Last 174 | Reliability Policy: Reliable 175 | Value: /clicked_point 176 | Transformation: 177 | Current: 178 | Class: rviz_default_plugins/TF 179 | Value: true 180 | Views: 181 | Current: 182 | Class: rviz_default_plugins/Orbit 183 | Distance: 10 184 | Enable Stereo Rendering: 185 | Stereo Eye Separation: 0.05999999865889549 186 | Stereo Focal Distance: 1 187 | Swap Stereo Eyes: false 188 | Value: false 189 | Focal Point: 190 | X: 0 191 | Y: 0 192 | Z: 0 193 | Focal Shape Fixed Size: true 194 | Focal Shape Size: 0.05000000074505806 195 | Invert Z Axis: false 196 | Name: Current View 197 | Near Clip Distance: 0.009999999776482582 198 | Pitch: 0.785398006439209 199 | Target Frame: 200 | Value: Orbit (rviz) 201 | Yaw: 0.785398006439209 202 | Saved: ~ 203 | Window Geometry: 204 | Displays: 205 | collapsed: false 206 | Height: 724 207 | Hide Left Dock: false 208 | Hide Right Dock: false 209 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000236fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000236000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000236fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000236000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f0000023600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 210 | Selection: 211 | collapsed: false 212 | Time: 213 | collapsed: false 214 | Tool Properties: 215 | collapsed: false 216 | Views: 217 | collapsed: false 218 | Width: 1200 219 | X: 648 220 | Y: 84 221 | -------------------------------------------------------------------------------- /examples/turtlebot3_manipulation/sigverse_turtlebot3_manipulation_hardware/src/sigverse_turtlebot3_system.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 8 | #include "rclcpp/rclcpp.hpp" 9 | 10 | #include "sigverse_turtlebot3_manipulation_hardware/sigverse_turtlebot3_system.hpp" 11 | 12 | namespace sigverse 13 | { 14 | auto logger = rclcpp::get_logger("sigverse_turtlebot3_system"); 15 | 16 | hardware_interface::CallbackReturn 17 | SIGVerseTurtleBot3System::on_init(const hardware_interface::HardwareInfo & info) 18 | { 19 | RCLCPP_INFO(logger, "TB3 hardware_interface on_init -Start-"); 20 | 21 | if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) 22 | { 23 | return hardware_interface::CallbackReturn::ERROR; 24 | } 25 | 26 | for (const auto &joint : info_.joints) 27 | { 28 | joint_position_map_ [joint.name] = 0.0; 29 | joint_position_command_map_[joint.name] = 0.0; 30 | for (const auto &state_interface : joint.state_interfaces) 31 | { 32 | if (state_interface.name == hardware_interface::HW_IF_POSITION && !state_interface.initial_value.empty()) 33 | { 34 | joint_position_map_ [joint.name] = std::stod(state_interface.initial_value); 35 | joint_position_command_map_[joint.name] = std::stod(state_interface.initial_value); 36 | } 37 | } 38 | } 39 | 40 | joint_states_buffer_.writeFromNonRT(joint_position_map_); 41 | 42 | return hardware_interface::CallbackReturn::SUCCESS; 43 | } 44 | 45 | std::vector 46 | SIGVerseTurtleBot3System::export_state_interfaces() 47 | { 48 | RCLCPP_INFO(logger, "TB3 hardware_interface export_state_interfaces -Start-"); 49 | 50 | std::vector state_interfaces; 51 | state_interfaces.reserve(info_.joints.size()); 52 | 53 | for (const auto &joint : info_.joints) 54 | { 55 | state_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_POSITION, &joint_position_map_.at(joint.name)); 56 | } 57 | 58 | return state_interfaces; 59 | } 60 | 61 | std::vector 62 | SIGVerseTurtleBot3System::export_command_interfaces() 63 | { 64 | RCLCPP_INFO(logger, "TB3 hardware_interface export_command_interfaces -Start-"); 65 | 66 | std::vector command_interfaces; 67 | command_interfaces.reserve(info_.joints.size()); 68 | 69 | for (const auto &joint : info_.joints) 70 | { 71 | command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_POSITION, &joint_position_command_map_.at(joint.name)); 72 | } 73 | 74 | // command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[6].name, hardware_interface::HW_IF_POSITION, &dxl_gripper_commands_[0])); 75 | // command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[7].name, hardware_interface::HW_IF_POSITION, &dxl_gripper_commands_[1])); 76 | 77 | return command_interfaces; 78 | } 79 | 80 | hardware_interface::CallbackReturn 81 | SIGVerseTurtleBot3System::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) 82 | { 83 | RCLCPP_INFO(logger, "TB3 hardware_interface on_activate -Start-"); 84 | 85 | node_ = std::make_shared("sigverse_tb3_hw_bridge"); 86 | thread_executor_ = std::make_unique(); 87 | thread_executor_->add_node(node_); 88 | 89 | joint_trajectory_pub_interval_ = std::stod(info_.hardware_parameters.at("joint_trajectory_pub_interval")); 90 | last_sent_joint_pos_time_ = node_->get_clock()->now(); 91 | 92 | // Subscribe JointStates 93 | joint_state_sub_ = node_->create_subscription 94 | ( 95 | info_.hardware_parameters["joint_state_topic"], rclcpp::QoS(10).best_effort(), 96 | [this](const sensor_msgs::msg::JointState &msg) 97 | { 98 | if (msg.name.size() != msg.position.size()) { return; } // safety 99 | 100 | // Non-RT context: build a snapshot map and push to RealtimeBuffer 101 | auto snapshot = joint_position_map_; 102 | 103 | for (size_t i = 0; i < msg.name.size(); i++) 104 | { 105 | if (snapshot.count(msg.name[i]) > 0) 106 | { 107 | snapshot[msg.name[i]] = msg.position[i]; 108 | } 109 | } 110 | joint_states_buffer_.writeFromNonRT(snapshot); 111 | } 112 | ); 113 | 114 | // Publisher for single-point JointTrajectory toward simulator 115 | joint_trajectory_pub_ = node_->create_publisher(info_.hardware_parameters["joint_trajectory_topic"], rclcpp::SystemDefaultsQoS()); 116 | realtime_joint_trajectory_pub_ = std::make_unique>(joint_trajectory_pub_); 117 | 118 | spin_thread_ = std::thread([this]{ thread_executor_->spin(); }); 119 | 120 | return hardware_interface::CallbackReturn::SUCCESS; 121 | } 122 | 123 | hardware_interface::CallbackReturn 124 | SIGVerseTurtleBot3System::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) 125 | { 126 | RCLCPP_INFO(logger, "TB3 hardware_interface on_deactivate -Start-"); 127 | 128 | if (thread_executor_) thread_executor_->cancel(); 129 | if (spin_thread_.joinable()) spin_thread_.join(); 130 | joint_state_sub_.reset(); 131 | thread_executor_.reset(); 132 | node_.reset(); 133 | 134 | return hardware_interface::CallbackReturn::SUCCESS; 135 | } 136 | 137 | hardware_interface::return_type 138 | SIGVerseTurtleBot3System::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) 139 | { 140 | RCLCPP_INFO_ONCE(logger, "TB3 hardware_interface read -Start-"); 141 | 142 | if (const auto* latest = joint_states_buffer_.readFromRT()) 143 | { 144 | for (auto& [name, position] : joint_position_map_) 145 | { 146 | if (latest->count(name) > 0) 147 | { 148 | position = latest->at(name); 149 | } 150 | } 151 | } 152 | 153 | return hardware_interface::return_type::OK; 154 | } 155 | 156 | hardware_interface::return_type 157 | SIGVerseTurtleBot3System::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) 158 | { 159 | RCLCPP_INFO_ONCE(logger, "TB3 hardware_interface write -Start-"); 160 | 161 | if (joint_position_command_map_.empty()) { return hardware_interface::return_type::OK; } 162 | 163 | if (!has_joint_command_changed()) return hardware_interface::return_type::OK; 164 | 165 | // build single-point JointTrajectory 166 | trajectory_msgs::msg::JointTrajectory joint_trajectory; 167 | joint_trajectory.header.stamp = node_->get_clock()->now(); 168 | 169 | trajectory_msgs::msg::JointTrajectoryPoint joint_trajectory_point; 170 | joint_trajectory.joint_names .reserve(joint_position_command_map_.size()); 171 | joint_trajectory_point.positions.reserve(joint_position_command_map_.size()); 172 | 173 | for (const auto &key_value : joint_position_command_map_) 174 | { 175 | joint_trajectory.joint_names .push_back(key_value.first); 176 | joint_trajectory_point.positions.push_back(key_value.second); 177 | } 178 | 179 | // Single immediate point 180 | joint_trajectory_point.time_from_start = rclcpp::Duration(0,0); 181 | joint_trajectory.points.emplace_back(std::move(joint_trajectory_point)); 182 | 183 | realtime_joint_trajectory_pub_->lock(); 184 | realtime_joint_trajectory_pub_->msg_ = joint_trajectory; 185 | realtime_joint_trajectory_pub_->unlockAndPublish(); 186 | 187 | last_sent_joint_pos_cmd_map_ = joint_position_command_map_; 188 | last_sent_joint_pos_time_ = node_->get_clock()->now(); 189 | 190 | return hardware_interface::return_type::OK; 191 | } 192 | 193 | bool SIGVerseTurtleBot3System::has_joint_command_changed() const noexcept 194 | { 195 | rclcpp::Time now = node_->get_clock()->now(); 196 | 197 | if ((now - last_sent_joint_pos_time_) < rclcpp::Duration::from_seconds(joint_trajectory_pub_interval_)) 198 | { 199 | return false; 200 | } 201 | 202 | if (joint_position_command_map_.size() != last_sent_joint_pos_cmd_map_.size()) { return true; } 203 | 204 | for (const auto &key_value : joint_position_command_map_) 205 | { 206 | auto it = last_sent_joint_pos_cmd_map_.find(key_value.first); 207 | 208 | if (it == last_sent_joint_pos_cmd_map_.end()) 209 | { 210 | return true; // missing key 211 | } 212 | if (std::abs(key_value.second - it->second) > kJointCommandDelta) 213 | { 214 | return true; // value changed 215 | } 216 | } 217 | return false; // no change 218 | } 219 | 220 | } // namespace sigverse 221 | 222 | #include "pluginlib/class_list_macros.hpp" 223 | PLUGINLIB_EXPORT_CLASS( 224 | sigverse::SIGVerseTurtleBot3System, 225 | hardware_interface::SystemInterface) 226 | -------------------------------------------------------------------------------- /examples/turtlebot3/src/recognize_pointed_direction.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | class SIGVerseTb3RecognizePointedDirection 16 | { 17 | private: 18 | const int RIGHT_SHOULDER = 7; 19 | // const int RIGHT_ELBOW = 9; 20 | const int RIGHT_WRIST = 11; 21 | const std::string JOINT1_NAME = "joint1"; 22 | const std::string INSTRUCTION_MESSAGE = "Rotate the arm to this side"; 23 | 24 | public: 25 | SIGVerseTb3RecognizePointedDirection(); 26 | 27 | void run(int argc, char** argv); 28 | 29 | private: 30 | void yolo_detection_callback(const yolo_msgs::msg::DetectionArray::SharedPtr detection_array); 31 | bool compute_pointing_floor_intersection(const geometry_msgs::msg::Point& shoulder, const geometry_msgs::msg::Point& wrist, double floor_z, geometry_msgs::msg::Point& floor_hit_point); 32 | void publish_debug_markers(const std::string& frame_id, const geometry_msgs::msg::Point& hit_point); 33 | 34 | void instruction_callback(const std_msgs::msg::String::SharedPtr instruction_message); 35 | void rotate_arm(); 36 | void move_arm(rclcpp::Publisher::SharedPtr publisher, const std::string &name, const double position); 37 | 38 | rclcpp::Node::SharedPtr node_; 39 | 40 | rclcpp::Publisher::SharedPtr pub_joint_trajectory_; 41 | rclcpp::Publisher::SharedPtr pub_debug_markers_; 42 | 43 | std::map keypoint_map_; 44 | geometry_msgs::msg::Point hit_point_; 45 | double floor_z_; 46 | }; 47 | 48 | SIGVerseTb3RecognizePointedDirection::SIGVerseTb3RecognizePointedDirection() 49 | { 50 | hit_point_.x = std::numeric_limits::quiet_NaN(); 51 | } 52 | 53 | void SIGVerseTb3RecognizePointedDirection::yolo_detection_callback(const yolo_msgs::msg::DetectionArray::SharedPtr detection_array) 54 | { 55 | if (detection_array->detections.empty()) { return; } 56 | 57 | // Find the detection with the highest score 58 | const yolo_msgs::msg::Detection* best_detection = nullptr; 59 | double best_score = -std::numeric_limits::infinity(); 60 | 61 | for (const auto &detection : detection_array->detections) 62 | { 63 | if (detection.score > best_score) 64 | { 65 | best_score = detection.score; 66 | best_detection = &detection; 67 | } 68 | } 69 | 70 | for (const auto &keypoint : best_detection->keypoints3d.data) 71 | { 72 | keypoint_map_[keypoint.id] = keypoint.point; 73 | } 74 | 75 | // Update hit_point_ 76 | if (compute_pointing_floor_intersection(keypoint_map_[RIGHT_SHOULDER], keypoint_map_[RIGHT_WRIST], floor_z_, hit_point_)) 77 | { 78 | // RCLCPP_INFO(node_->get_logger(), "Hit Position=(%.3f, %.3f, %.3f)", hit_point_.x, hit_point_.y, hit_point_.z); 79 | } 80 | else 81 | { 82 | RCLCPP_WARN(node_->get_logger(), "Couldn't compute pointing floor intersection"); 83 | } 84 | 85 | publish_debug_markers(best_detection->keypoints3d.frame_id, hit_point_); 86 | } 87 | 88 | /** 89 | * @brief Compute intersection of the ray (shoulder -> wrist) with floor z=floor_z. 90 | * @param shoulder Shoulder position in base_link (m) 91 | * @param wrist Wrist position in base_link (m) 92 | * @param floor_z Floor height (e.g., -0.5) 93 | * @param floor_hit_point Output: intersection point with the floor 94 | * @return true if intersection exists; false otherwise 95 | */ 96 | bool SIGVerseTb3RecognizePointedDirection::compute_pointing_floor_intersection( 97 | const geometry_msgs::msg::Point& shoulder, const geometry_msgs::msg::Point& wrist, double floor_z, geometry_msgs::msg::Point& floor_hit_point) 98 | { 99 | if (wrist.z >= shoulder.z){ return false; } 100 | if (floor_z >= wrist.z) { return false; } 101 | 102 | const double dx = wrist.x - shoulder.x; 103 | const double dy = wrist.y - shoulder.y; 104 | const double dz = wrist.z - shoulder.z; 105 | 106 | if (std::fabs(dz) < 1e-9) { return false; } 107 | 108 | // Parameter to reach z = floor_z along the ray from the shoulder toward the wrist 109 | const double t = (floor_z - shoulder.z) / dz; 110 | 111 | floor_hit_point.x = shoulder.x + t * dx; 112 | floor_hit_point.y = shoulder.y + t * dy; 113 | floor_hit_point.z = floor_z; 114 | 115 | return true; 116 | } 117 | 118 | void SIGVerseTb3RecognizePointedDirection::publish_debug_markers(const std::string& frame_id, const geometry_msgs::msg::Point& hit_point) 119 | { 120 | visualization_msgs::msg::MarkerArray markerArray; 121 | const auto stamp = rclcpp::Time(0); 122 | 123 | // Keypoints: green spheres 124 | visualization_msgs::msg::Marker marker_keypoints; 125 | marker_keypoints.header.frame_id = frame_id; 126 | marker_keypoints.header.stamp = stamp; 127 | marker_keypoints.ns = "keypoints"; 128 | marker_keypoints.id = 0; // Keep constant to overwrite 129 | marker_keypoints.type = visualization_msgs::msg::Marker::SPHERE_LIST; 130 | marker_keypoints.action = visualization_msgs::msg::Marker::ADD; 131 | marker_keypoints.scale = []{ geometry_msgs::msg::Vector3 v; v.x=0.05; v.y=0.05; v.z=0.05; return v; }(); 132 | marker_keypoints.color = []{ std_msgs::msg::ColorRGBA c; c.r=0.0f; c.g=1.0f; c.b=0.0f; c.a=1.0f; return c; }(); // green 133 | marker_keypoints.pose.orientation.w = 1.0; 134 | marker_keypoints.points.reserve(keypoint_map_.size()); 135 | for (const auto& kv : keypoint_map_) marker_keypoints.points.push_back(kv.second); 136 | markerArray.markers.push_back(marker_keypoints); 137 | 138 | // Hit point: red sphere 139 | visualization_msgs::msg::Marker marker_hit_point; 140 | marker_hit_point.header.frame_id = frame_id; 141 | marker_hit_point.header.stamp = stamp; 142 | marker_hit_point.ns = "hit_point"; 143 | marker_hit_point.id = 0; // Keep constant to overwrite 144 | marker_hit_point.type = visualization_msgs::msg::Marker::SPHERE; 145 | marker_hit_point.action = visualization_msgs::msg::Marker::ADD; 146 | marker_hit_point.scale = []{ geometry_msgs::msg::Vector3 v; v.x=0.1; v.y=0.1; v.z=0.1; return v; }(); 147 | marker_hit_point.color = []{ std_msgs::msg::ColorRGBA c; c.r=1.0f; c.g=0.0f; c.b=0.0f; c.a=1.0f; return c; }(); // red 148 | marker_hit_point.pose.orientation.w = 1.0; 149 | marker_hit_point.pose.position = hit_point; 150 | markerArray.markers.push_back(marker_hit_point); 151 | 152 | pub_debug_markers_->publish(markerArray); 153 | } 154 | 155 | 156 | void SIGVerseTb3RecognizePointedDirection::instruction_callback(const std_msgs::msg::String::SharedPtr instruction_message) 157 | { 158 | if(instruction_message->data==INSTRUCTION_MESSAGE) 159 | { 160 | RCLCPP_INFO(node_->get_logger(), "Received Instruction Message"); 161 | // RCLCPP_INFO(node_->get_logger(), "Hit Position=(%.3f, %.3f, %.3f)", hit_point_.x, hit_point_.y, hit_point_.z); 162 | rotate_arm(); 163 | } 164 | } 165 | 166 | void SIGVerseTb3RecognizePointedDirection::rotate_arm() 167 | { 168 | if (std::isnan(hit_point_.x)) 169 | { 170 | RCLCPP_WARN(node_->get_logger(), "the hit_point_ is uninitialized."); 171 | return; 172 | } 173 | 174 | // Rotate the arm 175 | if (hit_point_.y > +0.4){ move_arm(pub_joint_trajectory_, JOINT1_NAME, +0.50); } 176 | else if(hit_point_.y > 0.0){ move_arm(pub_joint_trajectory_, JOINT1_NAME, +0.25); } 177 | else if(hit_point_.y > -0.4){ move_arm(pub_joint_trajectory_, JOINT1_NAME, -0.25); } 178 | else { move_arm(pub_joint_trajectory_, JOINT1_NAME, -0.50); } 179 | } 180 | 181 | void SIGVerseTb3RecognizePointedDirection::move_arm(rclcpp::Publisher::SharedPtr publisher, const std::string &name, const double position) 182 | { 183 | std::vector names; 184 | names.push_back(name); 185 | 186 | std::vector positions; 187 | positions.push_back(position); 188 | 189 | builtin_interfaces::msg::Duration duration; 190 | duration.sec = 2; 191 | duration.nanosec = 0; 192 | 193 | trajectory_msgs::msg::JointTrajectory joint_trajectory; 194 | 195 | trajectory_msgs::msg::JointTrajectoryPoint arm_joint_point; 196 | 197 | joint_trajectory.points.push_back(arm_joint_point); 198 | 199 | joint_trajectory.joint_names = names; 200 | joint_trajectory.points[0].positions = positions; 201 | joint_trajectory.points[0].time_from_start = duration; 202 | 203 | publisher->publish(joint_trajectory); 204 | } 205 | 206 | 207 | void SIGVerseTb3RecognizePointedDirection::run(int argc, char** argv) 208 | { 209 | rclcpp::init(argc, argv); 210 | 211 | node_ = rclcpp::Node::make_shared("tb3_omc_recognize_pointed_direction"); 212 | 213 | floor_z_ = node_->declare_parameter("floor_z", -0.5); 214 | 215 | rclcpp::Rate loop_rate(10); 216 | 217 | pub_joint_trajectory_ = node_->create_publisher("/tb3/joint_trajectory", 10); 218 | pub_debug_markers_ = node_->create_publisher ("/tb3/debug_markers", 10); 219 | 220 | auto sub_instruction = node_->create_subscription ("/tb3/instruction", 10, std::bind(&SIGVerseTb3RecognizePointedDirection::instruction_callback, this, std::placeholders::_1)); 221 | auto sub_yolo_detections = node_->create_subscription ("/yolo_human_pose/detections_3d", 10, std::bind(&SIGVerseTb3RecognizePointedDirection::yolo_detection_callback, this, std::placeholders::_1)); 222 | 223 | sleep(1); 224 | 225 | rclcpp::spin(node_); 226 | 227 | return; 228 | } 229 | 230 | 231 | int main(int argc, char** argv) 232 | { 233 | SIGVerseTb3RecognizePointedDirection recognize_pointed_direction; 234 | 235 | recognize_pointed_direction.run(argc, argv); 236 | 237 | return(EXIT_SUCCESS); 238 | } 239 | -------------------------------------------------------------------------------- /examples/turtlebot3/launch/grasping_auto_launch.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Grid1/Offset1 9 | - /TF1/Frames1 10 | - /DepthCloud1/Auto Size1 11 | - /Image1 12 | - /MarkerArray1 13 | - /MarkerArray1/Topic1 14 | - /MarkerArray1/Namespaces1 15 | Splitter Ratio: 0.5 16 | Tree Height: 229 17 | - Class: rviz_common/Selection 18 | Name: Selection 19 | - Class: rviz_common/Tool Properties 20 | Expanded: 21 | - /2D Goal Pose1 22 | - /Publish Point1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.5886790156364441 25 | - Class: rviz_common/Views 26 | Expanded: 27 | - /Current View1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz_common/Time 31 | Experimental: false 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: "" 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz_default_plugins/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: -0.5 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Class: rviz_default_plugins/TF 57 | Enabled: true 58 | Frame Timeout: 15 59 | Frames: 60 | All Enabled: true 61 | base_footprint: 62 | Value: true 63 | base_link: 64 | Value: true 65 | base_scan: 66 | Value: true 67 | caster_back_left_link: 68 | Value: true 69 | caster_back_right_link: 70 | Value: true 71 | dummy_mimic_fix: 72 | Value: true 73 | end_effector_link: 74 | Value: true 75 | gripper_left_link: 76 | Value: true 77 | gripper_right_link: 78 | Value: true 79 | link1: 80 | Value: true 81 | link2: 82 | Value: true 83 | link3: 84 | Value: true 85 | link4: 86 | Value: true 87 | link5: 88 | Value: true 89 | odom: 90 | Value: true 91 | wheel_left_link: 92 | Value: true 93 | wheel_right_link: 94 | Value: true 95 | zedm_base_link: 96 | Value: true 97 | zedm_camera_center: 98 | Value: true 99 | zedm_left_camera_frame: 100 | Value: true 101 | zedm_left_camera_optical_frame: 102 | Value: true 103 | zedm_right_camera_frame: 104 | Value: true 105 | zedm_right_camera_optical_frame: 106 | Value: true 107 | Marker Scale: 1 108 | Name: TF 109 | Show Arrows: true 110 | Show Axes: true 111 | Show Names: false 112 | Tree: 113 | odom: 114 | base_footprint: 115 | base_link: 116 | base_scan: 117 | {} 118 | caster_back_left_link: 119 | {} 120 | caster_back_right_link: 121 | {} 122 | link1: 123 | link2: 124 | link3: 125 | link4: 126 | link5: 127 | dummy_mimic_fix: 128 | {} 129 | end_effector_link: 130 | {} 131 | gripper_left_link: 132 | {} 133 | gripper_right_link: 134 | {} 135 | wheel_left_link: 136 | {} 137 | wheel_right_link: 138 | {} 139 | zedm_base_link: 140 | zedm_camera_center: 141 | zedm_left_camera_frame: 142 | zedm_left_camera_optical_frame: 143 | {} 144 | zedm_right_camera_frame: 145 | zedm_right_camera_optical_frame: 146 | {} 147 | Update Interval: 0 148 | Value: true 149 | - Alpha: 1 150 | Auto Size: 151 | Auto Size Factor: 1 152 | Value: true 153 | Autocompute Intensity Bounds: true 154 | Autocompute Value Bounds: 155 | Max Value: 10 156 | Min Value: -10 157 | Value: true 158 | Axis: Z 159 | Channel Name: intensity 160 | Class: rviz_default_plugins/DepthCloud 161 | Color: 255; 255; 255 162 | Color Image Topic: "" 163 | Color Transformer: RGB8 164 | Color Transport Hint: raw 165 | Decay Time: 0 166 | Depth Map Topic: /camera/depth/image_raw 167 | Depth Map Transport Hint: raw 168 | Enabled: false 169 | Invert Rainbow: false 170 | Max Color: 255; 255; 255 171 | Max Intensity: 4096 172 | Min Color: 0; 0; 0 173 | Min Intensity: 0 174 | Name: DepthCloud 175 | Occlusion Compensation: 176 | Occlusion Time-Out: 30 177 | Value: false 178 | Position Transformer: XYZ 179 | Queue Size: 1 180 | Reliability Policy: Best effort 181 | Selectable: true 182 | Size (Pixels): 1 183 | Style: Points 184 | Topic Filter: true 185 | Use Fixed Frame: true 186 | Use rainbow: true 187 | Value: false 188 | - Class: rviz_default_plugins/Image 189 | Enabled: true 190 | Max Value: 1 191 | Median window: 5 192 | Min Value: 0 193 | Name: Image 194 | Normalize Range: true 195 | Topic: 196 | Depth: 5 197 | Durability Policy: Volatile 198 | History Policy: Keep Last 199 | Reliability Policy: Reliable 200 | Value: /yolo_objects/dbg_image 201 | Value: true 202 | - Class: rviz_default_plugins/MarkerArray 203 | Enabled: true 204 | Name: MarkerArray 205 | Namespaces: 206 | {} 207 | Topic: 208 | Depth: 5 209 | Durability Policy: Volatile 210 | History Policy: Keep Last 211 | Reliability Policy: Reliable 212 | Value: /tb3omc/debug_markers 213 | Value: true 214 | Enabled: true 215 | Global Options: 216 | Background Color: 48; 48; 48 217 | Fixed Frame: odom 218 | Frame Rate: 30 219 | Name: root 220 | Tools: 221 | - Class: rviz_default_plugins/Interact 222 | Hide Inactive Objects: true 223 | - Class: rviz_default_plugins/MoveCamera 224 | - Class: rviz_default_plugins/Select 225 | - Class: rviz_default_plugins/FocusCamera 226 | - Class: rviz_default_plugins/Measure 227 | Line color: 128; 128; 0 228 | - Class: rviz_default_plugins/SetInitialPose 229 | Covariance x: 0.25 230 | Covariance y: 0.25 231 | Covariance yaw: 0.06853891909122467 232 | Topic: 233 | Depth: 5 234 | Durability Policy: Volatile 235 | History Policy: Keep Last 236 | Reliability Policy: Reliable 237 | Value: /initialpose 238 | - Class: rviz_default_plugins/SetGoal 239 | Topic: 240 | Depth: 5 241 | Durability Policy: Volatile 242 | History Policy: Keep Last 243 | Reliability Policy: Reliable 244 | Value: /goal_pose 245 | - Class: rviz_default_plugins/PublishPoint 246 | Single click: true 247 | Topic: 248 | Depth: 5 249 | Durability Policy: Volatile 250 | History Policy: Keep Last 251 | Reliability Policy: Reliable 252 | Value: /clicked_point 253 | Transformation: 254 | Current: 255 | Class: rviz_default_plugins/TF 256 | Value: true 257 | Views: 258 | Current: 259 | Class: rviz_default_plugins/Orbit 260 | Distance: 6.644200801849365 261 | Enable Stereo Rendering: 262 | Stereo Eye Separation: 0.05999999865889549 263 | Stereo Focal Distance: 1 264 | Swap Stereo Eyes: false 265 | Value: false 266 | Focal Point: 267 | X: 0.4672154188156128 268 | Y: -0.27640262246131897 269 | Z: 0.053322188556194305 270 | Focal Shape Fixed Size: true 271 | Focal Shape Size: 0.05000000074505806 272 | Invert Z Axis: false 273 | Name: Current View 274 | Near Clip Distance: 0.009999999776482582 275 | Pitch: 0.7903977632522583 276 | Target Frame: 277 | Value: Orbit (rviz) 278 | Yaw: 1.8753706216812134 279 | Saved: ~ 280 | Window Geometry: 281 | Displays: 282 | collapsed: false 283 | Height: 786 284 | Hide Left Dock: false 285 | Hide Right Dock: false 286 | Image: 287 | collapsed: false 288 | QMainWindow State: 000000ff00000000fd00000004000000000000024600000274fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000122000000c900fffffffb0000000a0049006d00610067006501000001650000014c0000002800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b00000236fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000236000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ee0000003efc0100000002fb0000000800540069006d00650100000000000004ee000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002a20000027400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 289 | Selection: 290 | collapsed: false 291 | Time: 292 | collapsed: false 293 | Tool Properties: 294 | collapsed: false 295 | Views: 296 | collapsed: false 297 | Width: 1262 298 | X: 615 299 | Y: 65 300 | -------------------------------------------------------------------------------- /examples/hsr/teleop_key/launch/hsr.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.6294117569923401 8 | Tree Height: 449 9 | - Class: rviz_common/Selection 10 | Name: Selection 11 | - Class: rviz_common/Tool Properties 12 | Expanded: 13 | - /2D Goal Pose1 14 | - /Publish Point1 15 | Name: Tool Properties 16 | Splitter Ratio: 0.5886790156364441 17 | - Class: rviz_common/Views 18 | Expanded: 19 | - /Current View1 20 | Name: Views 21 | Splitter Ratio: 0.5 22 | - Class: rviz_common/Time 23 | Experimental: false 24 | Name: Time 25 | SyncMode: 0 26 | SyncSource: "" 27 | Visualization Manager: 28 | Class: "" 29 | Displays: 30 | - Alpha: 0.5 31 | Cell Size: 1 32 | Class: rviz_default_plugins/Grid 33 | Color: 160; 160; 164 34 | Enabled: true 35 | Line Style: 36 | Line Width: 0.029999999329447746 37 | Value: Lines 38 | Name: Grid 39 | Normal Cell Count: 0 40 | Offset: 41 | X: 0 42 | Y: 0 43 | Z: 0 44 | Plane: XY 45 | Plane Cell Count: 10 46 | Reference Frame: 47 | Value: true 48 | - Alpha: 1 49 | Autocompute Intensity Bounds: true 50 | Autocompute Value Bounds: 51 | Max Value: 10 52 | Min Value: -10 53 | Value: true 54 | Axis: Z 55 | Channel Name: intensity 56 | Class: rviz_default_plugins/LaserScan 57 | Color: 255; 255; 255 58 | Color Transformer: Intensity 59 | Decay Time: 0 60 | Enabled: true 61 | Invert Rainbow: false 62 | Max Color: 255; 255; 255 63 | Max Intensity: 0 64 | Min Color: 0; 0; 0 65 | Min Intensity: 0 66 | Name: LaserScan 67 | Position Transformer: XYZ 68 | Selectable: true 69 | Size (Pixels): 3 70 | Size (m): 0.009999999776482582 71 | Style: Flat Squares 72 | Topic: 73 | Depth: 5 74 | Durability Policy: Volatile 75 | Filter size: 10 76 | History Policy: Keep Last 77 | Reliability Policy: Reliable 78 | Value: /hsrb/base_scan 79 | Use Fixed Frame: true 80 | Use rainbow: true 81 | Value: true 82 | - Class: rviz_default_plugins/TF 83 | Enabled: true 84 | Frame Timeout: 15 85 | Frames: 86 | All Enabled: true 87 | Marker Scale: 1 88 | Name: TF 89 | Show Arrows: true 90 | Show Axes: true 91 | Show Names: false 92 | Tree: 93 | {} 94 | Update Interval: 0 95 | Value: true 96 | - Alpha: 0.699999988079071 97 | Class: rviz_default_plugins/Map 98 | Color Scheme: map 99 | Draw Behind: false 100 | Enabled: false 101 | Name: Map 102 | Topic: 103 | Depth: 5 104 | Durability Policy: Volatile 105 | Filter size: 10 106 | History Policy: Keep Last 107 | Reliability Policy: Reliable 108 | Value: /map 109 | Update Topic: 110 | Depth: 5 111 | Durability Policy: Volatile 112 | History Policy: Keep Last 113 | Reliability Policy: Reliable 114 | Value: /map_updates 115 | Use Timestamp: false 116 | Value: false 117 | - Class: rviz_default_plugins/Image 118 | Enabled: true 119 | Max Value: 1 120 | Median window: 5 121 | Min Value: 0 122 | Name: Hand 123 | Normalize Range: true 124 | Topic: 125 | Depth: 5 126 | Durability Policy: Volatile 127 | History Policy: Keep Last 128 | Reliability Policy: Reliable 129 | Value: /hsrb/hand_camera/image_raw 130 | Value: true 131 | - Class: rviz_default_plugins/Image 132 | Enabled: true 133 | Max Value: 1 134 | Median window: 5 135 | Min Value: 0 136 | Name: Head Depth 137 | Normalize Range: true 138 | Topic: 139 | Depth: 5 140 | Durability Policy: Volatile 141 | History Policy: Keep Last 142 | Reliability Policy: Reliable 143 | Value: /hsrb/head_rgbd_sensor/depth_registered/image_raw 144 | Value: true 145 | - Class: rviz_default_plugins/Image 146 | Enabled: false 147 | Max Value: 1 148 | Median window: 5 149 | Min Value: 0 150 | Name: Head RGB 151 | Normalize Range: true 152 | Topic: 153 | Depth: 5 154 | Durability Policy: Volatile 155 | History Policy: Keep Last 156 | Reliability Policy: Reliable 157 | Value: /hsrb/head_rgbd_sensor/rgb/image_raw 158 | Value: false 159 | - Class: rviz_default_plugins/Image 160 | Enabled: true 161 | Max Value: 1 162 | Median window: 5 163 | Min Value: 0 164 | Name: Head Center 165 | Normalize Range: true 166 | Topic: 167 | Depth: 5 168 | Durability Policy: Volatile 169 | History Policy: Keep Last 170 | Reliability Policy: Reliable 171 | Value: /hsrb/head_center_camera/image_raw 172 | Value: true 173 | - Class: rviz_default_plugins/Image 174 | Enabled: false 175 | Max Value: 1 176 | Median window: 5 177 | Min Value: 0 178 | Name: Head Stereo Left 179 | Normalize Range: true 180 | Topic: 181 | Depth: 5 182 | Durability Policy: Volatile 183 | History Policy: Keep Last 184 | Reliability Policy: Reliable 185 | Value: /hsrb/head_l_stereo_camera/image_rect_color 186 | Value: false 187 | - Class: rviz_default_plugins/Image 188 | Enabled: false 189 | Max Value: 1 190 | Median window: 5 191 | Min Value: 0 192 | Name: Head Stereo Right 193 | Normalize Range: true 194 | Topic: 195 | Depth: 5 196 | Durability Policy: Volatile 197 | History Policy: Keep Last 198 | Reliability Policy: Reliable 199 | Value: /hsrb/head_r_stereo_camera/image_rect_color 200 | Value: false 201 | - Alpha: 1 202 | Autocompute Intensity Bounds: true 203 | Autocompute Value Bounds: 204 | Max Value: 10 205 | Min Value: -10 206 | Value: true 207 | Axis: Z 208 | Channel Name: intensity 209 | Class: rviz_default_plugins/PointCloud2 210 | Color: 255; 255; 255 211 | Color Transformer: "" 212 | Decay Time: 0 213 | Enabled: false 214 | Invert Rainbow: false 215 | Max Color: 255; 255; 255 216 | Max Intensity: 4096 217 | Min Color: 0; 0; 0 218 | Min Intensity: 0 219 | Name: PointCloud2 220 | Position Transformer: "" 221 | Selectable: true 222 | Size (Pixels): 3 223 | Size (m): 0.009999999776482582 224 | Style: Flat Squares 225 | Topic: 226 | Depth: 5 227 | Durability Policy: Volatile 228 | Filter size: 10 229 | History Policy: Keep Last 230 | Reliability Policy: Reliable 231 | Value: /hsrb/head_rgbd_sensor/depth_registered/points 232 | Use Fixed Frame: true 233 | Use rainbow: true 234 | Value: false 235 | Enabled: true 236 | Global Options: 237 | Background Color: 48; 48; 48 238 | Fixed Frame: odom 239 | Frame Rate: 30 240 | Name: root 241 | Tools: 242 | - Class: rviz_default_plugins/Interact 243 | Hide Inactive Objects: true 244 | - Class: rviz_default_plugins/MoveCamera 245 | - Class: rviz_default_plugins/Select 246 | - Class: rviz_default_plugins/FocusCamera 247 | - Class: rviz_default_plugins/Measure 248 | Line color: 128; 128; 0 249 | - Class: rviz_default_plugins/SetInitialPose 250 | Covariance x: 0.25 251 | Covariance y: 0.25 252 | Covariance yaw: 0.06853891909122467 253 | Topic: 254 | Depth: 5 255 | Durability Policy: Volatile 256 | History Policy: Keep Last 257 | Reliability Policy: Reliable 258 | Value: /initialpose 259 | - Class: rviz_default_plugins/SetGoal 260 | Topic: 261 | Depth: 5 262 | Durability Policy: Volatile 263 | History Policy: Keep Last 264 | Reliability Policy: Reliable 265 | Value: /goal_pose 266 | - Class: rviz_default_plugins/PublishPoint 267 | Single click: true 268 | Topic: 269 | Depth: 5 270 | Durability Policy: Volatile 271 | History Policy: Keep Last 272 | Reliability Policy: Reliable 273 | Value: /clicked_point 274 | Transformation: 275 | Current: 276 | Class: rviz_default_plugins/TF 277 | Value: true 278 | Views: 279 | Current: 280 | Class: rviz_default_plugins/Orbit 281 | Distance: 3.012810707092285 282 | Enable Stereo Rendering: 283 | Stereo Eye Separation: 0.05999999865889549 284 | Stereo Focal Distance: 1 285 | Swap Stereo Eyes: false 286 | Value: false 287 | Focal Point: 288 | X: -0.14633533358573914 289 | Y: -0.21723000705242157 290 | Z: 0.2570796012878418 291 | Focal Shape Fixed Size: true 292 | Focal Shape Size: 0.05000000074505806 293 | Invert Z Axis: false 294 | Name: Current View 295 | Near Clip Distance: 0.009999999776482582 296 | Pitch: 0.6253981590270996 297 | Target Frame: 298 | Value: Orbit (rviz) 299 | Yaw: 0.7253977656364441 300 | Saved: ~ 301 | Window Geometry: 302 | Displays: 303 | collapsed: false 304 | Hand: 305 | collapsed: false 306 | Head Center: 307 | collapsed: false 308 | Head Depth: 309 | collapsed: false 310 | Head RGB: 311 | collapsed: false 312 | Head Stereo Left: 313 | collapsed: false 314 | Head Stereo Right: 315 | collapsed: false 316 | Height: 746 317 | Hide Left Dock: false 318 | Hide Right Dock: false 319 | QMainWindow State: 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 320 | Selection: 321 | collapsed: false 322 | Time: 323 | collapsed: false 324 | Tool Properties: 325 | collapsed: false 326 | Views: 327 | collapsed: false 328 | Width: 1179 329 | X: 579 330 | Y: 82 331 | -------------------------------------------------------------------------------- /examples/turtlebot3/src/turtlebot3_teleop_key.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class SIGVerseTb3TeleopKey 13 | { 14 | private: 15 | static const char KEYCODE_UP = 0x41; 16 | static const char KEYCODE_DOWN = 0x42; 17 | static const char KEYCODE_RIGHT = 0x43; 18 | static const char KEYCODE_LEFT = 0x44; 19 | 20 | const std::string JOINT1_NAME = "joint1"; 21 | const std::string JOINT2_NAME = "joint2"; 22 | const std::string JOINT3_NAME = "joint3"; 23 | const std::string JOINT4_NAME = "joint4"; 24 | 25 | const std::string GRIP_JOINT_NAME = "gripper_left_joint"; 26 | const std::string GRIP_JOINT_SUB_NAME = "gripper_right_joint"; 27 | 28 | const double LINEAR_VEL = 0.2; 29 | const double ANGULAR_VEL = 0.4; 30 | const double JOINT_MIN = -2.83; 31 | const double JOINT_MAX = +2.83; 32 | const double GRIP_MIN = -0.01; 33 | const double GRIP_MAX = +0.035; 34 | 35 | public: 36 | SIGVerseTb3TeleopKey(); 37 | 38 | void key_loop(int argc, char** argv); 39 | 40 | private: 41 | static int can_receive_key( const int fd ); 42 | 43 | void joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr joint_state); 44 | void move_base(rclcpp::Publisher::SharedPtr publisher, const double linear_x, const double angular_z); 45 | void move_arm(rclcpp::Publisher::SharedPtr publisher, const std::string &name, const double position, const double current_pos); 46 | void move_hand(rclcpp::Publisher::SharedPtr publisher, const double position, const double current_pos); 47 | void stop_joints(rclcpp::Publisher::SharedPtr publisher, const int duration_sec); 48 | 49 | static int calc_trajectory_duration(const double val, const double current_val); 50 | 51 | void show_help(); 52 | 53 | rclcpp::Node::SharedPtr node_; 54 | 55 | // Current positions updated by JointState 56 | double joint1_pos1_, joint2_pos1_, joint3_pos1_, joint4_pos1_, grip_joint_pos1_; 57 | double joint1_pos2_, joint2_pos2_, joint3_pos2_, joint4_pos2_; 58 | }; 59 | 60 | 61 | SIGVerseTb3TeleopKey::SIGVerseTb3TeleopKey() 62 | { 63 | joint1_pos1_ = 0.0; joint2_pos1_ = 0.0; joint3_pos1_ = 0.0; joint4_pos1_ = 0.0; grip_joint_pos1_ = 0.0; 64 | joint1_pos2_ = 0.0; joint2_pos2_ = 0.0; joint3_pos2_ = 0.0; joint4_pos2_ = 0.0; 65 | } 66 | 67 | 68 | int SIGVerseTb3TeleopKey::can_receive_key( const int fd ) 69 | { 70 | fd_set fdset; 71 | struct timeval timeout; 72 | FD_ZERO( &fdset ); 73 | FD_SET( fd , &fdset ); 74 | 75 | timeout.tv_sec = 0; 76 | timeout.tv_usec = 0; 77 | 78 | return select( fd+1 , &fdset , NULL , NULL , &timeout ); 79 | } 80 | 81 | void SIGVerseTb3TeleopKey::joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr joint_state) 82 | { 83 | for(size_t i=0; iname.size(); i++) 84 | { 85 | if(joint_state->name[i]==JOINT1_NAME) 86 | { 87 | joint1_pos2_ = joint1_pos1_; 88 | joint1_pos1_ = joint_state->position[i]; 89 | } 90 | else if(joint_state->name[i]==JOINT2_NAME) 91 | { 92 | joint2_pos2_ = joint2_pos1_; 93 | joint2_pos1_ = joint_state->position[i]; 94 | } 95 | else if(joint_state->name[i]==JOINT3_NAME) 96 | { 97 | joint3_pos2_ = joint3_pos1_; 98 | joint3_pos1_ = joint_state->position[i]; 99 | } 100 | else if(joint_state->name[i]==JOINT4_NAME) 101 | { 102 | joint4_pos2_ = joint4_pos1_; 103 | joint4_pos1_ = joint_state->position[i]; 104 | } 105 | else if(joint_state->name[i]==GRIP_JOINT_NAME) 106 | { 107 | grip_joint_pos1_ = joint_state->position[i]; 108 | } 109 | } 110 | } 111 | 112 | void SIGVerseTb3TeleopKey::move_base(rclcpp::Publisher::SharedPtr publisher, const double linear_x, const double angular_z) 113 | { 114 | geometry_msgs::msg::Twist twist; 115 | 116 | twist.linear.x = linear_x; 117 | twist.linear.y = 0.0; 118 | twist.linear.z = 0.0; 119 | twist.angular.x = 0.0; 120 | twist.angular.y = 0.0; 121 | twist.angular.z = angular_z; 122 | 123 | publisher->publish(twist); 124 | } 125 | 126 | 127 | void SIGVerseTb3TeleopKey::move_arm(rclcpp::Publisher::SharedPtr publisher, const std::string &name, const double position, const double current_pos) 128 | { 129 | std::vector names; 130 | names.push_back(name); 131 | 132 | std::vector positions; 133 | positions.push_back(position); 134 | 135 | double duration_sec = calc_trajectory_duration(position, current_pos); 136 | 137 | builtin_interfaces::msg::Duration duration; 138 | duration.sec = static_cast(duration_sec); 139 | duration.nanosec = static_cast((duration_sec - duration.sec) * 1e9); 140 | 141 | trajectory_msgs::msg::JointTrajectory joint_trajectory; 142 | 143 | trajectory_msgs::msg::JointTrajectoryPoint arm_joint_point; 144 | 145 | joint_trajectory.points.push_back(arm_joint_point); 146 | 147 | joint_trajectory.joint_names = names; 148 | joint_trajectory.points[0].positions = positions; 149 | joint_trajectory.points[0].time_from_start = duration; 150 | 151 | publisher->publish(joint_trajectory); 152 | } 153 | 154 | void SIGVerseTb3TeleopKey::move_hand(rclcpp::Publisher::SharedPtr publisher, const double position, const double current_pos) 155 | { 156 | std::vector joint_names {GRIP_JOINT_NAME, GRIP_JOINT_SUB_NAME}; 157 | 158 | std::vector positions; 159 | 160 | positions.push_back(position); // for gripper_left_joint 161 | positions.push_back(position); // for gripper_right_joint 162 | 163 | double duration_sec = calc_trajectory_duration(position, current_pos); 164 | 165 | builtin_interfaces::msg::Duration duration; 166 | duration.sec = static_cast(duration_sec); 167 | duration.nanosec = static_cast((duration_sec - duration.sec) * 1e9); 168 | 169 | trajectory_msgs::msg::JointTrajectoryPoint point; 170 | point.positions = positions; 171 | point.time_from_start = duration; 172 | 173 | trajectory_msgs::msg::JointTrajectory joint_trajectory; 174 | joint_trajectory.joint_names = joint_names; 175 | joint_trajectory.points.push_back(point); 176 | 177 | publisher->publish(joint_trajectory); 178 | } 179 | 180 | 181 | void SIGVerseTb3TeleopKey::stop_joints(rclcpp::Publisher::SharedPtr publisher, const int duration_sec) 182 | { 183 | std::vector names {JOINT1_NAME, JOINT2_NAME, JOINT3_NAME, JOINT4_NAME}; 184 | 185 | std::vector positions; 186 | positions.push_back(2.0*joint1_pos1_-joint1_pos2_); 187 | positions.push_back(2.0*joint2_pos1_-joint2_pos2_); 188 | positions.push_back(2.0*joint3_pos1_-joint3_pos2_); 189 | positions.push_back(2.0*joint4_pos1_-joint4_pos2_); 190 | 191 | builtin_interfaces::msg::Duration duration; 192 | duration.sec = duration_sec; 193 | duration.nanosec = 0; 194 | 195 | trajectory_msgs::msg::JointTrajectory joint_trajectory; 196 | 197 | trajectory_msgs::msg::JointTrajectoryPoint arm_joint_point; 198 | 199 | joint_trajectory.points.push_back(arm_joint_point); 200 | 201 | joint_trajectory.joint_names = names; 202 | joint_trajectory.points[0].positions = positions; 203 | joint_trajectory.points[0].time_from_start = duration; 204 | 205 | publisher->publish(joint_trajectory); 206 | } 207 | 208 | 209 | int SIGVerseTb3TeleopKey::calc_trajectory_duration(const double val, const double current_val) 210 | { 211 | return std::max((int)(std::abs(val - current_val) / 0.5), 1); 212 | } 213 | 214 | 215 | void SIGVerseTb3TeleopKey::show_help() 216 | { 217 | puts("\n"); 218 | puts("---------------------------"); 219 | puts("Operate from keyboard"); 220 | puts("---------------------------"); 221 | puts("arrow keys : Move"); 222 | puts("---------------------------"); 223 | puts("Space: Stop"); 224 | puts("---------------------------"); 225 | puts("w: Go Forward"); 226 | puts("s: Go Back"); 227 | puts("d: Turn Right"); 228 | puts("a: Turn Left"); 229 | puts("---------------------------"); 230 | puts("u: Rotate Arm - Upward"); 231 | puts("j: Rotate Arm - Horizontal"); 232 | puts("m: Rotate Arm - Downward"); 233 | puts("---------------------------"); 234 | puts("1/2: Joint1 Right/Left"); 235 | puts("3/4: Joint2 Up/Down"); 236 | puts("5/6: Joint3 Up/Down"); 237 | puts("7/8: Joint4 Up/Down"); 238 | puts("---------------------------"); 239 | puts("o: Hand Open"); 240 | puts("c: Hand Close"); 241 | puts("---------------------------"); 242 | puts("h: Show help"); 243 | } 244 | 245 | 246 | void SIGVerseTb3TeleopKey::key_loop(int argc, char** argv) 247 | { 248 | char c; 249 | int ret; 250 | char buf[1024]; 251 | 252 | ///////////////////////////////////////////// 253 | // get the console in raw mode 254 | int kfd = 0; 255 | struct termios cooked; 256 | 257 | struct termios raw; 258 | tcgetattr(kfd, &cooked); 259 | memcpy(&raw, &cooked, sizeof(struct termios)); 260 | raw.c_lflag &=~ (ICANON | ECHO); 261 | raw.c_cc[VEOL] = 1; 262 | raw.c_cc[VEOF] = 2; 263 | tcsetattr(kfd, TCSANOW, &raw); 264 | ///////////////////////////////////////////// 265 | 266 | rclcpp::init(argc, argv); 267 | 268 | node_ = rclcpp::Node::make_shared("tb3_omc_teleop_key"); 269 | 270 | auto logger = node_->get_logger(); 271 | 272 | rclcpp::Rate loop_rate(50); 273 | 274 | auto sub_joint_state = node_->create_subscription ("/tb3/joint_state", 10, std::bind(&SIGVerseTb3TeleopKey::joint_state_callback, this, std::placeholders::_1)); 275 | auto pub_base_twist = node_->create_publisher ("/tb3/cmd_vel", 10); 276 | auto pub_joint_traj = node_->create_publisher("/tb3/joint_trajectory", 10); 277 | 278 | sleep(2); 279 | 280 | show_help(); 281 | 282 | while (rclcpp::ok()) 283 | { 284 | if(can_receive_key(kfd)) 285 | { 286 | // get the next event from the keyboard 287 | if((ret = read(kfd, &buf, sizeof(buf))) < 0) 288 | { 289 | perror("read():"); 290 | exit(EXIT_FAILURE); 291 | } 292 | 293 | c = buf[ret-1]; 294 | 295 | switch(c) 296 | { 297 | case ' ': 298 | { 299 | RCLCPP_DEBUG(logger, "Stop"); 300 | move_base(pub_base_twist, 0.0, 0.0); 301 | stop_joints(pub_joint_traj, 1.0); 302 | break; 303 | } 304 | case 'w': 305 | case KEYCODE_UP: 306 | { 307 | RCLCPP_DEBUG(logger, "Go Forward"); 308 | move_base(pub_base_twist, +LINEAR_VEL, 0.0); 309 | break; 310 | } 311 | case 's': 312 | case KEYCODE_DOWN: 313 | { 314 | RCLCPP_DEBUG(logger, "Go Back"); 315 | move_base(pub_base_twist, -LINEAR_VEL, 0.0); 316 | break; 317 | } 318 | case 'd': 319 | case KEYCODE_RIGHT: 320 | { 321 | RCLCPP_DEBUG(logger, "Turn Right"); 322 | move_base(pub_base_twist, 0.0, -ANGULAR_VEL); 323 | break; 324 | } 325 | case 'a': 326 | case KEYCODE_LEFT: 327 | { 328 | RCLCPP_DEBUG(logger, "Turn Left"); 329 | move_base(pub_base_twist, 0.0, +ANGULAR_VEL); 330 | break; 331 | } 332 | case 'u': 333 | { 334 | RCLCPP_DEBUG(logger, "Rotate Arm - Upward"); 335 | move_arm(pub_joint_traj, JOINT2_NAME, 0.0, joint2_pos1_); 336 | move_arm(pub_joint_traj, JOINT3_NAME, 0.0, joint3_pos1_); 337 | move_arm(pub_joint_traj, JOINT4_NAME, 0.0, joint4_pos1_); 338 | break; 339 | } 340 | case 'j': 341 | { 342 | RCLCPP_DEBUG(logger, "Rotate Arm - Horizontal"); 343 | move_arm(pub_joint_traj, JOINT2_NAME, +1.20, joint2_pos1_); 344 | move_arm(pub_joint_traj, JOINT3_NAME, -0.80, joint3_pos1_); 345 | move_arm(pub_joint_traj, JOINT4_NAME, +0.00, joint4_pos1_); 346 | break; 347 | } 348 | case 'm': 349 | { 350 | RCLCPP_DEBUG(logger, "Rotate Arm - Downward"); 351 | move_arm(pub_joint_traj, JOINT2_NAME, +1.20, joint2_pos1_); 352 | move_arm(pub_joint_traj, JOINT3_NAME, -0.80, joint3_pos1_); 353 | move_arm(pub_joint_traj, JOINT4_NAME, +0.80, joint4_pos1_); 354 | break; 355 | } 356 | case '1': 357 | { 358 | RCLCPP_DEBUG(logger, "Joint1 Right"); 359 | move_arm(pub_joint_traj, JOINT1_NAME, JOINT_MIN, joint1_pos1_); 360 | break; 361 | } 362 | case '2': 363 | { 364 | RCLCPP_DEBUG(logger, "Joint1 Left"); 365 | move_arm(pub_joint_traj, JOINT1_NAME, JOINT_MAX, joint1_pos1_); 366 | break; 367 | } 368 | case '3': 369 | { 370 | RCLCPP_DEBUG(logger, "Joint2 Up"); 371 | move_arm(pub_joint_traj, JOINT2_NAME, JOINT_MIN, joint2_pos1_); 372 | break; 373 | } 374 | case '4': 375 | { 376 | RCLCPP_DEBUG(logger, "Joint2 Down"); 377 | move_arm(pub_joint_traj, JOINT2_NAME, JOINT_MAX, joint2_pos1_); 378 | break; 379 | } 380 | case '5': 381 | { 382 | RCLCPP_DEBUG(logger, "Joint3 Up"); 383 | move_arm(pub_joint_traj, JOINT3_NAME, JOINT_MIN, joint3_pos1_); 384 | break; 385 | } 386 | case '6': 387 | { 388 | RCLCPP_DEBUG(logger, "Joint3 Down"); 389 | move_arm(pub_joint_traj, JOINT3_NAME, JOINT_MAX, joint3_pos1_); 390 | break; 391 | } 392 | case '7': 393 | { 394 | RCLCPP_DEBUG(logger, "Joint4 Up"); 395 | move_arm(pub_joint_traj, JOINT4_NAME, JOINT_MIN, joint4_pos1_); 396 | break; 397 | } 398 | case '8': 399 | { 400 | RCLCPP_DEBUG(logger, "Joint4 Down"); 401 | move_arm(pub_joint_traj, JOINT4_NAME, JOINT_MAX, joint4_pos1_); 402 | break; 403 | } 404 | case 'o': 405 | { 406 | RCLCPP_DEBUG(logger, "Hand Open"); 407 | move_hand(pub_joint_traj, GRIP_MIN, grip_joint_pos1_); 408 | break; 409 | } 410 | case 'c': 411 | { 412 | RCLCPP_DEBUG(logger, "Hand Close"); 413 | move_hand(pub_joint_traj, GRIP_MAX, grip_joint_pos1_); 414 | break; 415 | } 416 | case 'h': 417 | { 418 | RCLCPP_DEBUG(logger, "Show Help"); 419 | show_help(); 420 | break; 421 | } 422 | } 423 | } 424 | 425 | rclcpp::spin_some(node_); 426 | 427 | loop_rate.sleep(); 428 | } 429 | 430 | ///////////////////////////////////////////// 431 | // cooked mode 432 | tcsetattr(kfd, TCSANOW, &cooked); 433 | ///////////////////////////////////////////// 434 | 435 | return; 436 | } 437 | 438 | 439 | int main(int argc, char** argv) 440 | { 441 | SIGVerseTb3TeleopKey teleop_key; 442 | 443 | teleop_key.key_loop(argc, argv); 444 | 445 | return(EXIT_SUCCESS); 446 | } 447 | 448 | -------------------------------------------------------------------------------- /examples/tiago/launch/tiago.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.3309352397918701 8 | Tree Height: 402 9 | - Class: rviz_common/Selection 10 | Name: Selection 11 | - Class: rviz_common/Tool Properties 12 | Expanded: 13 | - /2D Goal Pose1 14 | - /Publish Point1 15 | Name: Tool Properties 16 | Splitter Ratio: 0.5886790156364441 17 | - Class: rviz_common/Views 18 | Expanded: 19 | - /Current View1 20 | Name: Views 21 | Splitter Ratio: 0.5 22 | - Class: rviz_common/Time 23 | Experimental: false 24 | Name: Time 25 | SyncMode: 0 26 | SyncSource: LaserScan 27 | Visualization Manager: 28 | Class: "" 29 | Displays: 30 | - Alpha: 0.5 31 | Cell Size: 1 32 | Class: rviz_default_plugins/Grid 33 | Color: 160; 160; 164 34 | Enabled: true 35 | Line Style: 36 | Line Width: 0.029999999329447746 37 | Value: Lines 38 | Name: Grid 39 | Normal Cell Count: 0 40 | Offset: 41 | X: 0 42 | Y: 0 43 | Z: 0 44 | Plane: XY 45 | Plane Cell Count: 10 46 | Reference Frame: 47 | Value: true 48 | - Alpha: 1 49 | Autocompute Intensity Bounds: true 50 | Autocompute Value Bounds: 51 | Max Value: 10 52 | Min Value: -10 53 | Value: true 54 | Axis: Z 55 | Channel Name: intensity 56 | Class: rviz_default_plugins/LaserScan 57 | Color: 255; 255; 255 58 | Color Transformer: Intensity 59 | Decay Time: 0 60 | Enabled: true 61 | Invert Rainbow: false 62 | Max Color: 255; 255; 255 63 | Max Intensity: 0 64 | Min Color: 0; 0; 0 65 | Min Intensity: 0 66 | Name: LaserScan 67 | Position Transformer: XYZ 68 | Selectable: true 69 | Size (Pixels): 3 70 | Size (m): 0.009999999776482582 71 | Style: Flat Squares 72 | Topic: 73 | Depth: 5 74 | Durability Policy: Volatile 75 | Filter size: 10 76 | History Policy: Keep Last 77 | Reliability Policy: Reliable 78 | Value: /scan_raw 79 | Use Fixed Frame: true 80 | Use rainbow: true 81 | Value: true 82 | - Class: rviz_default_plugins/TF 83 | Enabled: true 84 | Frame Timeout: 15 85 | Frames: 86 | All Enabled: true 87 | arm_1_link: 88 | Value: true 89 | arm_2_link: 90 | Value: true 91 | arm_3_link: 92 | Value: true 93 | arm_4_link: 94 | Value: true 95 | arm_5_link: 96 | Value: true 97 | arm_6_link: 98 | Value: true 99 | arm_7_link: 100 | Value: true 101 | arm_tool_link: 102 | Value: true 103 | base_antenna_left_link: 104 | Value: true 105 | base_antenna_right_link: 106 | Value: true 107 | base_cover_link: 108 | Value: true 109 | base_footprint: 110 | Value: true 111 | base_imu_link: 112 | Value: true 113 | base_laser_link: 114 | Value: true 115 | base_link: 116 | Value: true 117 | base_mic_back_left_link: 118 | Value: true 119 | base_mic_back_right_link: 120 | Value: true 121 | base_mic_front_left_link: 122 | Value: true 123 | base_mic_front_right_link: 124 | Value: true 125 | base_sonar_01_link: 126 | Value: true 127 | base_sonar_02_link: 128 | Value: true 129 | base_sonar_03_link: 130 | Value: true 131 | caster_back_left_1_link: 132 | Value: true 133 | caster_back_left_2_link: 134 | Value: true 135 | caster_back_right_1_link: 136 | Value: true 137 | caster_back_right_2_link: 138 | Value: true 139 | caster_front_left_1_link: 140 | Value: true 141 | caster_front_left_2_link: 142 | Value: true 143 | caster_front_right_1_link: 144 | Value: true 145 | caster_front_right_2_link: 146 | Value: true 147 | gripper_grasping_frame: 148 | Value: true 149 | gripper_left_finger_link: 150 | Value: true 151 | gripper_link: 152 | Value: true 153 | gripper_right_finger_link: 154 | Value: true 155 | gripper_tool_link: 156 | Value: true 157 | head_1_link: 158 | Value: true 159 | head_2_link: 160 | Value: true 161 | odom: 162 | Value: true 163 | rgbd_laser_link: 164 | Value: true 165 | suspension_left_link: 166 | Value: true 167 | suspension_right_link: 168 | Value: true 169 | torso_fixed_column_link: 170 | Value: true 171 | torso_fixed_link: 172 | Value: true 173 | torso_lift_link: 174 | Value: true 175 | wheel_left_link: 176 | Value: true 177 | wheel_right_link: 178 | Value: true 179 | wrist_ft_link: 180 | Value: true 181 | wrist_ft_tool_link: 182 | Value: true 183 | xtion_depth_frame: 184 | Value: true 185 | xtion_depth_optical_frame: 186 | Value: true 187 | xtion_link: 188 | Value: true 189 | xtion_optical_frame: 190 | Value: true 191 | xtion_rgb_frame: 192 | Value: true 193 | xtion_rgb_optical_frame: 194 | Value: true 195 | Marker Scale: 1 196 | Name: TF 197 | Show Arrows: true 198 | Show Axes: true 199 | Show Names: false 200 | Tree: 201 | odom: 202 | base_footprint: 203 | base_cover_link: 204 | {} 205 | base_link: 206 | base_antenna_left_link: 207 | {} 208 | base_antenna_right_link: 209 | {} 210 | base_imu_link: 211 | {} 212 | base_laser_link: 213 | {} 214 | base_mic_back_left_link: 215 | {} 216 | base_mic_back_right_link: 217 | {} 218 | base_mic_front_left_link: 219 | {} 220 | base_mic_front_right_link: 221 | {} 222 | base_sonar_01_link: 223 | {} 224 | base_sonar_02_link: 225 | {} 226 | base_sonar_03_link: 227 | {} 228 | caster_back_left_1_link: 229 | caster_back_left_2_link: 230 | {} 231 | caster_back_right_1_link: 232 | caster_back_right_2_link: 233 | {} 234 | caster_front_left_1_link: 235 | caster_front_left_2_link: 236 | {} 237 | caster_front_right_1_link: 238 | caster_front_right_2_link: 239 | {} 240 | suspension_left_link: 241 | wheel_left_link: 242 | {} 243 | suspension_right_link: 244 | wheel_right_link: 245 | {} 246 | torso_fixed_column_link: 247 | {} 248 | torso_fixed_link: 249 | torso_lift_link: 250 | arm_1_link: 251 | arm_2_link: 252 | arm_3_link: 253 | arm_4_link: 254 | arm_5_link: 255 | arm_6_link: 256 | arm_7_link: 257 | arm_tool_link: 258 | wrist_ft_link: 259 | wrist_ft_tool_link: 260 | gripper_link: 261 | gripper_grasping_frame: 262 | {} 263 | gripper_left_finger_link: 264 | {} 265 | gripper_right_finger_link: 266 | {} 267 | gripper_tool_link: 268 | {} 269 | head_1_link: 270 | head_2_link: 271 | xtion_link: 272 | xtion_depth_frame: 273 | xtion_depth_optical_frame: 274 | {} 275 | xtion_optical_frame: 276 | {} 277 | xtion_rgb_frame: 278 | xtion_rgb_optical_frame: 279 | {} 280 | rgbd_laser_link: 281 | {} 282 | Update Interval: 0 283 | Value: true 284 | - Alpha: 0.699999988079071 285 | Class: rviz_default_plugins/Map 286 | Color Scheme: map 287 | Draw Behind: false 288 | Enabled: false 289 | Name: Map 290 | Topic: 291 | Depth: 5 292 | Durability Policy: Volatile 293 | Filter size: 10 294 | History Policy: Keep Last 295 | Reliability Policy: Reliable 296 | Value: /map 297 | Update Topic: 298 | Depth: 5 299 | Durability Policy: Volatile 300 | History Policy: Keep Last 301 | Reliability Policy: Reliable 302 | Value: /map_updates 303 | Use Timestamp: false 304 | Value: false 305 | - Class: rviz_default_plugins/Image 306 | Enabled: true 307 | Max Value: 1 308 | Median window: 5 309 | Min Value: 0 310 | Name: Head Depth 311 | Normalize Range: true 312 | Topic: 313 | Depth: 5 314 | Durability Policy: Volatile 315 | History Policy: Keep Last 316 | Reliability Policy: Reliable 317 | Value: /xtion/depth_registered/image_raw 318 | Value: true 319 | - Class: rviz_default_plugins/Image 320 | Enabled: true 321 | Max Value: 1 322 | Median window: 5 323 | Min Value: 0 324 | Name: Head RGB 325 | Normalize Range: true 326 | Topic: 327 | Depth: 5 328 | Durability Policy: Volatile 329 | History Policy: Keep Last 330 | Reliability Policy: Reliable 331 | Value: /xtion/rgb/image_raw 332 | Value: true 333 | - Alpha: 0.5 334 | Buffer Length: 1 335 | Class: rviz_default_plugins/Range 336 | Color: 255; 255; 255 337 | Enabled: true 338 | Name: Range 339 | Topic: 340 | Depth: 5 341 | Durability Policy: Volatile 342 | Filter size: 10 343 | History Policy: Keep Last 344 | Reliability Policy: Reliable 345 | Value: /sonar_base 346 | Value: true 347 | Enabled: true 348 | Global Options: 349 | Background Color: 48; 48; 48 350 | Fixed Frame: odom 351 | Frame Rate: 30 352 | Name: root 353 | Tools: 354 | - Class: rviz_default_plugins/Interact 355 | Hide Inactive Objects: true 356 | - Class: rviz_default_plugins/MoveCamera 357 | - Class: rviz_default_plugins/Select 358 | - Class: rviz_default_plugins/FocusCamera 359 | - Class: rviz_default_plugins/Measure 360 | Line color: 128; 128; 0 361 | - Class: rviz_default_plugins/SetInitialPose 362 | Covariance x: 0.25 363 | Covariance y: 0.25 364 | Covariance yaw: 0.06853891909122467 365 | Topic: 366 | Depth: 5 367 | Durability Policy: Volatile 368 | History Policy: Keep Last 369 | Reliability Policy: Reliable 370 | Value: /initialpose 371 | - Class: rviz_default_plugins/SetGoal 372 | Topic: 373 | Depth: 5 374 | Durability Policy: Volatile 375 | History Policy: Keep Last 376 | Reliability Policy: Reliable 377 | Value: /goal_pose 378 | - Class: rviz_default_plugins/PublishPoint 379 | Single click: true 380 | Topic: 381 | Depth: 5 382 | Durability Policy: Volatile 383 | History Policy: Keep Last 384 | Reliability Policy: Reliable 385 | Value: /clicked_point 386 | Transformation: 387 | Current: 388 | Class: rviz_default_plugins/TF 389 | Value: true 390 | Views: 391 | Current: 392 | Class: rviz_default_plugins/Orbit 393 | Distance: 7.499440670013428 394 | Enable Stereo Rendering: 395 | Stereo Eye Separation: 0.05999999865889549 396 | Stereo Focal Distance: 1 397 | Swap Stereo Eyes: false 398 | Value: false 399 | Focal Point: 400 | X: 0.7904294729232788 401 | Y: 0.2764089107513428 402 | Z: -0.7543702125549316 403 | Focal Shape Fixed Size: true 404 | Focal Shape Size: 0.05000000074505806 405 | Invert Z Axis: false 406 | Name: Current View 407 | Near Clip Distance: 0.009999999776482582 408 | Pitch: 0.785398006439209 409 | Target Frame: 410 | Value: Orbit (rviz) 411 | Yaw: 0.785398006439209 412 | Saved: ~ 413 | Window Geometry: 414 | Displays: 415 | collapsed: false 416 | Head Depth: 417 | collapsed: false 418 | Head RGB: 419 | collapsed: false 420 | Height: 699 421 | Hide Left Dock: false 422 | Hide Right Dock: false 423 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000021dfc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000021d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001200480065006100640020004c00650066007400000001f7000000980000000000000000fb00000014004800650061006400200052006900670068007400000001db0000008e0000000000000000fb000000200048006500610064002000530074006500720065006f0020004c006500660074000000026d000000c00000000000000000fb000000220048006500610064002000530074006500720065006f002000520069006700680074000000026d000000c00000000000000000000000010000013c0000021dfc020000000afb000000140048006500610064002000440065007000740068010000003d000001110000002800fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000800480061006e0064010000003d000001790000000000000000fb00000010004800650061006400200052004700420100000154000001060000002800fffffffb0000000800480061006e0064010000003d0000014e0000000000000000fb0000001400480065006100640020004400650070007400680100000119000001060000000000000000fb000000160048006500610064002000430065006e0074006500720100000209000000ee0000000000000000fb000000160048006500610064002000430065006e00740065007201000001a30000018a0000000000000000fb0000000a00560069006500770073000000010a00000225000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000670000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000047b0000003efc0100000002fb0000000800540069006d006501000000000000047b000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000001dd0000021d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 424 | Selection: 425 | collapsed: false 426 | Time: 427 | collapsed: false 428 | Tool Properties: 429 | collapsed: false 430 | Views: 431 | collapsed: false 432 | Width: 1147 433 | X: 653 434 | Y: 99 435 | -------------------------------------------------------------------------------- /examples/turtlebot3/src/grasping_auto.cpp: -------------------------------------------------------------------------------- 1 | #include "grasping_auto.hpp" 2 | 3 | std::atomic SIGVerseTb3GraspingAuto::need_redraw_window_{false}; 4 | 5 | void SIGVerseTb3GraspingAuto::yolo_detection_callback(const yolo_msgs::msg::DetectionArray::SharedPtr detection_array) 6 | { 7 | if (detection_array->detections.empty()) { return; } 8 | 9 | std::scoped_lock lock(yolo_mutex_); 10 | yolo_objects_ = *detection_array; // deep copy 11 | } 12 | 13 | void SIGVerseTb3GraspingAuto::joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr joint_state) 14 | { 15 | // Check Time Stamp 16 | // auto logger = node_->get_logger(); 17 | // const auto &st = joint_state->header.stamp; 18 | // RCLCPP_INFO(logger, "JointState stamp: %d.%09u (sec.nsec)", st.sec, st.nanosec); 19 | 20 | for(size_t i=0; iname.size(); i++) 21 | { 22 | if(joint_state->name[i]==JOINT1_NAME) { joint1_pos_ = joint_state->position[i]; continue; } 23 | if(joint_state->name[i]==JOINT2_NAME) { joint2_pos_ = joint_state->position[i]; continue; } 24 | if(joint_state->name[i]==JOINT3_NAME) { joint3_pos_ = joint_state->position[i]; continue; } 25 | if(joint_state->name[i]==JOINT4_NAME) { joint4_pos_ = joint_state->position[i]; continue; } 26 | if(joint_state->name[i]==GRIP_JOINT_NAME){ grip_joint_pos_ = joint_state->position[i]; continue; } 27 | } 28 | } 29 | 30 | 31 | void SIGVerseTb3GraspingAuto::move_base(const double linear_x, const double angular_z) 32 | { 33 | geometry_msgs::msg::Twist twist; 34 | 35 | twist.linear.x = linear_x; 36 | twist.linear.y = 0.0; 37 | twist.linear.z = 0.0; 38 | twist.angular.x = 0.0; 39 | twist.angular.y = 0.0; 40 | twist.angular.z = angular_z; 41 | 42 | pub_base_twist_->publish(twist); 43 | } 44 | 45 | 46 | void SIGVerseTb3GraspingAuto::move_arm(const std::string &name, const double position, const double current_pos) 47 | { 48 | std::vector names; 49 | names.push_back(name); 50 | 51 | std::vector positions; 52 | positions.push_back(position); 53 | 54 | int duration_sec = calc_trajectory_duration(position, current_pos); 55 | 56 | trajectory_msgs::msg::JointTrajectory joint_trajectory; 57 | trajectory_msgs::msg::JointTrajectoryPoint arm_joint_point; 58 | 59 | joint_trajectory.points.push_back(arm_joint_point); 60 | 61 | joint_trajectory.joint_names = names; 62 | joint_trajectory.points[0].positions = positions; 63 | joint_trajectory.points[0].time_from_start = rclcpp::Duration::from_seconds(duration_sec); 64 | 65 | pub_joint_trajectory_->publish(joint_trajectory); 66 | } 67 | 68 | 69 | void SIGVerseTb3GraspingAuto::move_gripper(const double position, const double current_pos) 70 | { 71 | std::vector joint_names {GRIP_JOINT_NAME, GRIP_JOINT_SUB_NAME}; 72 | 73 | std::vector positions; 74 | 75 | positions.push_back(position); // for grip_joint 76 | positions.push_back(position); // for grip_joint_sub 77 | 78 | int duration_sec = calc_trajectory_duration(position, current_pos); 79 | 80 | trajectory_msgs::msg::JointTrajectoryPoint point; 81 | point.positions = positions; 82 | point.time_from_start = rclcpp::Duration::from_seconds(duration_sec); 83 | 84 | trajectory_msgs::msg::JointTrajectory joint_trajectory; 85 | joint_trajectory.joint_names = joint_names; 86 | joint_trajectory.points.push_back(point); 87 | 88 | pub_joint_trajectory_->publish(joint_trajectory); 89 | } 90 | 91 | 92 | void SIGVerseTb3GraspingAuto::stop_joints(const int duration_sec) 93 | { 94 | std::vector names {JOINT1_NAME, JOINT2_NAME, JOINT3_NAME, JOINT4_NAME}; 95 | 96 | std::vector positions; 97 | positions.push_back(joint1_pos_); 98 | positions.push_back(joint2_pos_); 99 | positions.push_back(joint3_pos_); 100 | positions.push_back(joint4_pos_); 101 | 102 | trajectory_msgs::msg::JointTrajectory joint_trajectory; 103 | trajectory_msgs::msg::JointTrajectoryPoint arm_joint_point; 104 | 105 | joint_trajectory.points.push_back(arm_joint_point); 106 | 107 | joint_trajectory.joint_names = names; 108 | joint_trajectory.points[0].positions = positions; 109 | joint_trajectory.points[0].time_from_start = rclcpp::Duration::from_seconds(duration_sec); 110 | 111 | pub_joint_trajectory_->publish(joint_trajectory); 112 | } 113 | 114 | 115 | bool SIGVerseTb3GraspingAuto::find_grasping_target(geometry_msgs::msg::Point &target_pos, const std::string &target_name) 116 | { 117 | std::scoped_lock lock(yolo_mutex_); 118 | if (!yolo_objects_ || yolo_objects_->detections.empty()){ return false; } 119 | 120 | double nearest_distance = std::numeric_limits::infinity(); 121 | std::optional nearest_pos; 122 | std::string frame_id; 123 | 124 | for (const auto &detection : yolo_objects_->detections) 125 | { 126 | if (detection.class_name != target_name){ continue; } 127 | 128 | // NOTE: Assumes bbox3d.center.position is already expressed in target_frame (e.g., base_link). 129 | const auto &pos = detection.bbox3d.center.position; 130 | const double distance = std::sqrt(pos.x*pos.x + pos.y*pos.y + pos.z*pos.z); 131 | 132 | if (distance < nearest_distance) 133 | { 134 | nearest_distance = distance; 135 | nearest_pos = pos; 136 | frame_id = detection.bbox3d.frame_id; 137 | } 138 | } 139 | 140 | if (nearest_pos==std::nullopt){ return false; } 141 | 142 | target_pos = nearest_pos.value(); 143 | 144 | publish_debug_markers(frame_id, target_pos); 145 | 146 | return true; 147 | } 148 | 149 | 150 | bool SIGVerseTb3GraspingAuto::move_arm_toward_object(const std::string &target_name) 151 | { 152 | auto logger = node_->get_logger(); 153 | 154 | try 155 | { 156 | geometry_msgs::msg::Point target_pos; 157 | 158 | if(!find_grasping_target(target_pos, target_name)){ return false; } 159 | 160 | RCLCPP_INFO(logger, "Target Object=%s", target_name.c_str()); 161 | 162 | // Gripper Open 163 | move_gripper(GRIP_MIN, grip_joint_pos_); 164 | 165 | RCLCPP_INFO(logger, "MoveIt -START-"); 166 | 167 | moveit::planning_interface::MoveGroupInterface arm(node_, "arm"); 168 | arm.setPoseReferenceFrame("base_link"); 169 | arm.setEndEffectorLink("end_effector_link"); 170 | arm.setGoalPositionTolerance(0.01); //[m] 171 | arm.setGoalOrientationTolerance(0.05); //[rad] 172 | 173 | // puts(("target_pos.x= " + std::to_string(target_pos.x)).c_str()); 174 | // puts(("target_pos.y= " + std::to_string(target_pos.y)).c_str()); 175 | // puts(("target_pos.z= " + std::to_string(target_pos.z)).c_str()); 176 | 177 | arm.setPositionTarget(target_pos.x, target_pos.y, target_pos.z); 178 | 179 | moveit::planning_interface::MoveGroupInterface::Plan plan; 180 | moveit::core::MoveItErrorCode result = arm.plan(plan); 181 | 182 | if (result == moveit::core::MoveItErrorCode::SUCCESS) 183 | { 184 | RCLCPP_INFO(logger, "Plan succeeded, executing..."); 185 | arm.execute(plan); 186 | } 187 | else 188 | { 189 | RCLCPP_ERROR(logger, "Plan failed, cannot execute."); 190 | } 191 | 192 | RCLCPP_INFO(logger, "MoveIt -END-"); 193 | } 194 | catch (const std::exception& e) 195 | { 196 | RCLCPP_ERROR(logger, "move_arm_toward_object: Standard exception: %s",e.what()); 197 | } 198 | catch (...) 199 | { 200 | RCLCPP_ERROR(logger, "move_arm_toward_object: Unknown exception caught"); 201 | } 202 | 203 | return true; 204 | } 205 | 206 | 207 | void SIGVerseTb3GraspingAuto::initialize_posture() 208 | { 209 | // Rotate joint1 210 | move_arm(JOINT1_NAME, 0.0, joint1_pos_); 211 | 212 | // Rotate joint2, joint3, joint4 213 | move_arm(JOINT2_NAME, 0.0, joint2_pos_); 214 | move_arm(JOINT3_NAME, 0.0, joint3_pos_); 215 | move_arm(JOINT4_NAME, 0.0, joint4_pos_); 216 | } 217 | 218 | 219 | std::string SIGVerseTb3GraspingAuto::get_detected_objects_list() 220 | { 221 | std::scoped_lock lock(yolo_mutex_); 222 | if (!yolo_objects_ || yolo_objects_->detections.empty()){ return ""; } 223 | 224 | std::string detected_objects = ""; 225 | 226 | for (const auto &detection : yolo_objects_->detections) 227 | { 228 | detected_objects += "/" + detection.class_name; 229 | } 230 | 231 | return detected_objects; 232 | } 233 | 234 | 235 | void SIGVerseTb3GraspingAuto::publish_debug_markers(const std::string& frame_id, const geometry_msgs::msg::Point& target_pos) 236 | { 237 | visualization_msgs::msg::MarkerArray markerArray; 238 | const auto stamp = rclcpp::Time(0); 239 | 240 | visualization_msgs::msg::Marker target; 241 | target.header.frame_id = frame_id; 242 | target.header.stamp = stamp; 243 | target.ns = "target"; 244 | target.id = 0; // Keep constant to overwrite 245 | target.type = visualization_msgs::msg::Marker::SPHERE; 246 | target.action = visualization_msgs::msg::Marker::ADD; 247 | target.scale = []{ geometry_msgs::msg::Vector3 v; v.x=0.1; v.y=0.1; v.z=0.1; return v; }(); 248 | target.color = []{ std_msgs::msg::ColorRGBA c; c.r=1.0f; c.g=0.0f; c.b=0.0f; c.a=1.0f; return c; }(); // red 249 | target.pose.orientation.w = 1.0; 250 | target.pose.position = target_pos; 251 | markerArray.markers.push_back(target); 252 | 253 | pub_debug_markers_->publish(markerArray); 254 | } 255 | 256 | void SIGVerseTb3GraspingAuto::display_message_in_window(const std::string& text) 257 | { 258 | int rows, cols; 259 | getmaxyx(stdscr, rows, cols); 260 | if (rows <= WINDOW_HEADER_HEIGHT + 6) { return; } 261 | resize_window(); 262 | 263 | mvprintw(WINDOW_HEADER_HEIGHT + 2, 0, "%s", text.c_str()); 264 | move(WINDOW_HEADER_HEIGHT + 3, 0); 265 | 266 | refresh(); 267 | } 268 | 269 | void SIGVerseTb3GraspingAuto::update_window_layout() 270 | { 271 | int rows, cols; 272 | getmaxyx(stdscr, rows, cols); 273 | 274 | if (rows <= 2) { return; } // terminal unavailable 275 | if (rows <= header_height_ + 2) { header_height_ = rows - 1; } 276 | else { header_height_ = WINDOW_HEADER_HEIGHT + 2; } 277 | 278 | if (win_header_) 279 | { 280 | wresize(win_header_, header_height_, cols); 281 | mvwin(win_header_, 0, 0); 282 | werase(stdscr); 283 | } 284 | else 285 | { 286 | win_header_ = newwin(header_height_, cols, 0, 0); 287 | if (!win_header_) { return; } 288 | wrefresh(win_header_); 289 | move(header_height_, 0); 290 | } 291 | 292 | refresh(); 293 | 294 | std::printf("\033[%d;%dr", header_height_ + 1, rows); // limit scrolling to [header+1 .. rows] 295 | std::fflush(stdout); 296 | } 297 | 298 | void SIGVerseTb3GraspingAuto::init_window() 299 | { 300 | setlocale(LC_ALL, ""); 301 | initscr(); 302 | cbreak(); // Disable line buffering 303 | noecho(); // Don't echo input characters 304 | nodelay(stdscr, TRUE); // Make getch() non-blocking 305 | keypad(stdscr, TRUE); // Enable special keys (arrow keys, etc.) 306 | refresh(); 307 | update_window_layout(); 308 | } 309 | 310 | void SIGVerseTb3GraspingAuto::resize_window() 311 | { 312 | update_window_layout(); 313 | show_help(); 314 | } 315 | 316 | void SIGVerseTb3GraspingAuto::shutdown_window() 317 | { 318 | std::printf("\033[r"); 319 | std::fflush(stdout); 320 | 321 | if (win_header_) { delwin(win_header_); win_header_ = nullptr; } 322 | endwin(); 323 | } 324 | 325 | void SIGVerseTb3GraspingAuto::show_help() 326 | { 327 | if (!win_header_){ return; } 328 | wclear(win_header_); 329 | box(win_header_, 0, 0); 330 | mvwprintw(win_header_, 1, 2, "arrow keys : Move"); 331 | mvwprintw(win_header_, 2, 2, "Space: Stop"); 332 | mvwprintw(win_header_, 3, 2, "w/s/d/a: Forward/Backward/Turn Right/Turn Left"); 333 | mvwprintw(win_header_, 4, 2, " 1: Grasp %s", GRASPING_TARGET1_NAME.c_str()); 334 | mvwprintw(win_header_, 5, 2, " 2: Grasp %s", GRASPING_TARGET2_NAME.c_str()); 335 | mvwprintw(win_header_, 6, 2, " 3: Grasp %s", GRASPING_TARGET3_NAME.c_str()); 336 | mvwprintw(win_header_, 7, 2, " 4: Grasp %s", GRASPING_TARGET4_NAME.c_str()); 337 | mvwprintw(win_header_, 8, 2, " 5: Grasp %s", GRASPING_TARGET5_NAME.c_str()); 338 | mvwprintw(win_header_, 9, 2, "o: Hand Open"); 339 | mvwprintw(win_header_,10, 2, "l: Show Detected objects list"); 340 | wrefresh(win_header_); 341 | move(header_height_, 0); 342 | refresh(); 343 | } 344 | 345 | void SIGVerseTb3GraspingAuto::key_loop(int argc, char** argv) 346 | { 347 | try 348 | { 349 | init_window(); 350 | show_help(); 351 | 352 | rclcpp::init(argc, argv); 353 | 354 | node_ = rclcpp::Node::make_shared("tb3_omc_grasping_auto"); 355 | 356 | signal(SIGWINCH, [](int){ need_redraw_window_.store(true, std::memory_order_relaxed); }); 357 | 358 | auto logger = node_->get_logger(); 359 | 360 | rclcpp::Rate loop_rate(10); 361 | 362 | pub_base_twist_ = node_->create_publisher ("/tb3/cmd_vel", 10); 363 | pub_joint_trajectory_ = node_->create_publisher("/tb3/joint_trajectory", 10); 364 | pub_debug_markers_ = node_->create_publisher ("/tb3/debug_markers", 10); 365 | 366 | auto sub_joint_state = node_->create_subscription ("/tb3/joint_state", 10, std::bind(&SIGVerseTb3GraspingAuto::joint_state_callback, this, std::placeholders::_1)); 367 | auto sub_yolo_detections = node_->create_subscription ("/yolo_objects/detections_3d", 10, std::bind(&SIGVerseTb3GraspingAuto::yolo_detection_callback, this, std::placeholders::_1)); 368 | 369 | sleep(2); 370 | 371 | bool is_grasping = false; 372 | GraspingStage stage = GraspingStage::MoveArm; 373 | time_point latest_stage_time; 374 | 375 | while (rclcpp::ok()) 376 | { 377 | if (need_redraw_window_.exchange(false)) { resize_window(); } 378 | 379 | if(!is_grasping) 380 | { 381 | int c = getch(); 382 | if(c != ERR) // Key was pressed 383 | { 384 | // Check for multi-byte characters (e.g., full-width input), But allow ncurses special keys. 385 | if(c > 127 && (c < KEY_MIN || c > KEY_MAX)) 386 | { 387 | display_message_in_window("Please use half-width input mode!"); 388 | continue; 389 | } 390 | 391 | switch(c) 392 | { 393 | case ' ': 394 | { 395 | RCLCPP_DEBUG(logger, "Stop"); 396 | move_base(0.0, 0.0); 397 | stop_joints(1); 398 | break; 399 | } 400 | case 'w': 401 | case KEY_UP: 402 | { 403 | RCLCPP_DEBUG(logger, "Go Forward"); 404 | move_base(+LINEAR_VEL, 0.0); 405 | break; 406 | } 407 | case 's': 408 | case KEY_DOWN: 409 | { 410 | RCLCPP_DEBUG(logger, "Go Back"); 411 | move_base(-LINEAR_VEL, 0.0); 412 | break; 413 | } 414 | case 'd': 415 | case KEY_RIGHT: 416 | { 417 | RCLCPP_DEBUG(logger, "Turn Right"); 418 | move_base(0.0, -ANGULAR_VEL); 419 | break; 420 | } 421 | case 'a': 422 | case KEY_LEFT: 423 | { 424 | RCLCPP_DEBUG(logger, "Turn Left"); 425 | move_base(0.0, +ANGULAR_VEL); 426 | break; 427 | } 428 | case '1': { is_grasping = move_arm_toward_object(GRASPING_TARGET1_NAME); break; } 429 | case '2': { is_grasping = move_arm_toward_object(GRASPING_TARGET2_NAME); break; } 430 | case '3': { is_grasping = move_arm_toward_object(GRASPING_TARGET3_NAME); break; } 431 | case '4': { is_grasping = move_arm_toward_object(GRASPING_TARGET4_NAME); break; } 432 | case '5': { is_grasping = move_arm_toward_object(GRASPING_TARGET5_NAME); break; } 433 | case 'o': 434 | { 435 | RCLCPP_DEBUG(logger, "Hand Open"); 436 | move_gripper(GRIP_MIN, grip_joint_pos_); 437 | break; 438 | } 439 | case 'l': 440 | { 441 | RCLCPP_DEBUG(logger, "Show Detected objects list"); 442 | display_message_in_window("Detected objects: " + get_detected_objects_list()); 443 | break; 444 | } 445 | } 446 | } 447 | } 448 | else 449 | { 450 | switch(stage) 451 | { 452 | case MoveArm: 453 | { 454 | go_next(latest_stage_time, stage); 455 | break; 456 | } 457 | case WaitMoveArm: 458 | { 459 | if(is_waiting(latest_stage_time, 3000)){ break; } 460 | 461 | go_next(latest_stage_time, stage); 462 | break; 463 | } 464 | case Grasp: 465 | { 466 | RCLCPP_DEBUG(logger, "Hand Close"); 467 | move_gripper(GRIP_MAX, grip_joint_pos_); 468 | 469 | go_next(latest_stage_time, stage); 470 | break; 471 | } 472 | case WaitGrasp: 473 | { 474 | if(is_waiting(latest_stage_time, 2000)){ break; } 475 | 476 | go_next(latest_stage_time, stage); 477 | break; 478 | } 479 | case UpArm: 480 | { 481 | RCLCPP_DEBUG(logger, "Up Arm"); 482 | initialize_posture(); 483 | 484 | go_next(latest_stage_time, stage); 485 | break; 486 | } 487 | case WaitUpArm: 488 | { 489 | if(is_waiting(latest_stage_time, 3000)){ break; } 490 | 491 | stage = GraspingStage::MoveArm; 492 | is_grasping = false; 493 | break; 494 | } 495 | } 496 | } 497 | 498 | rclcpp::spin_some(node_); 499 | 500 | // puts("rclcpp::spin rclcpp::spin rclcpp::spin rclcpp::spin rclcpp::spin"); 501 | 502 | loop_rate.sleep(); 503 | } 504 | } 505 | catch(...) 506 | { 507 | puts("An exception occurred!"); 508 | } 509 | 510 | shutdown_window(); 511 | 512 | return; 513 | } 514 | 515 | 516 | int main(int argc, char** argv) 517 | { 518 | SIGVerseTb3GraspingAuto grasping_auto; 519 | 520 | grasping_auto.key_loop(argc, argv); 521 | 522 | return(EXIT_SUCCESS); 523 | } 524 | 525 | -------------------------------------------------------------------------------- /sigverse_ros_bridge/src/sigverse_ros_bridge.cpp: -------------------------------------------------------------------------------- 1 | #include "sigverse_ros_bridge.hpp" 2 | 3 | int SIGVerseROSBridge::syncTimeCnt; 4 | int SIGVerseROSBridge::syncTimeMaxNum; 5 | 6 | pid_t SIGVerseROSBridge::get_tid(void) 7 | { 8 | return syscall(SYS_gettid); 9 | } 10 | 11 | bool SIGVerseROSBridge::check_receivable( int fd ) 12 | { 13 | fd_set fdset; 14 | int ret; 15 | struct timeval timeout; 16 | FD_ZERO( &fdset ); 17 | FD_SET( fd , &fdset ); 18 | 19 | // timeout is 1 sec 20 | timeout.tv_sec = 1; 21 | timeout.tv_usec = 0; 22 | 23 | ret = select( fd+1 , &fdset , NULL , NULL , &timeout ); 24 | 25 | return ret == 1; 26 | } 27 | 28 | 29 | void SIGVerseROSBridge::set_vector_double(std::vector &destVec, const bsoncxx::array::view &arrayView) 30 | { 31 | int i = 0; 32 | 33 | for(auto itr = arrayView.cbegin(); itr != arrayView.cend(); ++itr) 34 | { 35 | destVec[i++] = (*itr).get_double(); 36 | } 37 | } 38 | 39 | void SIGVerseROSBridge::set_vector_float(std::vector &destVec, const bsoncxx::array::view &arrayView) 40 | { 41 | int i = 0; 42 | 43 | for(auto itr = arrayView.cbegin(); itr != arrayView.cend(); ++itr) 44 | { 45 | destVec[i++] = (float)((*itr).get_double()); 46 | } 47 | } 48 | 49 | template 50 | void SIGVerseROSBridge::set_array_double(std::array &destArray, const bsoncxx::array::view &arrayView) { 51 | int i = 0; 52 | for (auto itr = arrayView.cbegin(); itr != arrayView.cend(); ++itr) { 53 | destArray[i++] = (*itr).get_double(); 54 | } 55 | } 56 | 57 | template 58 | void SIGVerseROSBridge::set_array_double(boost::array &destArray, const bsoncxx::array::view &arrayView) { 59 | int i = 0; 60 | for (auto itr = arrayView.cbegin(); itr != arrayView.cend(); ++itr) { 61 | destArray[i++] = (*itr).get_double(); 62 | } 63 | } 64 | 65 | void * SIGVerseROSBridge::receiving_thread(void *param) 66 | { 67 | int dstSocket = *((int *)param); 68 | 69 | char *buf; 70 | buf = new char [BUFFER_SIZE]; 71 | 72 | if(buf == NULL) 73 | { 74 | std::cout << "Cannot malloc!" << std::endl; 75 | exit(EXIT_FAILURE); 76 | } 77 | 78 | long int totalReceivedSize; 79 | 80 | std::map::SharedPtr> twistPublisherMap; 81 | std::map::SharedPtr> cameraInfoPublisherMap; 82 | std::map::SharedPtr> imagePublisherMap; 83 | std::map::SharedPtr> laserScanPublisherMap; 84 | 85 | std::cout << "Socket open. tid=" << get_tid() << std::endl; 86 | 87 | auto node = rclcpp::Node::make_shared("sigverse_ros_bridge_" + std::to_string(get_tid())); 88 | 89 | rclcpp::Rate loop_rate(100); 90 | 91 | while(rclcpp::ok()) 92 | { 93 | // Get total BSON data size 94 | totalReceivedSize = 0; 95 | 96 | if(!check_receivable(dstSocket)){ continue; } 97 | 98 | char bufHeader[4]; 99 | 100 | long int numRcv = read(dstSocket, bufHeader, sizeof(4)); 101 | 102 | if(numRcv == 0) 103 | { 104 | close(dstSocket); 105 | std::cout << "Socket closed. tid=" << get_tid() << std::endl; 106 | break; 107 | } 108 | if(numRcv == -1) 109 | { 110 | close(dstSocket); 111 | std::cout << "Socket error. tid=" << get_tid() << std::endl; 112 | break; 113 | } 114 | if(numRcv < 4) 115 | { 116 | close(dstSocket); 117 | std::cout << "Can not get data size... tid=" << get_tid() << std::endl; 118 | break; 119 | } 120 | totalReceivedSize += 4; 121 | 122 | int32_t msgSize; 123 | memcpy(&msgSize, &bufHeader, sizeof(int32_t)); 124 | memcpy(&buf[0] , &bufHeader, sizeof(int32_t)); 125 | 126 | if(msgSize > BUFFER_SIZE) 127 | { 128 | close(dstSocket); 129 | std::cout << "Data size is too big. tid=" << get_tid() << std::endl; 130 | break; 131 | } 132 | 133 | // std::cout << "msg size=" << msgSize << std::endl; 134 | 135 | // Get BSON data 136 | while(msgSize!=totalReceivedSize) 137 | { 138 | size_t unreceivedSize = msgSize - (size_t)totalReceivedSize; 139 | 140 | if(!check_receivable(dstSocket)){ break; } 141 | 142 | long int receivedSize = read(dstSocket, &(buf[totalReceivedSize]), unreceivedSize); 143 | 144 | totalReceivedSize += receivedSize; 145 | } 146 | 147 | if(msgSize!=totalReceivedSize) 148 | { 149 | std::cout << "msgSize!=totalReceivedSize ?????? tid=" << get_tid() << std::endl; 150 | continue; 151 | }; 152 | 153 | bsoncxx::document::view bsonView((const uint8_t*)buf, (std::size_t)msgSize); 154 | 155 | bsoncxx::builder::core bsonCore(false); 156 | 157 | bsonCore.concatenate(bsonView); 158 | 159 | bsoncxx::builder::basic::sub_document bsonSubdocument(&bsonCore); 160 | 161 | std::string opValue = std::string(bsonView["op"] .get_string().value); 162 | std::string topicValue = std::string(bsonView["topic"].get_string().value); 163 | std::string typeValue = std::string(bsonView["type"] .get_string().value); 164 | // std::cout << "op:" << opValue << std::endl; 165 | // std::cout << "tp:" << topicValue << std::endl; 166 | // std::cout << "tv:" << typeValue << std::endl; 167 | 168 | // Create Publisher 169 | if(typeValue!=TYPE_TIME_SYNC && typeValue!=TYPE_TF_LIST) 170 | { 171 | if(typeValue==TYPE_TWIST) 172 | { 173 | if(twistPublisherMap.count(topicValue)==0) 174 | { 175 | auto publisher = node->create_publisher(topicValue, 1000); 176 | twistPublisherMap[topicValue] = publisher; 177 | std::cout << "Create Publisher: " << topicValue << std::endl; 178 | } 179 | } 180 | else if(typeValue==TYPE_CAMERA_INFO) 181 | { 182 | if(cameraInfoPublisherMap.count(topicValue)==0) 183 | { 184 | auto publisher = node->create_publisher(topicValue, 10); 185 | cameraInfoPublisherMap[topicValue] = publisher; 186 | std::cout << "Create Publisher: " << topicValue << std::endl; 187 | } 188 | } 189 | else if(typeValue==TYPE_IMAGE) 190 | { 191 | if(imagePublisherMap.count(topicValue)==0) 192 | { 193 | auto publisher = node->create_publisher(topicValue, 10); 194 | imagePublisherMap[topicValue] = publisher; 195 | std::cout << "Create Publisher: " << topicValue << std::endl; 196 | } 197 | } 198 | else if(typeValue==TYPE_LASER_SCAN) 199 | { 200 | if(laserScanPublisherMap.count(topicValue)==0) 201 | { 202 | auto publisher = node->create_publisher(topicValue, 10); 203 | laserScanPublisherMap[topicValue] = publisher; 204 | std::cout << "Create Publisher: " << topicValue << std::endl; 205 | } 206 | } 207 | else 208 | { 209 | std::cout << "Not compatible message type! :" << typeValue << std::endl; 210 | continue; 211 | } 212 | } 213 | 214 | // Publish 215 | // Twist 216 | if(typeValue==TYPE_TWIST) 217 | { 218 | geometry_msgs::msg::Twist twist; 219 | 220 | twist.linear.x = bsonView["msg"]["linear"]["x"].get_double(); 221 | twist.linear.y = bsonView["msg"]["linear"]["y"].get_double(); 222 | twist.linear.z = bsonView["msg"]["linear"]["z"].get_double(); 223 | 224 | twist.angular.x = bsonView["msg"]["angular"]["x"].get_double(); 225 | twist.angular.y = bsonView["msg"]["angular"]["y"].get_double(); 226 | twist.angular.z = bsonView["msg"]["angular"]["z"].get_double(); 227 | 228 | twistPublisherMap[topicValue]->publish(twist); 229 | } 230 | // CameraInfo 231 | else if(typeValue==TYPE_CAMERA_INFO) 232 | { 233 | sensor_msgs::msg::CameraInfo cameraInfo; 234 | 235 | int32_t sec = bsonView["msg"]["header"]["stamp"]["sec"] .get_int32(); 236 | int32_t nsec = bsonView["msg"]["header"]["stamp"]["nanosec"].get_int32(); // get_uint32() is not available; use get_int32() and cast if safe. 237 | cameraInfo.header.stamp = rclcpp::Time(static_cast(sec) * 1000000000ULL + static_cast(nsec)); 238 | cameraInfo.header.frame_id = std::string(bsonView["msg"]["header"]["frame_id"].get_string().value); 239 | 240 | cameraInfo.height = (uint32_t) bsonView["msg"]["height"].get_int32(); 241 | cameraInfo.width = (uint32_t) bsonView["msg"]["width"] .get_int32(); 242 | cameraInfo.distortion_model = std::string(bsonView["msg"]["distortion_model"].get_string().value); 243 | 244 | bsoncxx::array::view dView = bsonView["msg"]["d"].get_array().value; 245 | cameraInfo.d.resize((size_t)std::distance(dView.cbegin(), dView.cend())); 246 | set_vector_double(cameraInfo.d, dView); 247 | 248 | set_array_double(cameraInfo.k, bsonView["msg"]["k"].get_array().value); 249 | set_array_double(cameraInfo.r, bsonView["msg"]["r"].get_array().value); 250 | set_array_double(cameraInfo.p, bsonView["msg"]["p"].get_array().value); 251 | 252 | cameraInfo.binning_x = (uint32_t)bsonView["msg"]["binning_x"].get_int32(); 253 | cameraInfo.binning_y = (uint32_t)bsonView["msg"]["binning_y"].get_int32(); 254 | cameraInfo.roi.x_offset = (uint32_t)bsonView["msg"]["roi"]["x_offset"] .get_int32(); 255 | cameraInfo.roi.y_offset = (uint32_t)bsonView["msg"]["roi"]["y_offset"] .get_int32(); 256 | cameraInfo.roi.height = (uint32_t)bsonView["msg"]["roi"]["height"] .get_int32(); 257 | cameraInfo.roi.width = (uint32_t)bsonView["msg"]["roi"]["width"] .get_int32(); 258 | cameraInfo.roi.do_rectify = (uint8_t) bsonView["msg"]["roi"]["do_rectify"].get_bool(); 259 | 260 | cameraInfoPublisherMap[topicValue]->publish(cameraInfo); 261 | } 262 | // Image 263 | else if(typeValue==TYPE_IMAGE) 264 | { 265 | sensor_msgs::msg::Image image; 266 | 267 | int32_t sec = bsonView["msg"]["header"]["stamp"]["sec"] .get_int32(); 268 | int32_t nsec = bsonView["msg"]["header"]["stamp"]["nanosec"].get_int32(); // get_uint32() is not available; use get_int32() and cast if safe. 269 | image.header.stamp = rclcpp::Time(static_cast(sec) * 1000000000ULL + static_cast(nsec)); 270 | image.header.frame_id = std::string(bsonView["msg"]["header"]["frame_id"].get_string().value); 271 | image.height = (uint32_t) bsonView["msg"]["height"] .get_int32(); 272 | image.width = (uint32_t) bsonView["msg"]["width"] .get_int32(); 273 | image.encoding = std::string(bsonView["msg"]["encoding"] .get_string().value); 274 | image.is_bigendian = (uint8_t) bsonView["msg"]["is_bigendian"].get_int32(); //.raw()[0]; 275 | image.step = (uint32_t) bsonView["msg"]["step"] .get_int32(); 276 | 277 | size_t sizet = (image.step * image.height); 278 | image.data.resize(sizet); 279 | memcpy(&image.data[0], bsonView["msg"]["data"].get_binary().bytes, sizet); 280 | 281 | imagePublisherMap[topicValue]->publish(image); 282 | } 283 | // LaserScan 284 | else if(typeValue==TYPE_LASER_SCAN) 285 | { 286 | sensor_msgs::msg::LaserScan laserScan; 287 | 288 | int32_t sec = bsonView["msg"]["header"]["stamp"]["sec"] .get_int32(); 289 | int32_t nsec = bsonView["msg"]["header"]["stamp"]["nanosec"].get_int32(); // get_uint32() is not available; use get_int32() and cast if safe. 290 | laserScan.header.stamp = rclcpp::Time(static_cast(sec) * 1000000000ULL + static_cast(nsec)); 291 | laserScan.header.frame_id = std::string(bsonView["msg"]["header"]["frame_id"].get_string().value); 292 | 293 | laserScan.angle_min = (float)bsonView["msg"]["angle_min"] .get_double(); 294 | laserScan.angle_max = (float)bsonView["msg"]["angle_max"] .get_double(); 295 | laserScan.angle_increment = (float)bsonView["msg"]["angle_increment"].get_double(); 296 | laserScan.time_increment = (float)bsonView["msg"]["time_increment"] .get_double(); 297 | laserScan.scan_time = (float)bsonView["msg"]["scan_time"] .get_double(); 298 | laserScan.range_min = (float)bsonView["msg"]["range_min"] .get_double(); 299 | laserScan.range_max = (float)bsonView["msg"]["range_max"] .get_double(); 300 | 301 | size_t sizet = (size_t)((laserScan.angle_max - laserScan.angle_min) / laserScan.angle_increment + 1); 302 | 303 | laserScan.ranges.resize(sizet); 304 | laserScan.intensities.resize(sizet); 305 | 306 | bsoncxx::array::view dView_ranges = bsonView["msg"]["ranges"].get_array().value; 307 | laserScan.ranges.resize(std::distance(dView_ranges.cbegin(), dView_ranges.cend())); 308 | set_vector_float(laserScan.ranges, dView_ranges); 309 | 310 | bsoncxx::array::view dView_intensities = bsonView["msg"]["intensities"].get_array().value; 311 | laserScan.intensities.resize(std::distance(dView_intensities.cbegin(), dView_intensities.cend())); 312 | set_vector_float(laserScan.intensities, dView_intensities); 313 | 314 | laserScanPublisherMap[topicValue]->publish(laserScan); 315 | } 316 | // Time Synchronization (SIGVerse Original Type) 317 | else if(typeValue==TYPE_TIME_SYNC) 318 | { 319 | if(syncTimeCnt < syncTimeMaxNum) 320 | { 321 | int32_t sec = bsonView["msg"]["sec"] .get_int32(); 322 | int32_t nsec = bsonView["msg"]["nanosec"].get_int32(); // get_uint32() is not available; use get_int32() and cast if safe. 323 | 324 | rclcpp::Clock clock; 325 | double now_sec = clock.now().seconds(); 326 | 327 | int gapSec = sec - static_cast(now_sec); 328 | int gapMsec = static_cast((nsec / 1e6) - ((now_sec - static_cast(now_sec)) * 1e3)); 329 | 330 | std::string timeGap = "time_gap," + std::to_string(gapSec) + "," + std::to_string(gapMsec); 331 | 332 | // ssize_t size = 333 | write(dstSocket, timeGap.c_str(), std::strlen(timeGap.c_str())); 334 | 335 | std::cout << "TYPE_TIME_SYNC " << timeGap.c_str() << std::endl; 336 | 337 | syncTimeCnt++; 338 | } 339 | } 340 | // Tf list data (SIGVerse Original Type) 341 | else if(typeValue==TYPE_TF_LIST) 342 | { 343 | static std::shared_ptr transformBroadcaster; 344 | if (!transformBroadcaster) 345 | transformBroadcaster = std::make_shared(node); 346 | 347 | bsoncxx::array::view tfArrayView = bsonView["msg"].get_array().value; 348 | 349 | std::vector stampedTransformList; 350 | 351 | for(auto itr = tfArrayView.cbegin(); itr != tfArrayView.cend(); ++itr) 352 | { 353 | std::string frameId = std::string((*itr)["header"]["frame_id"].get_string().value); 354 | int32_t sec = (*itr)["header"]["stamp"]["sec"] .get_int32(); 355 | int32_t nsec = (*itr)["header"]["stamp"]["nanosec"].get_int32(); // get_uint32() is not available; use get_int32() and cast if safe. 356 | std::string childFrameId = std::string((*itr)["child_frame_id"] .get_string().value); 357 | 358 | geometry_msgs::msg::TransformStamped stampedTransform; 359 | 360 | if (sec == 0) { 361 | stampedTransform.header.stamp = node->get_clock()->now(); 362 | } else { 363 | stampedTransform.header.stamp = rclcpp::Time(static_cast(sec) * 1000000000ULL + static_cast(nsec)); 364 | } 365 | stampedTransform.header.frame_id = frameId; 366 | stampedTransform.child_frame_id = childFrameId; 367 | 368 | stampedTransform.transform.translation.x = (*itr)["transform"]["translation"]["x"].get_double(); 369 | stampedTransform.transform.translation.y = (*itr)["transform"]["translation"]["y"].get_double(); 370 | stampedTransform.transform.translation.z = (*itr)["transform"]["translation"]["z"].get_double(); 371 | 372 | stampedTransform.transform.rotation.x = (*itr)["transform"]["rotation"]["x"].get_double(); 373 | stampedTransform.transform.rotation.y = (*itr)["transform"]["rotation"]["y"].get_double(); 374 | stampedTransform.transform.rotation.z = (*itr)["transform"]["rotation"]["z"].get_double(); 375 | stampedTransform.transform.rotation.w = (*itr)["transform"]["rotation"]["w"].get_double(); 376 | 377 | stampedTransformList.push_back(stampedTransform); 378 | } 379 | 380 | transformBroadcaster->sendTransform(stampedTransformList); 381 | } 382 | 383 | // std::cout << "published. topic=" << topicValue << std::endl; 384 | 385 | rclcpp::spin_some(node); 386 | } 387 | 388 | delete[] buf; 389 | 390 | return NULL; 391 | } 392 | 393 | 394 | int SIGVerseROSBridge::run(int argc, char **argv) 395 | { 396 | uint16_t portNumber; 397 | 398 | portNumber = DEFAULT_PORT; 399 | syncTimeMaxNum = DEFAULT_SYNC_TIME_MAX_NUM; 400 | 401 | for (int i = 1; i < argc; ++i) 402 | { 403 | std::string arg = argv[i]; 404 | if (arg == "--ros-args") { break; } 405 | 406 | if(i==1) 407 | { 408 | portNumber = (uint16_t)std::atoi(argv[1]); 409 | std::cout << "argv[1] portNumber=" << argv[1] << std::endl; 410 | } 411 | if(i==2) 412 | { 413 | syncTimeMaxNum = (uint16_t)std::atoi(argv[2]); 414 | std::cout << "argv[2] syncTimeMaxNum=" << argv[2] << std::endl; 415 | } 416 | } 417 | 418 | syncTimeCnt = 0; 419 | 420 | int srcSocket; 421 | struct sockaddr_in srcAddr; 422 | 423 | // TCP connection setting 424 | // bzero((char *)&srcAddr, sizeof(srcAddr)); 425 | memset(&srcAddr, 0, sizeof(srcAddr)); 426 | srcAddr.sin_port = htons(portNumber); 427 | srcAddr.sin_family = AF_INET; 428 | srcAddr.sin_addr.s_addr = INADDR_ANY; 429 | 430 | srcSocket = socket(AF_INET, SOCK_STREAM, 0); 431 | 432 | int opt = 1; 433 | setsockopt(srcSocket, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); 434 | 435 | bind(srcSocket, (struct sockaddr *)&srcAddr, sizeof(srcAddr)); 436 | 437 | listen(srcSocket, 100); 438 | 439 | std::cout << "Waiting for connection... port=" << portNumber << std::endl; 440 | 441 | while(rclcpp::ok()) 442 | { 443 | int dstSocket; 444 | 445 | struct sockaddr_in dstAddr; 446 | int dstAddrSize = sizeof(dstAddr); 447 | 448 | if(!check_receivable(srcSocket)) 449 | { 450 | continue; 451 | } 452 | 453 | dstSocket = accept(srcSocket, (struct sockaddr *)&dstAddr, (socklen_t *)&dstAddrSize); 454 | 455 | std::cout << "Connected from IP=" << inet_ntoa(dstAddr.sin_addr) << " Port=" << dstAddr.sin_port << std::endl; 456 | 457 | pthread_t thread; 458 | pthread_create( &thread, NULL, receiving_thread, (void *)(&dstSocket)); 459 | pthread_detach(thread); 460 | 461 | usleep(10 * 1000); 462 | } 463 | 464 | close(srcSocket); 465 | 466 | return 0; 467 | } 468 | 469 | 470 | int main(int argc, char **argv) 471 | { 472 | std::cout << "pid=" << getpid() << std::endl; 473 | 474 | rclcpp::init(argc, argv); 475 | 476 | SIGVerseROSBridge sigverseROSBridge; 477 | sigverseROSBridge.run(argc, argv); 478 | 479 | rclcpp::shutdown(); 480 | }; 481 | 482 | --------------------------------------------------------------------------------