diff --git a/install.sh b/install.sh index e319524..bab59ae 100755 --- a/install.sh +++ b/install.sh @@ -9,17 +9,15 @@ cd .. # Dowload required packages for SOBIT EDU ros_packages=( - "sobits_common" \ "sobits_msgs" \ - "urg_node" \ - "azure_kinect_ros_driver" \ - "turtlebot2_on_noetic" + "dynamixel_hardware" \ + "realsense_ros" ) -# Clone all packages +#Clone all packages for ((i = 0; i < ${#ros_packages[@]}; i++)) { echo "Clonning: ${ros_packages[i]}" - git clone https://github.com/TeamSOBITS/${ros_packages[i]}.git + git clone -b $ROS_DISTRO-devel https://github.com/TeamSOBITS/${ros_packages[i]}.git # Check if install.sh exists in each package if [ -f ${ros_packages[i]}/install.sh ]; then @@ -32,12 +30,23 @@ for ((i = 0; i < ${#ros_packages[@]}; i++)) { # Setup Turtlebot2 (Kobuki) for ROS Noetic cd ${DIR} -bash ../turtlebot2_on_noetic/turtlebot/setup_kobuki.sh +# bash ../turtlebot2_on_noetic/turtlebot/setup_kobuki.sh + +# Download required dependencies +python3 -m pip install \ + transforms3d + # Download ROS packages sudo apt-get update sudo apt-get install -y \ - ros-$ROS_DISTRO-pybind11-catkin \ + ros-$ROS_DISTRO-ecl-linear-algebra \ + ros-$ROS_DISTRO-kobuki-ros-interfaces \ + ros-$ROS_DISTRO-kobuki-core \ + ros-$ROS_DISTRO-laser-proc \ + ros-$ROS_DISTRO-urg-c \ + ros-$ROS_DISTRO-urg-node \ + ros-$ROS_DISTRO-urg-node-msgs \ ros-$ROS_DISTRO-robot-state-publisher \ ros-$ROS_DISTRO-joint-state-controller \ ros-$ROS_DISTRO-joint-state-publisher \ @@ -53,8 +62,47 @@ sudo apt-get install -y \ ros-$ROS_DISTRO-trajectory-msgs \ ros-$ROS_DISTRO-geometry-msgs \ ros-$ROS_DISTRO-joy + + # refer to sobit_light + ros-$ROS_DISTRO-ros2-control \ + ros-$ROS_DISTRO-ros2-controllers \ + ros-$ROS_DISTRO-control-toolbox \ + ros-$ROS_DISTRO-controller-interface \ + ros-$ROS_DISTRO-controller-manager \ + ros-$ROS_DISTRO-position-controllers \ + ros-$ROS_DISTRO-velocity-controllers \ + ros-$ROS_DISTRO-effort-controllers \ + ros-$ROS_DISTRO-joint-trajectory-controller \ + ros-$ROS_DISTRO-joint-group-impedance-controller \ + ros-$ROS_DISTRO-joint-state-publisher \ + ros-$ROS_DISTRO-joint-state-publisher-gui \ + ros-$ROS_DISTRO-joint-state-broadcaster \ + ros-$ROS_DISTRO-joint-limits \ + ros-$ROS_DISTRO-robot-controllers \ + ros-$ROS_DISTRO-robot-controllers-interface \ + ros-$ROS_DISTRO-robot-state-publisher \ + ros-$ROS_DISTRO-hardware-interface \ + ros-$ROS_DISTRO-transmission-interface \ + ros-$ROS_DISTRO-urdf \ + ros-$ROS_DISTRO-urdf-launch \ + ros-$ROS_DISTRO-xacro \ + ros-$ROS_DISTRO-tf-transformations + + + # Install Gazebo Fortress with binaries + sudo apt-get install -y \ + ros-${ROS_DISTRO}-ros-gz \ + ros-${ROS_DISTRO}-ign-ros2-control \ + ros-${ROS_DISTRO}-ign-ros2-control-demos + + echo "╚══╣ Setup: SOBIT EDU (FINISHED) ╠══╝" + + + +### ここから下は未確認。いるいらないがわからない。 +''' # Setting up Dynamixel USB configuration (SOBIT EDU: Head and Arm Robot Mechanism) echo "SUBSYSTEM==\"tty\", ATTRS{idVendor}==\"0403\", ATTRS{idProduct}==\"6015\", SYMLINK+=\"input/dx_upper\", MODE=\"0666\"" | sudo tee /etc/udev/rules.d/dx_upper.rules @@ -78,6 +126,10 @@ sudo udevadm control --reload-rules # Trigger the new rules sudo udevadm trigger +''' + +# Go back to previous directory +cd ${DIR} -echo "╚══╣ Setup: SOBIT EDU (FINISHED) ╠══╝" +echo "╚══╣ Setup: SOBIT LIGHT (FINISHED) ╠══╝" \ No newline at end of file diff --git a/sobit_edu/CMakeLists.txt b/sobit_edu/CMakeLists.txt deleted file mode 100644 index 4652c37..0000000 --- a/sobit_edu/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -cmake_minimum_required(VERSION 3.10.2) -project(sobit_edu) -find_package(catkin REQUIRED) -catkin_metapackage() \ No newline at end of file diff --git a/sobit_edu/docs/img/.gitkeep b/sobit_edu/docs/img/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/sobit_edu/docs/img/sobit_edu.png b/sobit_edu/docs/img/sobit_edu.png deleted file mode 100644 index 3e338bd..0000000 Binary files a/sobit_edu/docs/img/sobit_edu.png and /dev/null differ diff --git a/sobit_edu/docs/img/sobit_edu_display.png b/sobit_edu/docs/img/sobit_edu_display.png deleted file mode 100644 index a5a8727..0000000 Binary files a/sobit_edu/docs/img/sobit_edu_display.png and /dev/null differ diff --git a/sobit_edu/docs/img/sobit_edu_onshape.png b/sobit_edu/docs/img/sobit_edu_onshape.png deleted file mode 100644 index db011b5..0000000 Binary files a/sobit_edu/docs/img/sobit_edu_onshape.png and /dev/null differ diff --git a/sobit_edu/package.xml b/sobit_edu/package.xml deleted file mode 100644 index e60b259..0000000 --- a/sobit_edu/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - sobit_edu - 0.0.0 - - ROS packages for SOBIT EDU (meta package). - - Yuki Okuma - - BSD - - https://home.soka.ac.jp/~choi/index.html - https://github.com/TeamSOBITS/sobit_edu - https://github.com/TeamSOBITS/sobit_edu/issues - Team SOBITS - - catkin - - sobit_edu_bringup - sobit_edu_control - sobit_edu_description - sobit_edu_library - - - - \ No newline at end of file diff --git a/sobit_edu_bringup/CMakeLists.txt b/sobit_edu_bringup/CMakeLists.txt old mode 100755 new mode 100644 index 57d5db7..3e1215b --- a/sobit_edu_bringup/CMakeLists.txt +++ b/sobit_edu_bringup/CMakeLists.txt @@ -1,39 +1,32 @@ -cmake_minimum_required(VERSION 3.10.2) +cmake_minimum_required(VERSION 3.8) project(sobit_edu_bringup) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rospy - sensor_msgs - trajectory_msgs - geometry_msgs -) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES sobit_pro_bringup - CATKIN_DEPENDS rospy sensor_msgs trajectory_msgs geometry_msgs -# DEPENDS system_lib +install(DIRECTORY + launch + rviz + DESTINATION share/${PROJECT_NAME} ) -########### -## Build ## -########### +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) \ No newline at end of file +ament_package() diff --git a/sobit_edu_bringup/include/sobit_edu_bringup/.gitkeep b/sobit_edu_bringup/include/sobit_edu_bringup/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/sobit_edu_bringup/launch/azure_kinect.launch b/sobit_edu_bringup/launch/azure_kinect.launch deleted file mode 100644 index 452132b..0000000 --- a/sobit_edu_bringup/launch/azure_kinect.launch +++ /dev/null @@ -1,58 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sobit_edu_bringup/launch/gz_minimal.launch.py b/sobit_edu_bringup/launch/gz_minimal.launch.py new file mode 100755 index 0000000..37b3044 --- /dev/null +++ b/sobit_edu_bringup/launch/gz_minimal.launch.py @@ -0,0 +1,106 @@ +import os +from ament_index_python.packages import get_package_share_directory + +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + gz_bridge_node = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=[ + "/clock" + "@rosgraph_msgs/msg/Clock" + "[ignition.msgs.Clock", + "/tf" + "@tf2_msgs/msg/TFMessage" + "[ignition.msgs.TFMessage", + ], + output='screen' + ) + + rviz_config = PathJoinSubstitution([ + FindPackageShare('sobit_edu_bringup'), + 'rviz', + 'gazebo.rviz' + ]) + rviz_node = Node( + package='rviz2', + executable='rviz2', + output='screen', + arguments=['-d', rviz_config], + ) + + world_file = os.path.join(get_package_share_directory( + 'sobit_light_description'), + 'worlds', + 'empty_w_physics.sdf' + ) + + return LaunchDescription([ + # Launch gazebo environment + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ]), + launch_arguments={ + 'gz_args' : ' -r -v 4 ' + world_file, + }.items() + ), + gz_bridge_node, + # Launch Robot No. 1 + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('sobit_edu_bringup'), + 'launch', + 'gz_robot.launch.py' + ]) + ]), + launch_arguments={ + 'robot_name': 'sobit_edu', + 'robot_coords_x': '0', # x + 'robot_coords_y': '0', # y + 'robot_coords_Y': '0', # yaw + 'enable_gz_front_cam_color' : 'True', + 'enable_gz_back_cam_color' : 'True', + 'enable_gz_head_cam_color' : 'True', + 'enable_gz_head_cam_depth' : 'True', + 'enable_gz_hand_cam_color' : 'True', + 'enable_gz_hand_cam_depth' : 'True', + 'enable_gz_lidar' : 'True', + 'enable_gz_imu' : 'True', + }.items() + ), + # Launch Robot No. 2 + # IncludeLaunchDescription( + # PythonLaunchDescriptionSource([ + # PathJoinSubstitution([ + # FindPackageShare('sobit_light_bringup'), + # 'launch', + # 'gz_robot.launch.py' + # ]) + # ]), + # launch_arguments={ + # 'robot_name': 'sobit_light_2', + # 'robot_coords_x': '0', # x + # 'robot_coords_y': '2', # y + # 'robot_coords_Y': '0', # yaw + # 'enable_gz_front_cam_color' : 'True', + # 'enable_gz_back_cam_color' : 'True', + # 'enable_gz_head_cam_color' : 'True', + # 'enable_gz_head_cam_depth' : 'True', + # 'enable_gz_hand_cam_color' : 'True', + # 'enable_gz_hand_cam_depth' : 'True', + # 'enable_gz_lidar' : 'True', + # 'enable_gz_imu' : 'True', + # }.items() + # ), + rviz_node, + ]) diff --git a/sobit_edu_bringup/launch/gz_robot.launch.py b/sobit_edu_bringup/launch/gz_robot.launch.py new file mode 100755 index 0000000..82b362e --- /dev/null +++ b/sobit_edu_bringup/launch/gz_robot.launch.py @@ -0,0 +1,246 @@ +import os +from ament_index_python.packages import get_package_share_directory + +from launch_ros.actions import Node + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.substitutions import LaunchConfiguration + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + +import xacro + +def generate_launch_description(): + arg_robot_name = DeclareLaunchArgument('robot_name', default_value='sobit_edu') + arg_robot_coords_x = DeclareLaunchArgument('robot_coords_x', default_value='0') + arg_robot_coords_y = DeclareLaunchArgument('robot_coords_y', default_value='0') + arg_robot_coords_Y = DeclareLaunchArgument('robot_coords_Y', default_value='0') + arg_enable_gz_front_cam_color = DeclareLaunchArgument('enable_gz_front_cam_color', default_value='True') + arg_enable_gz_back_cam_color = DeclareLaunchArgument('enable_gz_back_cam_color', default_value='True') + arg_enable_gz_head_cam_color = DeclareLaunchArgument('enable_gz_head_cam_color', default_value='True') + arg_enable_gz_head_cam_depth = DeclareLaunchArgument('enable_gz_head_cam_depth', default_value='True') + arg_enable_gz_hand_cam_color = DeclareLaunchArgument('enable_gz_hand_cam_color', default_value='True') + arg_enable_gz_hand_cam_depth = DeclareLaunchArgument('enable_gz_hand_cam_depth', default_value='True') + arg_enable_gz_lidar = DeclareLaunchArgument('enable_gz_lidar', default_value='True') + arg_enable_gz_imu = DeclareLaunchArgument('enable_gz_imu', default_value='True') + + return LaunchDescription([ + arg_robot_name, + arg_robot_coords_x, + arg_robot_coords_y, + arg_robot_coords_Y, + arg_enable_gz_front_cam_color, + arg_enable_gz_back_cam_color, + arg_enable_gz_head_cam_color, + arg_enable_gz_head_cam_depth, + arg_enable_gz_hand_cam_color, + arg_enable_gz_hand_cam_depth, + arg_enable_gz_lidar, + arg_enable_gz_imu, + OpaqueFunction(function = launch_gz), + ]) + + +def launch_gz(context, *args, **kwargs): + robot_name = LaunchConfiguration('robot_name').perform(context) + robot_coords_x = LaunchConfiguration('robot_coords_x').perform(context) + robot_coords_y = LaunchConfiguration('robot_coords_y').perform(context) + robot_coords_Y = LaunchConfiguration('robot_coords_Y').perform(context) + enable_gz_front_cam_color = LaunchConfiguration('enable_gz_front_cam_color').perform(context) + enable_gz_back_cam_color = LaunchConfiguration('enable_gz_back_cam_color').perform(context) + enable_gz_head_cam_color = LaunchConfiguration('enable_gz_head_cam_color').perform(context) + enable_gz_head_cam_depth = LaunchConfiguration('enable_gz_head_cam_depth').perform(context) + enable_gz_hand_cam_color = LaunchConfiguration('enable_gz_hand_cam_color').perform(context) + enable_gz_hand_cam_depth = LaunchConfiguration('enable_gz_hand_cam_depth').perform(context) + enable_gz_lidar = LaunchConfiguration('enable_gz_lidar').perform(context) + enable_gz_imu = LaunchConfiguration('enable_gz_imu').perform(context) + + robot_description = os.path.join(get_package_share_directory( + 'sobit_edu_description'), + 'robots', + 'sobit_edu.urdf.xacro' + ) + robot_description_config = xacro.process_file( + robot_description, + mappings={ + 'enable_gz' : 'True', + 'robot_name' : robot_name, + 'enable_gz_front_cam_color' : enable_gz_front_cam_color, + 'enable_gz_back_cam_color' : enable_gz_back_cam_color, + 'enable_gz_head_cam_color' : enable_gz_head_cam_color, + 'enable_gz_head_cam_depth' : enable_gz_head_cam_depth, + 'enable_gz_hand_cam_color' : enable_gz_hand_cam_color, + 'enable_gz_hand_cam_depth' : enable_gz_hand_cam_depth, + 'enable_gz_lidar' : enable_gz_lidar, + 'enable_gz_imu' : enable_gz_imu, + }) + + + joint_state_broadcaster = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', + '--set-state', 'active', + '--controller-manager', robot_name+'/controller_manager', + '--use-sim-time', + 'joint_state_broadcaster' + ], + output='screen' + ) + + joint_trajectory_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', + '--set-state', 'active', + '--controller-manager', robot_name+'/controller_manager', + '--use-sim-time', + 'joint_trajectory_controller' + ], + output='screen' + ) + + velocity_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', + '--set-state', 'configure', + '--controller-manager', robot_name+'/controller_manager', + '--use-sim-time', + 'velocity_controller' + ], + output='screen' + ) + + diff_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', + '--set-state', 'active', + '--controller-manager', robot_name+'/controller_manager', + '--use-sim-time', + 'diff_controller' + ], + output='screen' + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + namespace=robot_name, + parameters=[ + {"frame_prefix": robot_name + '/'}, + {"robot_description": robot_description_config.toxml()}, + {"use_sim_time": True}, + ], + output="screen", + ) + + gz_spawn_entity_node = Node( + package='ros_gz_sim', + executable='create', + namespace=robot_name, + arguments=[ + '-topic', '/' + robot_name + '/robot_description', + '-name', robot_name, + '-x', robot_coords_x, + '-y', robot_coords_y, + '-Y', robot_coords_Y, + ], + output='screen', + ) + + gz_bridge_node = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + namespace=robot_name, + arguments=[ + "/" + robot_name + "/joint_states" + "@sensor_msgs/msg/JointState" + "[ignition.msgs.Model", + # "/model/" + robot_name + "/pose" + "@geometry_msgs/msg/Pose" + "[ignition.msgs.Pose", + "/" + robot_name + "/base_front_camera/camera_info" + "@sensor_msgs/msg/CameraInfo" + "[ignition.msgs.CameraInfo", + "/" + robot_name + "/base_front_camera/color" + "@sensor_msgs/msg/Image" + "[ignition.msgs.Image", + "/" + robot_name + "/base_front_camera/depth" + "@sensor_msgs/msg/Image" + "[ignition.msgs.Image", + # "/" + robot_name + "/base_back_camera/camera_info" + "@sensor_msgs/msg/CameraInfo" + "[ignition.msgs.CameraInfo", + # "/" + robot_name + "/base_back_camera/color" + "@sensor_msgs/msg/Image" + "[ignition.msgs.Image", + # "/" + robot_name + "/base_back_camera/depth" + "@sensor_msgs/msg/Image" + "[ignition.msgs.Image", + "/" + robot_name + "/head_camera/camera_info" + "@sensor_msgs/msg/CameraInfo" + "[ignition.msgs.CameraInfo", + "/" + robot_name + "/head_camera/color" + "@sensor_msgs/msg/Image" + "[ignition.msgs.Image", + "/" + robot_name + "/head_camera/depth" + "@sensor_msgs/msg/Image" + "[ignition.msgs.Image", + # "/" + robot_name + "/head_camera/depth/points" + "@sensor_msgs/msg/PointCloud2" + "[ignition.msgs.PointCloudPacked", + # "/" + robot_name + "/hand_camera/camera_info" + "@sensor_msgs/msg/CameraInfo" + "[ignition.msgs.CameraInfo", + # "/" + robot_name + "/hand_camera/color" + "@sensor_msgs/msg/Image" + "[ignition.msgs.Image", + # "/" + robot_name + "/hand_camera/depth" + "@sensor_msgs/msg/Image" + "[ignition.msgs.Image", + # "/" + robot_name + "/hand_camera/depth/points" + "@sensor_msgs/msg/PointCloud2" + "[ignition.msgs.PointCloudPacked", + "/" + robot_name + "/lidar/scan" + "@sensor_msgs/msg/LaserScan" + "[ignition.msgs.LaserScan", + "/" + robot_name + "/lidar/scan/points" + "@sensor_msgs/msg/PointCloud2" + "[ignition.msgs.PointCloudPacked", + "/" + robot_name + "/imu" + "@sensor_msgs/msg/Imu" + "[ignition.msgs.IMU", + ], + + output='screen' + ) + + # gz_tf_head_cam_node = Node( + # package='tf2_ros', + # executable='static_transform_publisher', + # arguments=['--frame-id', robot_name + '/head_camera_depth_optical_frame', + # '--child-frame-id', robot_name + '/head_pitch_link/head_camera_depth', + # '--pitch', '-1.57', + # '--roll', '1.57'], + # output='screen', + # ) + + # gz_tf_hand_cam_node = Node( + # package='tf2_ros', + # executable='static_transform_publisher', + # arguments=['--frame-id', robot_name + '/hand_camera_depth_optical_frame', + # '--child-frame-id', robot_name + '/arm_wrist_roll_link/hand_camera_depth', + # '--pitch', '-1.57', + # '--roll', '1.57'], + # output='screen', + # ) + # controller_pkg = robot_name + "_control" + # controller_config = os.path.join( + # get_package_share_directory( + # controller_pkg), "config", "controllers_gz.yaml" + # ) + + + # ros2_control_node = Node( + # package="controller_manager", + # executable="ros2_control_node", + # parameters=[ + # {"robot_description": robot_description_config.toxml()}, controller_config], + # output="screen", + # ) + return [ + # ros2_control_node, + gz_spawn_entity_node, + gz_bridge_node, + # gz_tf_head_cam_node, + # gz_tf_hand_cam_node, + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gz_spawn_entity_node, + on_exit=[joint_state_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster, + on_exit=[joint_trajectory_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster, + on_exit=[velocity_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster, + on_exit=[diff_controller], + ) + ), + robot_state_publisher_node, + ] \ No newline at end of file diff --git a/sobit_edu_bringup/launch/minimal.launch b/sobit_edu_bringup/launch/minimal.launch deleted file mode 100644 index 2e2a3bb..0000000 --- a/sobit_edu_bringup/launch/minimal.launch +++ /dev/null @@ -1,39 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sobit_edu_bringup/launch/minimal.launch.py b/sobit_edu_bringup/launch/minimal.launch.py new file mode 100755 index 0000000..891c683 --- /dev/null +++ b/sobit_edu_bringup/launch/minimal.launch.py @@ -0,0 +1,150 @@ +# Copyright 2020 Yutaka Kondo +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + robot_name = "sobit_edu" ### 各ロボットの名前にする + + + ##====================turtle bot2================================================= + + share_dir = get_package_share_directory('kobuki_node') + # There are two different ways to pass parameters to a non-composed node; + # either by specifying the path to the file containing the parameters, or by + # passing a dictionary containing the key -> value pairs of the parameters. + # When starting a *composed* node on the other hand, only the dictionary + # style is supported. To keep the code between the non-composed and + # composed launch file similar, we use that style here as well. + params_file = os.path.join(share_dir, 'config', 'kobuki_node_params.yaml') + with open(params_file, 'r') as f: + params = yaml.safe_load(f)['kobuki_ros_node']['ros__parameters'] + kobuki_ros_node = launch_ros.actions.Node(package='kobuki_node', + executable='kobuki_ros_node', + output='both', + parameters=[params]) + ##================================================================================== + + + + ##==================urg============================================================= + urg_node_dir = get_package_share_directory('urg_node') + launch_description = LaunchDescription([ + DeclareLaunchArgument( + 'sensor_interface', + default_value='serial', + description='sensor_interface: supported: serial, ethernet')]) + + def expand_param_file_name(context): + param_file = os.path.join( + urg_node_dir, 'launch', + 'urg_node_' + context.launch_configurations['sensor_interface'] + '.yaml') + if os.path.exists(param_file): + return [SetLaunchConfiguration('param', param_file)] + + param_file_path = OpaqueFunction(function=expand_param_file_name) + launch_description.add_action(param_file_path) + + hokuyo_node = Node( + package='urg_node', node_executable='urg_node', output='screen', + parameters=[LaunchConfiguration('param')] + ) + + launch_description.add_action(hokuyo_node) + ##================================================================================== + + + bringup_pkg = robot_name + "_bringup" + description_pkg = robot_name + "_description" + controller_pkg = robot_name + "_control" + + rviz_config = os.path.join(get_package_share_directory( + bringup_pkg), "rviz", "real.rviz") + + robot_description = os.path.join(get_package_share_directory( + description_pkg), "robots", robot_name + ".urdf.xacro") + robot_description_config = \ + xacro.process_file(robot_description, mappings={'enable_gz' : 'False'}) + + controller_config = os.path.join( + get_package_share_directory( + controller_pkg), "config", "controllers.yaml" + ) + + + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + {"robot_description": robot_description_config.toxml()}, controller_config], + output="screen", + ) + + joint_state_broadcaster_node = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + output="screen", + ) + + velocity_controller_node = Node( + package="controller_manager", + executable="spawner", + arguments=["velocity_controller", "-c", "/controller_manager"], + output="screen", + ) + + joint_trajectory_controller_node = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_trajectory_controller", "-c", "/controller_manager"], + output="screen", + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + parameters=[ + {"robot_description": robot_description_config.toxml()}, + {"use_sim_time": 'False'},], + output="screen", + ) + + rviz2_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config], + output="screen", + ) + + return LaunchDescription([ + launch_description, + kobuki_ros_node, + ros2_control_node, + joint_state_broadcaster_node, + velocity_controller_node, + joint_trajectory_controller_node, + robot_state_publisher_node, + rviz2_node, + ]) diff --git a/sobit_edu_bringup/launch/ps3_control.launch b/sobit_edu_bringup/launch/ps3_control.launch deleted file mode 100644 index 6942916..0000000 --- a/sobit_edu_bringup/launch/ps3_control.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - \ No newline at end of file diff --git a/sobit_edu_bringup/launch/ps4_control.launch b/sobit_edu_bringup/launch/ps4_control.launch deleted file mode 100644 index 7c68807..0000000 --- a/sobit_edu_bringup/launch/ps4_control.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - \ No newline at end of file diff --git a/sobit_edu_bringup/launch/urg.launch b/sobit_edu_bringup/launch/urg.launch deleted file mode 100644 index f657120..0000000 --- a/sobit_edu_bringup/launch/urg.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/sobit_edu_bringup/package.xml b/sobit_edu_bringup/package.xml old mode 100755 new mode 100644 index 4c339bc..d0bee9c --- a/sobit_edu_bringup/package.xml +++ b/sobit_edu_bringup/package.xml @@ -1,32 +1,18 @@ - + sobit_edu_bringup 0.0.0 - - This package allows shared configuration with SOBITS robots - - Yuki Okuma + TODO: Package description + sobits + TODO: License declaration - BSD + ament_cmake - https://home.soka.ac.jp/~choi/index.html - https://github.com/TeamSOBITS/sobit_common - https://github.com/TeamSOBITS/sobit_common/issues - Team SOBITS + ament_lint_auto + ament_lint_common - catkin - - rospy - sensor_msgs - trajectory_msgs - geometry_msgs - - - - + ament_cmake diff --git a/sobit_edu_bringup/rviz/real.rviz b/sobit_edu_bringup/rviz/real.rviz new file mode 100644 index 0000000..a554934 --- /dev/null +++ b/sobit_edu_bringup/rviz/real.rviz @@ -0,0 +1,249 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.5 + Tree Height: 419 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: "" + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: "" + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rgb/depth/topic_name + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_footprint: + Value: true + lidar: + Value: true + odom: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + odom: + base_footprint: + lidar: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 7.74399995803833 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.21821850538253784 + Y: -1.0755172967910767 + Z: 1.062055230140686 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.32039833068847656 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.965397834777832 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 704 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001560000022afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000390000022a000000c500fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000039000002b80000009d00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005100000003efc0100000002fb0000000800540069006d00650100000000000005100000021400fffffffb0000000800540069006d00650100000000000004500000000000000000000003b40000022a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1296 + X: 70 + Y: 27 diff --git a/sobit_edu_bringup/scripts/sobit_edu_ps3_control.py b/sobit_edu_bringup/scripts/sobit_edu_ps3_control.py deleted file mode 100755 index 7d289d4..0000000 --- a/sobit_edu_bringup/scripts/sobit_edu_ps3_control.py +++ /dev/null @@ -1,144 +0,0 @@ -#!/usr/bin/env python3 -# coding: utf-8 -import rospy - -from sensor_msgs.msg import Joy -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from sensor_msgs.msg import JointState -from geometry_msgs.msg import Twist - -class JoyControl: - def __init__(self): - # Subscriber - self.sub_joy = rospy.Subscriber('/joy', Joy, self.Joy_Callback, queue_size=1) - self.sub_joint_state = rospy.Subscriber('/joint_states', JointState, self.Joint_state_Callback, queue_size=10) - - # Publisher - self.pub_joint_trajectory = rospy.Publisher('/joint_trajectory', JointTrajectory, queue_size=10) - self.pub_twist = rospy.Publisher('/cmd_vel_mux/input/navi',Twist,queue_size=10) - - # Parameters - self.joint_state_msg = JointState() - self.joy_button = [0] * 17 - self.left_joystick_lr = 0 - self.left_joystick_ud = 0 - self.right_joystick_lr = 0 - self.right_joystick_ud = 0 - self.magnifications = 0.3 - - # Other buttons - """ - self.cross_button = False - self.circle_button = False - self.triangle_button = False - self.square_button = False - - self.l1_button = False - self.r1_button = False - self.l2_button = False - self.r2_button = False - - self.select_button = False - self.start_button = False - self.home_button = False - - self.l3_button = False - self.r3_button = False - - self.top_arrow_button = False - self.bottom_arrow_button = False - self.left_arrow_button = False - self.right_arrow_button = False - """ - - - def Joint_state_Callback(self, msg): - if msg.name: - self.joint_state_msg = msg - - - def Joy_Callback(self, msg): - self.joy_button = msg.buttons - self.left_joystick_lr = msg.axes[0] * self.magnifications - self.left_joystick_ud = msg.axes[1] * self.magnifications - self.right_joystick_lr = msg.axes[3] * self.magnifications - self.right_joystick_ud = msg.axes[4] * self.magnifications - - # L2 button pressed: Move the end effector - if self.joy_button[6]: - rospy.loginfo("Mode: end effector control") - - self.move_joint("hand_joint", self.left_joystick_ud + self.joint_state_msg.position[8]) - - # R2 button pressed: Move the camera - elif self.joy_button[7]: - rospy.loginfo("Mode: camera control") - - if self.joy_button[1]: - self.move_joint("head_camera_pan_joint" , self.left_joystick_ud + self.joint_state_msg.position[9]) - - elif self.joy_button[2]: - self.move_joint("head_camera_tilt_joint", - self.left_joystick_ud + self.joint_state_msg.position[10]) - - # L1 button pressed: Move the arm - elif self.joy_button[4]: - rospy.loginfo("Mode: arm control") - - # x button pressed - if self.joy_button[1]: - self.move_joint("arm_wrist_tilt_joint" , - self.left_joystick_ud + self.joint_state_msg.position[5]) - - # circle button pressed - elif self.joy_button[2]: - self.move_joint("arm_elbow_1_tilt_joint" , - self.left_joystick_ud + self.joint_state_msg.position[3]) - - # triangle button pressed - elif self.joy_button[3]: - self.move_joint("arm_shoulder_1_tilt_joint", - self.left_joystick_ud + self.joint_state_msg.position[1]) - - # square button pressed - elif self.joy_button[0]: - self.move_joint("arm_shoulder_pan_joint" , self.left_joystick_ud + self.joint_state_msg.position[0]) - - # Start button pressed: Initialize the robot - elif self.joy_button[12]: - self.magnifications = 0.3 - - self.move_joint("head_camera_pan_joint" , 0.0) - self.move_joint("head_camera_tilt_joint" , 0.0) - self.move_joint("arm_shoulder_pan_joint" , 0.0) - self.move_joint("arm_shoulder_1_tilt_joint", 0.0) - self.move_joint("arm_elbow_1_tilt_joint" , 0.0) - self.move_joint("arm_wrist_tilt_joint" , 0.0) - self.move_joint("hand_motor_joint" , 0.0) - self.move_joint("hand_joint" , 0.0) - - elif self.left_joystick_lr == 0 and self.left_joystick_ud == 0 and self.right_joystick_lr == 0 and self.right_joystick_ud == 0: - pass - - # Move the legs - else: - rospy.loginfo("Mode: leg control") - - twist = Twist() - twist.linear.x = self.left_joystick_ud * 0.2 - twist.angular.z = self.left_joystick_lr * 0.8 - - self.pub_twist.publish(twist) - - - def move_joint(self, joint_name, value): - point = JointTrajectoryPoint() - point.positions.append(value) - - send_data = JointTrajectory() - send_data.joint_names.append(joint_name) - send_data.points.append(point) - - self.pub_joint_trajectory.publish(send_data) - - -if __name__ == '__main__': - rospy.init_node('sobit_edu_ps3_control_node') - jc = JoyControl() - rospy.spin() diff --git a/sobit_edu_bringup/scripts/sobit_edu_ps4_control.py b/sobit_edu_bringup/scripts/sobit_edu_ps4_control.py deleted file mode 100755 index 02f66b7..0000000 --- a/sobit_edu_bringup/scripts/sobit_edu_ps4_control.py +++ /dev/null @@ -1,144 +0,0 @@ -#!/usr/bin/env python3 -# coding: utf-8 -import rospy - -from sensor_msgs.msg import Joy -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from sensor_msgs.msg import JointState -from geometry_msgs.msg import Twist - -class JoyControl: - def __init__(self): - # Subscriber - self.sub_joy = rospy.Subscriber('/joy', Joy, self.Joy_Callback, queue_size=1) - self.sub_joint_state = rospy.Subscriber('/joint_states', JointState, self.Joint_state_Callback, queue_size=10) - - # Publisher - self.pub_joint_trajectory = rospy.Publisher('/joint_trajectory', JointTrajectory, queue_size=10) - self.pub_twist = rospy.Publisher('/cmd_vel_mux/input/navi',Twist,queue_size=10) - - # Parameters - self.joint_state_msg = JointState() - self.joy_button = [0] * 17 - self.left_joystick_lr = 0 - self.left_joystick_ud = 0 - self.right_joystick_lr = 0 - self.right_joystick_ud = 0 - self.magnifications = 0.3 - - # Other buttons - """ - self.cross_button = False - self.circle_button = False - self.triangle_button = False - self.square_button = False - - self.l1_button = False - self.r1_button = False - self.l2_button = False - self.r2_button = False - - self.select_button = False - self.start_button = False - self.home_button = False - - self.l3_button = False - self.r3_button = False - - self.top_arrow_button = False - self.bottom_arrow_button = False - self.left_arrow_button = False - self.right_arrow_button = False - """ - - - def Joint_state_Callback(self, msg): - if msg.name: - self.joint_state_msg = msg - - - def Joy_Callback(self, msg): - self.joy_button = msg.buttons - self.left_joystick_lr = msg.axes[0] * self.magnifications - self.left_joystick_ud = msg.axes[1] * self.magnifications - self.right_joystick_lr = msg.axes[3] * self.magnifications - self.right_joystick_ud = msg.axes[4] * self.magnifications - - # L2 button pressed: Move the end effector - if self.joy_button[6]: - rospy.loginfo("Mode: end effector control") - - self.move_joint("hand_joint", self.left_joystick_ud + self.joint_state_msg.position[8]) - - # R2 button pressed: Move the camera - elif self.joy_button[7]: - rospy.loginfo("Mode: camera control") - - if self.joy_button[1]: - self.move_joint("head_camera_pan_joint" , self.left_joystick_ud + self.joint_state_msg.position[9]) - - elif self.joy_button[2]: - self.move_joint("head_camera_tilt_joint", - self.left_joystick_ud + self.joint_state_msg.position[10]) - - # L1 button pressed: Move the arm - elif self.joy_button[4]: - rospy.loginfo("Mode: arm control") - - # x button pressed - if self.joy_button[1]: - self.move_joint("arm_wrist_tilt_joint" , - self.left_joystick_ud + self.joint_state_msg.position[5]) - - # circle button pressed - elif self.joy_button[2]: - self.move_joint("arm_elbow_1_tilt_joint" , - self.left_joystick_ud + self.joint_state_msg.position[3]) - - # triangle button pressed - elif self.joy_button[3]: - self.move_joint("arm_shoulder_1_tilt_joint", - self.left_joystick_ud + self.joint_state_msg.position[1]) - - # square button pressed - elif self.joy_button[0]: - self.move_joint("arm_shoulder_pan_joint" , self.left_joystick_ud + self.joint_state_msg.position[0]) - - # Start button pressed: Initialize the robot - elif self.joy_button[12]: - self.magnifications = 0.3 - - self.move_joint("head_camera_pan_joint" , 0.0) - self.move_joint("head_camera_tilt_joint" , 0.0) - self.move_joint("arm_shoulder_pan_joint" , 0.0) - self.move_joint("arm_shoulder_1_tilt_joint", 0.0) - self.move_joint("arm_elbow_1_tilt_joint" , 0.0) - self.move_joint("arm_wrist_tilt_joint" , 0.0) - self.move_joint("hand_motor_joint" , 0.0) - self.move_joint("hand_joint" , 0.0) - - # elif self.left_joystick_lr == 0 and self.left_joystick_ud == 0 and self.right_joystick_lr == 0 and self.right_joystick_ud == 0: - # pass - - # Move the legs - else: - rospy.loginfo("Mode: leg control") - - twist = Twist() - twist.linear.x = self.left_joystick_ud * 0.2 - twist.angular.z = self.left_joystick_lr * 0.8 - - self.pub_twist.publish(twist) - - - def move_joint(self, joint_name, value): - point = JointTrajectoryPoint() - point.positions.append(value) - - send_data = JointTrajectory() - send_data.joint_names.append(joint_name) - send_data.points.append(point) - - self.pub_joint_trajectory.publish(send_data) - - -if __name__ == '__main__': - rospy.init_node('sobit_edu_ps4_control_node') - jc = JoyControl() - rospy.spin() diff --git a/sobit_edu_bringup/src/.gitkeep b/sobit_edu_bringup/src/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/sobit_edu_control/CMakeLists.txt b/sobit_edu_control/CMakeLists.txt index eae6c5c..9ef5843 100644 --- a/sobit_edu_control/CMakeLists.txt +++ b/sobit_edu_control/CMakeLists.txt @@ -1,28 +1,31 @@ -cmake_minimum_required(VERSION 3.10.2) +cmake_minimum_required(VERSION 3.8) project(sobit_edu_control) -add_compile_options(-std=c++14) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - controller_manager - robot_state_publisher - sobits_common +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME} ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - # INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS controller_manager robot_state_publisher sobits_common -) \ No newline at end of file +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/sobit_edu_control/config/controllers.yaml b/sobit_edu_control/config/controllers.yaml new file mode 100644 index 0000000..a8c2eaf --- /dev/null +++ b/sobit_edu_control/config/controllers.yaml @@ -0,0 +1,41 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + velocity_controller: + type: velocity_controllers/JointGroupVelocityController + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +velocity_controller: + ros__parameters: + joints: ### アームなどの関節＝Dynamixelのモータ名を全部リストで！ + - arm_shoulder_pan_joint + - arm_shoulder_1_tilt_joint + - arm_elbow_1_tilt_joint + - arm_wrist_tilt_joint + - hand_joint + - head_camera_pan_joint + - head_camera_tilt_joint + +joint_trajectory_controller: + ros__parameters: + joints: + - arm_shoulder_pan_joint + - arm_shoulder_1_tilt_joint + - arm_elbow_1_tilt_joint + - arm_wrist_tilt_joint + - hand_joint + - head_camera_pan_joint + - head_camera_tilt_joint + + command_interfaces: + - position + + state_interfaces: + - position + - velocity \ No newline at end of file diff --git a/sobit_edu_control/config/controllers_gz.yaml b/sobit_edu_control/config/controllers_gz.yaml new file mode 100644 index 0000000..062524b --- /dev/null +++ b/sobit_edu_control/config/controllers_gz.yaml @@ -0,0 +1,96 @@ +/**/controller_manager: + ros__parameters: + update_rate: 100 # Hz + use_sim_time: True + + velocity_controller: + type: "velocity_controllers/JointGroupVelocityController" + + joint_trajectory_controller: + type: "joint_trajectory_controller/JointTrajectoryController" + + diff_controller: + type: "diff_drive_controller/DiffDriveController" + + joint_state_broadcaster: + type: "joint_state_broadcaster/JointStateBroadcaster" + + +/**/velocity_controller: + ros__parameters: + joints: + - arm_shoulder_pan_joint + - arm_shoulder_1_tilt_joint + - arm_elbow_1_tilt_joint + - arm_wrist_tilt_joint + - hand_joint + - head_camera_pan_joint + - head_camera_tilt_joint + +/**/joint_trajectory_controller: + ros__parameters: + joints: + - arm_shoulder_pan_joint + - arm_shoulder_1_tilt_joint + - arm_elbow_1_tilt_joint + - arm_wrist_tilt_joint + - hand_joint + - head_camera_pan_joint + - head_camera_tilt_joint + + command_interfaces: + - position + + state_interfaces: + - position + - velocity + + +# https://control.ros.org/humble/doc/ros2_controllers/diff_drive_controller/doc/userdoc.html +/**/diff_controller: + ros__parameters: + left_wheel_names : ["base_l_drive_wheel_joint"] + right_wheel_names: ["base_r_drive_wheel_joint"] + + wheel_separation: 0.200 + #wheels_per_side: 1 # actually 2, but both are controlled by 1 signal + wheel_radius: 0.045 + + wheel_separation_multiplier : 1.0 + left_wheel_radius_multiplier : 1.0 + right_wheel_radius_multiplier: 1.0 + + publish_rate : 50.0 + odom_frame_id: odom + base_frame_id: base_footprint + pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01] + twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01] + + open_loop: true + enable_odom_tf: true + + cmd_vel_timeout: 0.5 + #publish_limited_velocity: true + use_stamped_vel: false + #velocity_rolling_window_size: 10 + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear.x.has_velocity_limits: true + linear.x.has_acceleration_limits: true + linear.x.has_jerk_limits: false + linear.x.max_velocity: 1.0 + linear.x.min_velocity: -1.0 + linear.x.max_acceleration: 1.0 + linear.x.max_jerk: 0.0 + linear.x.min_jerk: 0.0 + + angular.z.has_velocity_limits: true + angular.z.has_acceleration_limits: true + angular.z.has_jerk_limits: false + angular.z.max_velocity: 1.0 + angular.z.min_velocity: -1.0 + angular.z.max_acceleration: 1.0 + angular.z.min_acceleration: -1.0 + angular.z.max_jerk: 0.0 + angular.z.min_jerk: 0.0 \ No newline at end of file diff --git a/sobit_edu_control/config/joint_limit.yaml b/sobit_edu_control/config/joint_limit.yaml deleted file mode 100644 index bd5e4ed..0000000 --- a/sobit_edu_control/config/joint_limit.yaml +++ /dev/null @@ -1,73 +0,0 @@ -joint_limits: - arm_shoulder_pan_joint: - has_position_limits: true - min_position: -3.14 - max_position: 3.14 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: true - max_acceleration: 4.0 - arm_shoulder_1_tilt_joint: - has_position_limits: true - min_position: -1.5708 - max_position: 1.5708 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: true - max_acceleration: 4.0 - arm_shoulder_2_tilt_joint: - has_position_limits: true - min_position: -1.5708 - max_position: 1.5708 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: true - max_acceleration: 4.0 - arm_elbow_1_tilt_joint: - has_position_limits: true - min_position: -1.5708 - max_position: 1.5708 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: true - max_acceleration: 4.0 - arm_elbow_2_tilt_joint: - has_position_limits: true - min_position: -1.5708 - max_position: 1.5708 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: true - max_acceleration: 4.0 - arm_wrist_tilt_joint: - has_position_limits: true - min_position: -1.5708 - max_position: 1.5708 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: true - max_acceleration: 4.0 - hand_joint: - has_position_limits: true - min_position: -1.5708 - max_position: 0 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: true - max_acceleration: 4.0 - head_camera_pan_joint: - has_position_limits: true - min_position: -1.5708 - max_position: 1.5708 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: true - max_acceleration: 4.0 - head_camera_tilt_joint: - has_position_limits: true - min_position: -1.5708 - max_position: 1.5708 - has_velocity_limits: true - max_velocity: 5.969211435 - has_acceleration_limits: true - max_acceleration: 4.0 \ No newline at end of file diff --git a/sobit_edu_control/config/state_control_arm.yaml b/sobit_edu_control/config/state_control_arm.yaml deleted file mode 100644 index cc33845..0000000 --- a/sobit_edu_control/config/state_control_arm.yaml +++ /dev/null @@ -1,21 +0,0 @@ -hardware: - publish_rate: 10 - -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 10 - -arm_trajectory_controller: - type: position_controllers/JointTrajectoryController - joints: - - arm_shoulder_pan_joint - - arm_shoulder_1_tilt_joint - - arm_shoulder_2_tilt_joint - - arm_elbow_1_tilt_joint - - arm_elbow_2_tilt_joint - - arm_wrist_tilt_joint - - hand_joint - constraints: - stopped_velocity_tolerance: 0.5 - goal_time: 0.05 - allow_partial_joints_goal: true \ No newline at end of file diff --git a/sobit_edu_control/config/state_control_dynamixel_arm.yaml b/sobit_edu_control/config/state_control_dynamixel_arm.yaml deleted file mode 100644 index 000a5f7..0000000 --- a/sobit_edu_control/config/state_control_dynamixel_arm.yaml +++ /dev/null @@ -1,20 +0,0 @@ -dynamixel_port: - port_name: "/dev/input/dx_upper" - baud_rate: 1000000 - joints: - - arm_shoulder_pan_joint - - arm_shoulder_1_tilt_joint - - arm_shoulder_2_tilt_joint - - arm_elbow_1_tilt_joint - - arm_elbow_2_tilt_joint - - arm_wrist_tilt_joint - - hand_joint - - arm_shoulder_pan_joint: {id: 10, center: 2048, home: 0.0, mode: 3, vel: 128, acc: 0, pos_d_gain: 1300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1 } - arm_shoulder_1_tilt_joint: {id: 11, center: 2048, home: 1.5708, mode: 3, vel: 128, acc: 0, pos_d_gain: 1300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1 } - arm_shoulder_2_tilt_joint: {id: 12, center: 2048, home: -1.5708, mode: 3, vel: 128, acc: 0, pos_d_gain: 1300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1 } - arm_elbow_1_tilt_joint: {id: 13, center: 2048, home: -1.40, mode: 3, vel: 128, acc: 0, pos_d_gain: 1300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1 } - arm_elbow_2_tilt_joint: {id: 14, center: 2048, home: 1.40, mode: 3, vel: 128, acc: 0, pos_d_gain: 1300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1 } - arm_wrist_tilt_joint: {id: 15, center: 2048, home: -0.17, mode: 3, vel: 128, acc: 0, pos_d_gain: 1300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1 } - hand_joint: {id: 16, center: 2048, home: 0.0, mode: 5, vel: 128, acc: 0, pos_d_gain: 300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1, lim: 100 } - \ No newline at end of file diff --git a/sobit_edu_control/config/state_control_dynamixel_arm_head.yaml b/sobit_edu_control/config/state_control_dynamixel_arm_head.yaml deleted file mode 100644 index 56c4784..0000000 --- a/sobit_edu_control/config/state_control_dynamixel_arm_head.yaml +++ /dev/null @@ -1,23 +0,0 @@ -dynamixel_port: - port_name: "/dev/input/dx_upper" - baud_rate: 1000000 - joints: - - arm_shoulder_pan_joint - - arm_shoulder_1_tilt_joint - - arm_shoulder_2_tilt_joint - - arm_elbow_1_tilt_joint - - arm_elbow_2_tilt_joint - - arm_wrist_tilt_joint - - hand_joint - - head_camera_pan_joint - - head_camera_tilt_joint - - arm_shoulder_pan_joint: {id: 10, center: 2048, home: 0.0, mode: 3, vel: 128, acc: 0, pos_d_gain: 1300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1 } - arm_shoulder_1_tilt_joint: {id: 11, center: 2048, home: 1.5708, mode: 3, vel: 128, acc: 0, pos_d_gain: 1300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1 } - arm_shoulder_2_tilt_joint: {id: 12, center: 2048, home: -1.5708, mode: 3, vel: 128, acc: 0, pos_d_gain: 1300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1 } - arm_elbow_1_tilt_joint: {id: 13, center: 2048, home: -1.40, mode: 3, vel: 128, acc: 0, pos_d_gain: 1300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1 } - arm_elbow_2_tilt_joint: {id: 14, center: 2048, home: 1.40, mode: 3, vel: 128, acc: 0, pos_d_gain: 1300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1 } - arm_wrist_tilt_joint: {id: 15, center: 2048, home: -0.17, mode: 3, vel: 128, acc: 0, pos_d_gain: 1300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1 } - hand_joint: {id: 16, center: 2048, home: 0.0, mode: 5, vel: 128, acc: 0, pos_d_gain: 300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1, lim: 100 } - head_camera_pan_joint: {id: 21, center: 2048, home: 0.0, mode: 3, vel: 128, acc: 0, pos_d_gain: 1300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1 } - head_camera_tilt_joint: {id: 22, center: 2048, home: 0.0, mode: 3, vel: 128, acc: 0, pos_d_gain: 1300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1 } \ No newline at end of file diff --git a/sobit_edu_control/config/state_control_dynamixel_head.yaml b/sobit_edu_control/config/state_control_dynamixel_head.yaml deleted file mode 100644 index e05e5c9..0000000 --- a/sobit_edu_control/config/state_control_dynamixel_head.yaml +++ /dev/null @@ -1,8 +0,0 @@ -dynamixel_port: - port_name: "/dev/input/dx_upper" - baud_rate: 1000000 - joints: - - head_camera_pan_joint - - head_camera_tilt_joint - head_camera_pan_joint: {id: 21, center: 2048, home: 0.0, mode: 3, vel: 128, acc: 0, pos_d_gain: 1300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1 } - head_camera_tilt_joint: {id: 22, center: 2048, home: 0.0, mode: 3, vel: 128, acc: 0, pos_d_gain: 1300, pos_i_gain: 5, pos_p_gain: 1000, gear_ratio: 1 } \ No newline at end of file diff --git a/sobit_edu_control/config/state_control_head.yaml b/sobit_edu_control/config/state_control_head.yaml deleted file mode 100644 index 5deaa42..0000000 --- a/sobit_edu_control/config/state_control_head.yaml +++ /dev/null @@ -1,16 +0,0 @@ -hardware: - publish_rate: 10 - -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 10 - -head_camera_trajectory_controller: - type: position_controllers/JointTrajectoryController - joints: - - head_camera_pan_joint - - head_camera_tilt_joint - constraints: - stopped_velocity_tolerance: 0.5 - goal_time: 0.05 - allow_partial_joints_goal: true \ No newline at end of file diff --git a/sobit_edu_control/include/sobit_edu_control/.gitkeep b/sobit_edu_control/include/sobit_edu_control/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/sobit_edu_control/launch/sobit_edu_control.launch b/sobit_edu_control/launch/sobit_edu_control.launch deleted file mode 100644 index 86196c2..0000000 --- a/sobit_edu_control/launch/sobit_edu_control.launch +++ /dev/null @@ -1,61 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/sobit_edu_control/package.xml b/sobit_edu_control/package.xml index 81f8363..25a0d46 100644 --- a/sobit_edu_control/package.xml +++ b/sobit_edu_control/package.xml @@ -1,31 +1,18 @@ - + sobit_edu_control 0.0.0 - - This package allows shared configuration with SOBITS robots - - Yuki Okuma + TODO: Package description + sobits + TODO: License declaration - BSD + ament_cmake - https://home.soka.ac.jp/~choi/index.html - https://github.com/TeamSOBITS/sobit_common - https://github.com/TeamSOBITS/sobit_common/issues - Team SOBITS + ament_lint_auto + ament_lint_common - catkin - - controller_manager - robot_state_publisher - sobits_common - - - - + ament_cmake - \ No newline at end of file + diff --git a/sobit_edu_control/src/.gitkeep b/sobit_edu_control/src/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/sobit_edu_description/CMakeLists.txt b/sobit_edu_description/CMakeLists.txt old mode 100755 new mode 100644 index b8b7d3a..3a6cb6e --- a/sobit_edu_description/CMakeLists.txt +++ b/sobit_edu_description/CMakeLists.txt @@ -1,27 +1,35 @@ -cmake_minimum_required(VERSION 3.10.2) +cmake_minimum_required(VERSION 3.8) project(sobit_edu_description) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - joint_state_publisher - joint_state_publisher_gui - robot_state_publisher - rviz +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +install(DIRECTORY + launch + meshes + rviz + urdf + robots + DESTINATION share/${PROJECT_NAME} ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - # INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS joint_state_publisher joint_state_publisher_gui robot_state_publisher rviz -) \ No newline at end of file +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/sobit_edu_description/launch/display.launch b/sobit_edu_description/launch/display.launch deleted file mode 100644 index 388b1b4..0000000 --- a/sobit_edu_description/launch/display.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/sobit_edu_description/launch/display.launch.py b/sobit_edu_description/launch/display.launch.py new file mode 100755 index 0000000..7e4f275 --- /dev/null +++ b/sobit_edu_description/launch/display.launch.py @@ -0,0 +1,67 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + + use_gui = LaunchConfiguration('use_gui', default='True') + + robot_name = "sobit_edu" + + description_pkg = robot_name + "_description" + + rviz_config = os.path.join(get_package_share_directory( + description_pkg), "rviz", "display.rviz") + + robot_description = os.path.join(get_package_share_directory( + description_pkg), "robots", robot_name + "_robot.urdf.xacro") + robot_description_config = \ + xacro.process_file(robot_description, mappings={'enable_gz' : 'True'}) + + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + parameters=[ + {"robot_description": robot_description_config.toxml()}, + {"use_sim_time": True},], + output="screen", + ) + + joint_state_publisher_node = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + output='screen', + condition=UnlessCondition(use_gui) + ) + + joint_state_publisher_gui_node = Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + output='screen', + condition=IfCondition(use_gui) + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + output='screen', + arguments=['-d', rviz_config], + ) + + return LaunchDescription([ + joint_state_publisher_node, + joint_state_publisher_gui_node, + robot_state_publisher_node, + rviz_node, + ]) diff --git a/sobit_edu_description/launch/gazebo.launch.py b/sobit_edu_description/launch/gazebo.launch.py new file mode 100755 index 0000000..f1cf441 --- /dev/null +++ b/sobit_edu_description/launch/gazebo.launch.py @@ -0,0 +1,172 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnExecutionComplete, OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + # Launch Arguments + use_sim_time = LaunchConfiguration('use_sim_time', default=True) + + description_package = os.path.join( + get_package_share_directory('sobit_edu_description'),) + + xacro_file = os.path.join(description_package, + 'robots', + 'sobit_edu.urdf.xacro') + + rviz_file = os.path.join(description_package, + 'rviz', + 'gazebo.rviz') + + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + params = {'robot_description': doc.toxml(), 'use_sim_time': use_sim_time} + + # print(params) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + output='screen', + arguments=['-d', rviz_file], + ) + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[params], + ) + + node_tf_camera = Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['--frame-id', 'head_camera_depth_optical_frame', + '--child-frame-id', 'sobit_edu/head_pitch_link/head_camera_depth', + '--pitch', '-1.57', + '--roll', '1.57'], + ) + + ignition_spawn_entity = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=['-string', doc.toxml(), + '-name', 'sobit_edu', + '-allow_renaming', 'true'], + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + '--use-sim-time', + 'joint_state_broadcaster'], + output='screen' + ) + + load_arm_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + '--use-sim-time', + 'arm_controller'], + output='screen' + ) + + load_head_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + '--use-sim-time', + 'head_controller'], + output='screen' + ) + + load_gripper_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + '--use-sim-time', + 'gripper_controller'], + output='screen' + ) + + load_diff_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + '--use-sim-time', + 'diff_controller'], + output='screen' + ) + + # Bridge + bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=[ + "/clock" + "@rosgraph_msgs/msg/Clock" + "[ignition.msgs.Clock", + # "/kachaka/lidar/scan" + "@sensor_msgs/msg/LaserScan" + "[ignition.msgs.LaserScan", + # "/kachaka/lidar/scan/points" + "@sensor_msgs/msg/PointCloud2" + "[ignition.msgs.PointCloudPacked", + "/tf" + "@tf2_msgs/msg/TFMessage" + "[ignition.msgs.TFMessage", + "/model/sobit_edu/pose" + "@geometry_msgs/msg/Pose" + "[ignition.msgs.Pose", + "/joint_states" + "@sensor_msgs/msg/JointState" + "[ignition.msgs.Model", + "/head_camera/camera_info" + "@sensor_msgs/msg/CameraInfo" + "[ignition.msgs.CameraInfo", + "/head_camera/color" + "@sensor_msgs/msg/Image" + "[ignition.msgs.Image", + "/head_camera/depth" + "@sensor_msgs/msg/Image" + "[ignition.msgs.Image", + "/head_camera/depth/points" + "@sensor_msgs/msg/PointCloud2" + "[ignition.msgs.PointCloudPacked", + ], + output='screen' + ) + + return LaunchDescription([ + bridge, + # Launch gazebo environment + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', 'gz_sim.launch.py')]), + launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=ignition_spawn_entity, + on_exit=[load_joint_state_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_arm_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_head_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_gripper_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_diff_controller], + ) + ), + node_robot_state_publisher, + node_tf_camera, + ignition_spawn_entity, + rviz_node, + # Launch Arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value=use_sim_time, + description='If true, use simulated clock'), + ]) \ No newline at end of file diff --git a/sobit_edu_description/meshes/arm/arm_base.stl b/sobit_edu_description/meshes/arm/arm_base.stl index b6d7032..a655f67 100644 Binary files a/sobit_edu_description/meshes/arm/arm_base.stl and b/sobit_edu_description/meshes/arm/arm_base.stl differ diff --git a/sobit_edu_description/meshes/arm/arm_lower.stl b/sobit_edu_description/meshes/arm/arm_lower.stl index b93f163..50c2386 100644 Binary files a/sobit_edu_description/meshes/arm/arm_lower.stl and b/sobit_edu_description/meshes/arm/arm_lower.stl differ diff --git a/sobit_edu_description/meshes/arm/arm_rotate.stl b/sobit_edu_description/meshes/arm/arm_rotate.stl index ab4726f..acc5ffc 100644 Binary files a/sobit_edu_description/meshes/arm/arm_rotate.stl and b/sobit_edu_description/meshes/arm/arm_rotate.stl differ diff --git a/sobit_edu_description/meshes/arm/arm_upper.stl b/sobit_edu_description/meshes/arm/arm_upper.stl index 9aded7f..a4087a0 100644 Binary files a/sobit_edu_description/meshes/arm/arm_upper.stl and b/sobit_edu_description/meshes/arm/arm_upper.stl differ diff --git a/sobit_edu_description/package.xml b/sobit_edu_description/package.xml old mode 100755 new mode 100644 index 931c205..c98ea4f --- a/sobit_edu_description/package.xml +++ b/sobit_edu_description/package.xml @@ -1,32 +1,18 @@ - + sobit_edu_description 0.0.0 - - This package allows shared configuration with SOBITS robots - - Yuki Okuma + TODO: Package description + sobits + TODO: License declaration - BSD + ament_cmake - https://home.soka.ac.jp/~choi/index.html - https://github.com/TeamSOBITS/sobit_common - https://github.com/TeamSOBITS/sobit_common/issues - Team SOBITS + ament_lint_auto + ament_lint_common - catkin - - joint_state_publisher - joint_state_publisher_gui - robot_state_publisher - rviz - - - - + ament_cmake - \ No newline at end of file + diff --git a/sobit_edu_description/robots/sobit_edu.urdf.xacro b/sobit_edu_description/robots/sobit_edu.urdf.xacro index fad8d33..823aed3 100644 --- a/sobit_edu_description/robots/sobit_edu.urdf.xacro +++ b/sobit_edu_description/robots/sobit_edu.urdf.xacro @@ -1,9 +1,12 @@ - + + + - + + @@ -21,8 +24,19 @@ - - + + + + + + + + + + + sobit_edu/joint_states + + diff --git a/sobit_edu_description/rviz/display.rviz b/sobit_edu_description/rviz/display.rviz index 4f2d904..b7c3add 100644 --- a/sobit_edu_description/rviz/display.rviz +++ b/sobit_edu_description/rviz/display.rviz @@ -1,42 +1,34 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - - /RobotModel1/Links1 - Splitter Ratio: 0.8414985537528992 - Tree Height: 731 - - Class: rviz/Selection + - /Global Options1 + - /Status1 + - /TF1 + Splitter Ratio: 0.5905882120132446 + Tree Height: 799 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 - /Current View1/Focal Point1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 + Class: rviz_default_plugins/Grid + Color: 160; 160; 160 Enabled: true Line Style: Line Width: 0.029999999329447746 @@ -51,9 +43,17 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 0.699999988079071 - Class: rviz/RobotModel + - Alpha: 0.5 + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: All Links Enabled: true @@ -66,625 +66,554 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - arm_elbow_1_tilt_link: + arm_elbow_pitch_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - arm_elbow_2_tilt_link: + arm_forearm_roll_link: Alpha: 1 Show Axes: false Show Trail: false - arm_shoulder_1_tilt_link: + Value: true + arm_hand_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - arm_shoulder_2_tilt_link: + arm_platform_link: Alpha: 1 Show Axes: false Show Trail: false - arm_shoulder_pan_link: + Value: true + arm_shoulder_pitch_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - arm_wrist_tilt_link: + arm_shoulder_pitch_sub_link: Alpha: 1 Show Axes: false Show Trail: false - back_base_link: + arm_shoulder_roll_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: + arm_wrist_pitch_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - caster_b_link: + arm_wrist_roll_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - caster_f_link: + back_base_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - cliff_sensor_f_link: + base_footprint: Alpha: 1 Show Axes: false Show Trail: false - cliff_sensor_l_link: + base_l_drive_wheel_link: Alpha: 1 Show Axes: false Show Trail: false - cliff_sensor_r_link: + Value: true + base_link: Alpha: 1 Show Axes: false Show Trail: false - cover_link: + Value: true + base_r_drive_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - gyro_link: + camera_back_link: Alpha: 1 Show Axes: false Show Trail: false - hand_base_link: + camera_front_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - hand_finger_l_link: + docking_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - hand_finger_r_link: + hand_camera_base_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - hand_inner_l_link: + hand_camera_bottom_screw_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - hand_inner_r_link: + hand_camera_color_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - hand_link: + hand_camera_color_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - hand_outer_l_link: + hand_camera_depth_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - hand_outer_r_link: + hand_camera_depth_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - head_camera_base_link: + hand_camera_infra1_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - head_camera_link: + hand_camera_infra1_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - head_camera_pan_link: + hand_camera_infra2_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - head_camera_plug_base_link: + hand_camera_infra2_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - head_camera_tilt_link: + hand_camera_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - head_camera_visor_base_link: + hand_end_effector_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - lidar_base_link: + hand_finger_tip_l_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - lidar_laser: - Alpha: 1 - Show Axes: false - Show Trail: false - lidar_laser_mount: + hand_finger_tip_r_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - matrix_base_link: + hand_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - matrix_board_link: + hand_sub_l_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - matrix_sensor_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - matrix_sensor_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - matrix_sensor_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - matrix_sensor_4_link: + hand_sub_link: Alpha: 1 Show Axes: false Show Trail: false - matrix_sensor_5_link: - Alpha: 1 - Show Axes: false - Show Trail: false - matrix_sensor_6_link: - Alpha: 1 - Show Axes: false - Show Trail: false - matrix_sensor_7_link: + Value: true + hand_sub_r_link: Alpha: 1 Show Axes: false Show Trail: false - matrix_sensor_8_link: + Value: true + head_base_link: Alpha: 1 Show Axes: false Show Trail: false - mic_base_link: + Value: true + head_camera_base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - mic_link: + head_camera_bottom_screw_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - plate_bottom_link: + head_camera_color_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - plate_middle_link: + head_camera_color_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - plate_top_link: + head_camera_depth_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - pole_bottom_0_link: + head_camera_depth_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - pole_bottom_1_link: + head_camera_infra1_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - pole_bottom_2_link: + head_camera_infra1_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - pole_bottom_3_link: + head_camera_infra2_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - pole_bottom_4_link: + head_camera_infra2_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - pole_bottom_5_link: + head_camera_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - pole_middle_4_link: + head_pitch_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - pole_middle_5_link: + head_yaw_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - pole_middle_6_link: + imu_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - pole_middle_7_link: + laser_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - pole_top_0_link: + mic_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - pole_top_1_link: + plate_bottom_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - pole_top_2_link: + plate_middle_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - pole_top_3_link: + sim_laser_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - wheel_l_link: + speaker_base_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wheel_r_link: + tof_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" Frame Timeout: 15 Frames: - All Enabled: false + All Enabled: true arm_base_link: Value: true - arm_elbow_1_tilt_link: - Value: true - arm_elbow_2_tilt_link: - Value: true - arm_shoulder_1_tilt_link: - Value: true - arm_shoulder_2_tilt_link: - Value: true - arm_shoulder_pan_link: - Value: true - arm_wrist_tilt_link: - Value: true - back_base_link: + arm_elbow_pitch_link: Value: true - base_footprint: + arm_forearm_roll_link: Value: true - base_link: + arm_hand_link: Value: true - caster_b_link: + arm_platform_link: Value: true - caster_f_link: + arm_shoulder_pitch_link: Value: true - cliff_sensor_f_link: + arm_shoulder_pitch_sub_link: Value: true - cliff_sensor_l_link: + arm_shoulder_roll_link: Value: true - cliff_sensor_r_link: + arm_wrist_pitch_link: Value: true - cover_link: + arm_wrist_roll_link: Value: true - gyro_link: + back_base_link: Value: true - hand_base_link: + base_footprint: Value: true - hand_finger_l_link: + base_l_drive_wheel_link: Value: true - hand_finger_r_link: + base_link: Value: true - hand_inner_l_link: + base_r_drive_wheel_link: Value: true - hand_inner_r_link: + camera_back_link: Value: true - hand_link: + camera_front_link: Value: true - hand_outer_l_link: + docking_link: Value: true - hand_outer_r_link: + hand_camera_base_link: Value: true - head_camera_base_link: + hand_camera_bottom_screw_frame: Value: true - head_camera_link: + hand_camera_color_frame: Value: true - head_camera_pan_link: + hand_camera_color_optical_frame: Value: true - head_camera_plug_base_link: + hand_camera_depth_frame: Value: true - head_camera_tilt_link: + hand_camera_depth_optical_frame: Value: true - head_camera_visor_base_link: + hand_camera_infra1_frame: Value: true - lidar_base_link: + hand_camera_infra1_optical_frame: Value: true - lidar_laser: + hand_camera_infra2_frame: Value: true - lidar_laser_mount: + hand_camera_infra2_optical_frame: Value: true - matrix_base_link: + hand_camera_link: Value: true - matrix_board_link: + hand_end_effector_link: Value: true - matrix_sensor_1_link: + hand_finger_tip_l_link: Value: true - matrix_sensor_2_link: + hand_finger_tip_r_link: Value: true - matrix_sensor_3_link: + hand_link: Value: true - matrix_sensor_4_link: + hand_sub_l_link: Value: true - matrix_sensor_5_link: + hand_sub_link: Value: true - matrix_sensor_6_link: + hand_sub_r_link: Value: true - matrix_sensor_7_link: + head_base_link: Value: true - matrix_sensor_8_link: + head_camera_base_link: Value: true - mic_base_link: + head_camera_bottom_screw_frame: Value: true - mic_link: + head_camera_color_frame: Value: true - plate_bottom_link: - Value: true - plate_middle_link: + head_camera_color_optical_frame: Value: true - plate_top_link: + head_camera_depth_frame: Value: true - pole_bottom_0_link: + head_camera_depth_optical_frame: Value: true - pole_bottom_1_link: + head_camera_infra1_frame: Value: true - pole_bottom_2_link: + head_camera_infra1_optical_frame: Value: true - pole_bottom_3_link: + head_camera_infra2_frame: Value: true - pole_bottom_4_link: + head_camera_infra2_optical_frame: Value: true - pole_bottom_5_link: + head_camera_link: Value: true - pole_middle_4_link: + head_pitch_link: Value: true - pole_middle_5_link: + head_yaw_link: Value: true - pole_middle_6_link: + imu_link: Value: true - pole_middle_7_link: + laser_frame: Value: true - pole_top_0_link: + mic_link: Value: true - pole_top_1_link: + plate_bottom_link: Value: true - pole_top_2_link: + plate_middle_link: Value: true - pole_top_3_link: + sim_laser_frame: Value: true - wheel_l_link: + speaker_base_link: Value: true - wheel_r_link: + tof_link: Value: true - Marker Alpha: 1 Marker Scale: 0.30000001192092896 Name: TF - Show Arrows: true + Show Arrows: false Show Axes: true - Show Names: false + Show Names: true Tree: base_footprint: base_link: - caster_b_link: + base_l_drive_wheel_link: {} - caster_f_link: + base_r_drive_wheel_link: {} - cliff_sensor_f_link: + camera_back_link: {} - cliff_sensor_l_link: + camera_front_link: {} - cliff_sensor_r_link: + docking_link: {} - gyro_link: + imu_link: {} + laser_frame: + sim_laser_frame: + {} plate_bottom_link: - lidar_base_link: - lidar_laser_mount: - lidar_laser: - {} - plate_middle_link: - {} - plate_top_link: - arm_base_link: - arm_shoulder_pan_link: - arm_shoulder_1_tilt_link: - arm_elbow_1_tilt_link: - arm_wrist_tilt_link: - hand_base_link: - hand_inner_l_link: - {} - hand_inner_r_link: - {} - hand_link: - {} - hand_outer_l_link: - hand_finger_l_link: - {} - hand_outer_r_link: - hand_finger_r_link: - {} - arm_elbow_2_tilt_link: - {} - arm_shoulder_2_tilt_link: - {} back_base_link: - head_camera_base_link: - head_camera_pan_link: - head_camera_tilt_link: - head_camera_link: - head_camera_plug_base_link: - head_camera_visor_base_link: - {} - matrix_base_link: - matrix_board_link: - matrix_sensor_1_link: - {} - matrix_sensor_2_link: - {} - matrix_sensor_3_link: - {} - matrix_sensor_4_link: - {} - matrix_sensor_5_link: - {} - matrix_sensor_6_link: - {} - matrix_sensor_7_link: - {} - matrix_sensor_8_link: - {} - mic_base_link: - mic_link: - {} - cover_link: - {} - pole_bottom_0_link: - {} - pole_bottom_1_link: - {} - pole_bottom_2_link: - {} - pole_bottom_3_link: - {} - pole_bottom_4_link: - {} - pole_bottom_5_link: - {} - pole_middle_4_link: - {} - pole_middle_5_link: - {} - pole_middle_6_link: - {} - pole_middle_7_link: - {} - pole_top_0_link: - {} - pole_top_1_link: - {} - pole_top_2_link: - {} - pole_top_3_link: - {} - wheel_l_link: - {} - wheel_r_link: + head_base_link: + head_yaw_link: + head_pitch_link: + head_camera_base_link: + head_camera_bottom_screw_frame: + head_camera_link: + head_camera_color_frame: + head_camera_color_optical_frame: + {} + head_camera_depth_frame: + head_camera_depth_optical_frame: + {} + head_camera_infra1_frame: + head_camera_infra1_optical_frame: + {} + head_camera_infra2_frame: + head_camera_infra2_optical_frame: + {} + mic_link: + {} + speaker_base_link: + {} + plate_middle_link: + arm_platform_link: + arm_base_link: + arm_shoulder_roll_link: + arm_shoulder_pitch_link: + arm_elbow_pitch_link: + arm_forearm_roll_link: + arm_wrist_pitch_link: + arm_wrist_roll_link: + arm_hand_link: + hand_sub_l_link: + hand_link: + hand_finger_tip_l_link: + {} + hand_sub_r_link: + hand_sub_link: + hand_finger_tip_r_link: + {} + hand_camera_base_link: + hand_camera_bottom_screw_frame: + hand_camera_link: + hand_camera_color_frame: + hand_camera_color_optical_frame: + {} + hand_camera_depth_frame: + hand_camera_depth_optical_frame: + {} + hand_camera_infra1_frame: + hand_camera_infra1_optical_frame: + {} + hand_camera_infra2_frame: + hand_camera_infra2_optical_frame: + {} + hand_end_effector_link: + {} + arm_shoulder_pitch_sub_link: + {} + tof_link: {} Update Interval: 0 Value: true Enabled: true Global Options: - Background Color: 85; 170; 255 - Default Light: true + Background Color: 225; 225; 225 Fixed Frame: base_footprint Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 2.118802547454834 + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 2.6829326152801514 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.75 Focal Point: - X: -0.2128812074661255 - Y: -0.21394094824790955 - Z: 0.3757147192955017 + X: 0 + Y: 0 + Z: 0 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6349996328353882 + Pitch: 0.7900000214576721 Target Frame: - Yaw: 0.859999418258667 + Value: ThirdPersonFollower (rviz_default_plugins) + Yaw: 0.7900000214576721 Saved: ~ Window Geometry: Displays: - collapsed: true + collapsed: false Height: 1016 - Hide Left Dock: true + Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000017500000362fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003900000362000000c500fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000362fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000039000003620000009d00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002c100fffffffb0000000800540069006d006501000000000000045000000000000000000000073a0000036200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001ab000003a6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000039000003a6000000c500fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003a6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000039000003a60000009d00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000589000003a600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false - Time: - collapsed: false Tool Properties: collapsed: false Views: diff --git a/sobit_edu_description/urdf/arm/arm.gazebo.xacro b/sobit_edu_description/urdf/arm/arm.gazebo.xacro new file mode 100644 index 0000000..aa7da5d --- /dev/null +++ b/sobit_edu_description/urdf/arm/arm.gazebo.xacro @@ -0,0 +1,36 @@ + + + + + false + 1.0 + 1.0 + + + false + 1.0 + 1.0 + + + false + 1.0 + 1.0 + + + false + 1.0 + 1.0 + + + false + 1.0 + 1.0 + + + + arm_shoulder_pan_joint, arm_shoulder_1_tilt_joint, arm_elbow_1_tilt_joint, arm_wrist_tilt_joint + 1000.0 + + + + \ No newline at end of file diff --git a/sobit_edu_description/urdf/arm/arm.transmission.xacro b/sobit_edu_description/urdf/arm/arm.transmission.xacro index 6201078..48a596c 100644 --- a/sobit_edu_description/urdf/arm/arm.transmission.xacro +++ b/sobit_edu_description/urdf/arm/arm.transmission.xacro @@ -1,11 +1,17 @@ - - - - - - + + + + + + + + + + + + \ No newline at end of file diff --git a/sobit_edu_description/urdf/arm/arm.urdf.xacro b/sobit_edu_description/urdf/arm/arm.urdf.xacro index 7dc97ec..cc3f7d5 100644 --- a/sobit_edu_description/urdf/arm/arm.urdf.xacro +++ b/sobit_edu_description/urdf/arm/arm.urdf.xacro @@ -1,36 +1,38 @@ - - + + - + + - + + - - - + + + @@ -45,24 +47,26 @@ - + - + - + + - + + - - - + + + @@ -70,80 +74,86 @@ - + + - + - + - + + - - + + - + - - + + + + + - + - + + - - + + - + - + - - + \ No newline at end of file diff --git a/sobit_edu_description/urdf/back/back.urdf.xacro b/sobit_edu_description/urdf/back/back.urdf.xacro index fe1d81b..3d1ee8e 100644 --- a/sobit_edu_description/urdf/back/back.urdf.xacro +++ b/sobit_edu_description/urdf/back/back.urdf.xacro @@ -13,22 +13,23 @@ - + - + + - - + + diff --git a/sobit_edu_description/urdf/back/pole.urdf.xacro.backup b/sobit_edu_description/urdf/back/pole.urdf.xacro.backup new file mode 100644 index 0000000..0417155 --- /dev/null +++ b/sobit_edu_description/urdf/back/pole.urdf.xacro.backup @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/sobit_edu_description/urdf/base/base.urdf.xacro b/sobit_edu_description/urdf/base/base.urdf.xacro index f2624fe..fdea435 100644 --- a/sobit_edu_description/urdf/base/base.urdf.xacro +++ b/sobit_edu_description/urdf/base/base.urdf.xacro @@ -2,13 +2,13 @@ - - + @@ -26,12 +26,11 @@ - - + @@ -52,80 +51,69 @@ iyy = 1/12 * m * (3 * r^2 + h^2) izz = 1/2 * m * r^2 --> - + - - - - + - + - + - - - - + - + - + - - - + - + - - + @@ -140,15 +128,12 @@ izz="0.001" /> - - - + - + - - + @@ -163,8 +148,6 @@ izz="0.001" /> - - @@ -172,70 +155,97 @@ - - + - - - + - + - - + - + - - - + - + - - + - + - - - + - + - - + - + - - + + + true + 1.0 + 1.0 + + + true + 0.0 + 0.0 + + + true + 0.0 + 0.0 + + + true + 1.0 + 1.0 + + + true + 1.0 + 1.0 + + + + + wheel_l_joint + wheel_r_joint + 0.23 + 0.070 + 1.0 + 20 + /mobile_base/commands/velocity + odom + odom + base_footprint + 1 + true + true + true + false + 50.0 + + \ No newline at end of file diff --git a/sobit_edu_description/urdf/body/cover.urdf.xacro b/sobit_edu_description/urdf/body/cover.urdf.xacro index e3b8339..af17db5 100644 --- a/sobit_edu_description/urdf/body/cover.urdf.xacro +++ b/sobit_edu_description/urdf/body/cover.urdf.xacro @@ -12,20 +12,24 @@ - + + - + + - - - + + + diff --git a/sobit_edu_description/urdf/body/stacks.urdf.xacro b/sobit_edu_description/urdf/body/stacks.urdf.xacro index 0047cc2..d584a81 100644 --- a/sobit_edu_description/urdf/body/stacks.urdf.xacro +++ b/sobit_edu_description/urdf/body/stacks.urdf.xacro @@ -3,7 +3,7 @@ Hexagon stacks --> - + @@ -22,7 +22,7 @@ - + @@ -54,7 +54,7 @@ - + @@ -86,7 +86,7 @@ - + @@ -117,7 +117,7 @@ - + @@ -159,7 +159,7 @@ - + @@ -197,13 +197,14 @@ - + - + + @@ -233,13 +234,14 @@ - + - + + diff --git a/sobit_edu_description/urdf/body/stacks.urdf.xacro.backup b/sobit_edu_description/urdf/body/stacks.urdf.xacro.backup new file mode 100644 index 0000000..1fdf062 --- /dev/null +++ b/sobit_edu_description/urdf/body/stacks.urdf.xacro.backup @@ -0,0 +1,267 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/sobit_edu_description/urdf/common.xacro b/sobit_edu_description/urdf/common.xacro index b1ca205..333af3d 100644 --- a/sobit_edu_description/urdf/common.xacro +++ b/sobit_edu_description/urdf/common.xacro @@ -2,19 +2,32 @@ - + transmission_interface/SimpleTransmission - PositionJointInterface + hardware_interface/PositionJointInterface - PositionJointInterface + hardware_interface/PositionJointInterface ${reduction} - + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + \ No newline at end of file diff --git a/sobit_edu_description/urdf/controllers.urdf.xacro b/sobit_edu_description/urdf/controllers.urdf.xacro new file mode 100644 index 0000000..88bbc5c --- /dev/null +++ b/sobit_edu_description/urdf/controllers.urdf.xacro @@ -0,0 +1,335 @@ + + + + + + + + + + + ign_ros2_control/IgnitionSystem + + + + dynamixel_hardware/DynamixelHardware + /dev/ttyUSB1 + 1000000 + + + + + + + + 1 + + + -3.141592654 + 3.141592654 + + + + 0.0 + + + + + + + 2 + + + + + -1.570796327 + 1.570796327 + + + + 1.57 + + + + + + + + + 3 + + + -1.570796327 + 1.570796327 + + + + 0.0 + + + + + + + + 4 + + + -1.570796327 + 1.570796327 + + + + 0.0 + + + + + + + + 5 + + + -1.570796327 + 1.570796327 + + + + 0.0 + + + + + + + + 6 + + + -1.570796327 + 1.570796327 + + + + 0.0 + + + + + + + + 7 + + + -1.570796327 + 1.570796327 + + + + 0.0 + + + + + + + + 8 + + + -1.570796327 + 1.570796327 + + + + 0.0 + + + + + + + + 9 + + + -1.570796327 + 1.570796327 + + + + 0.0 + + + + + + + + 10 + + + -1.570796327 + 1.570796327 + + + + 0.0 + + + + + + + + + + 8 + + + + + 0.0 + + + + + + + + + + 11 + + + -3.141592654 + 3.141592654 + + + + 0.0 + + + + + + + 12 + + + -1.570796327 + 1.570796327 + + + + 0.0 + + + + + + + + + + + ign_ros2_control/IgnitionSystem + + + + + + -1.0 + 1.0 + + + + + + + -1.0 + 1.0 + + + + + + + + 0.0 + 0.0 + + + + + + + + + + + ${robot_name} + + $(find sobit_edu_control)/config/controllers_gz.yaml + + + + + + + \ No newline at end of file diff --git a/sobit_edu_description/urdf/gazebo.urdf.xacro b/sobit_edu_description/urdf/gazebo.urdf.xacro new file mode 100644 index 0000000..aa60e7e --- /dev/null +++ b/sobit_edu_description/urdf/gazebo.urdf.xacro @@ -0,0 +1,375 @@ + + + + + + + + + + 0 0 0 0 0 0 + + ${robot_name}/imu_link + ${robot_name}/imu + + 1 + true + 1 + + + + + + + + + + + + + 0 0 ${0.0175/2} ${M_PI/2} -${M_PI/2} 0 + + ${robot_name}/base_front_camera_color + + ${robot_name}/base_front_camera/color + + 1 + true + 1 + + + 1.21126 + + 640 + 480 + RGB_INT8 + + + 0.1 + 10 + + + + + + + + + + + + + + + + 0 0 ${0.0175/2} ${M_PI/2} -${M_PI/2} 0 + + ${robot_name}/camera_back_link + + ${robot_name}/base_back_camera/color + + 1 + true + 1 + + + 1.21126 + + 640 + 480 + RGB_INT8 + + + 0.1 + 10 + + + + + + + + + + + + + 0 0 ${0.0175/2} ${M_PI/2} -${M_PI/2} 0 + + ${robot_name}/head_camera_color_optical_frame + + ${robot_name}/head_camera/color + + 1 + true + 1 + + + 1.21126 + + 640 + 480 + RGB_INT8 + + + 0.1 + 10 + + + + + + + + + + + + + 0 0 ${0.0175/2} ${M_PI/2} -${M_PI/2} 0 + + ${robot_name}/head_camera_depth_optical_frame + + ${robot_name}/head_camera/depth + + 1 + true + 1 + + + 1.48702 + + 1280 + 720 + + + 0.1 + 10 + + + + + + + + + + + + + 0 0 ${0.0175/2} ${M_PI/2} -${M_PI/2} 0 + + ${robot_name}/hand_camera_color_optical_frame + + ${robot_name}/hand_camera/color + + 1 + true + 1 + + + 1.21126 + + 640 + 480 + RGB_INT8 + + + 0.1 + 10 + + + + + + + + + + + + + 0 0 ${0.0175/2} ${M_PI/2} -${M_PI/2} 0 + + ${robot_name}/hand_camera_depth_optical_frame + + ${robot_name}/hand_camera/depth + + 1 + true + 1 + + + 1.48702 + + 1280 + 720 + + + 0.1 + 10 + + + + + + + + + + + + + 0 0 0 0 0 0 + + ${robot_name}/sim_laser_frame + + ${robot_name}/lidar/scan + + 1 + true + 10 + + + + + 640 + 1 + -2.4870942 + 2.4870942 + + + 1 + 0.01 + 0 + 0 + + + + 0.10000000149011612 + 10.0 + 0.01 + + + + + + + + + + + + + + + ${robot_name}/joint_states + + + + + diff --git a/sobit_edu_description/urdf/hand/hand.gazebo.xacro b/sobit_edu_description/urdf/hand/hand.gazebo.xacro new file mode 100644 index 0000000..25d5f71 --- /dev/null +++ b/sobit_edu_description/urdf/hand/hand.gazebo.xacro @@ -0,0 +1,100 @@ + + + + + false + 1.0 + 1.0 + + + + + hand_joint + 1000.0 + + + + + + + + ${parent_joint} + ${mimic_joint} + + + + ${multiplier} + ${offset} + ${sensitiveness} + ${max_effort} + + ${robot_namespace} + + + + + + + + + + /$(arg robot_namespace) + gazebo_ros_control/DefaultRobotHWSim + true + + + + + + + + + + + + + + + + + diff --git a/sobit_edu_description/urdf/hand/hand.transmission.xacro b/sobit_edu_description/urdf/hand/hand.transmission.xacro index e303349..417b0f1 100644 --- a/sobit_edu_description/urdf/hand/hand.transmission.xacro +++ b/sobit_edu_description/urdf/hand/hand.transmission.xacro @@ -1,12 +1,13 @@ - + - - - - - - - + + + - \ No newline at end of file + diff --git a/sobit_edu_description/urdf/hand/hand.urdf.xacro b/sobit_edu_description/urdf/hand/hand.urdf.xacro index c2cc1f0..3d7b9e1 100644 --- a/sobit_edu_description/urdf/hand/hand.urdf.xacro +++ b/sobit_edu_description/urdf/hand/hand.urdf.xacro @@ -1,250 +1,225 @@ - - + - + - + - + - + - + - - + + - - - + + - - - - - - - - - - - - - + - + - + - - + + - - + - - - + + + - - - + - + - + - - + + - - - - - + + + + - - + - + - + - + - + - + - - - + + + - - - - - + + + + - - + - + - + - + - + - + - - - + + + - - - - - - - - + + + + + + - + - + - + - + - + - - - + + + - - - - - - + + + + + - - + - + - + - + - + - - + + - + \ No newline at end of file diff --git a/sobit_edu_description/urdf/hand/hand.urdf.xacro.backup b/sobit_edu_description/urdf/hand/hand.urdf.xacro.backup new file mode 100644 index 0000000..57c7ec6 --- /dev/null +++ b/sobit_edu_description/urdf/hand/hand.urdf.xacro.backup @@ -0,0 +1,231 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/sobit_edu_description/urdf/head/head.gazebo.xacro b/sobit_edu_description/urdf/head/head.gazebo.xacro new file mode 100644 index 0000000..6fd8786 --- /dev/null +++ b/sobit_edu_description/urdf/head/head.gazebo.xacro @@ -0,0 +1,27 @@ + + + + + false + 1.0 + 1.0 + + + false + 1.0 + 1.0 + + + false + 1.0 + 1.0 + + + + head_camera_pan_joint, head_camera_tilt_joint + 1000.0 + + + + + \ No newline at end of file diff --git a/sobit_edu_description/urdf/head/head.transmission.xacro b/sobit_edu_description/urdf/head/head.transmission.xacro index a5adb6e..6ddb95e 100644 --- a/sobit_edu_description/urdf/head/head.transmission.xacro +++ b/sobit_edu_description/urdf/head/head.transmission.xacro @@ -1,7 +1,7 @@ - - + + \ No newline at end of file diff --git a/sobit_edu_description/urdf/head/head.urdf.xacro b/sobit_edu_description/urdf/head/head.urdf.xacro index 5bbd4cf..13aad93 100644 --- a/sobit_edu_description/urdf/head/head.urdf.xacro +++ b/sobit_edu_description/urdf/head/head.urdf.xacro @@ -1,6 +1,7 @@ + @@ -14,22 +15,24 @@ - + + - + + - - - + + + @@ -46,22 +49,24 @@ - + + - + + - - - + + + @@ -78,27 +83,30 @@ - + + - + + - - - + + + + diff --git a/sobit_edu_description/urdf/sensor/head_camera.urdf.xacro b/sobit_edu_description/urdf/sensor/head_camera.urdf.xacro index 8139fb3..b6f3211 100644 --- a/sobit_edu_description/urdf/sensor/head_camera.urdf.xacro +++ b/sobit_edu_description/urdf/sensor/head_camera.urdf.xacro @@ -11,10 +11,10 @@ - + \ No newline at end of file diff --git a/sobit_edu_description/urdf/sensor/lidar.urdf.xacro b/sobit_edu_description/urdf/sensor/lidar.urdf.xacro index 17e6307..2838b83 100644 --- a/sobit_edu_description/urdf/sensor/lidar.urdf.xacro +++ b/sobit_edu_description/urdf/sensor/lidar.urdf.xacro @@ -13,22 +13,24 @@ - + + - + + - - - + + + diff --git a/sobit_edu_description/urdf/sensor/matrix.urdf.xacro b/sobit_edu_description/urdf/sensor/matrix.urdf.xacro index a1b925e..3fc4448 100644 --- a/sobit_edu_description/urdf/sensor/matrix.urdf.xacro +++ b/sobit_edu_description/urdf/sensor/matrix.urdf.xacro @@ -16,22 +16,23 @@ - + - + + - - + + @@ -47,22 +48,24 @@ - + - + + - + + - - - + + + diff --git a/sobit_edu_description/urdf/sensor/matrix.urdf.xacro.backup b/sobit_edu_description/urdf/sensor/matrix.urdf.xacro.backup new file mode 100644 index 0000000..9104d0d --- /dev/null +++ b/sobit_edu_description/urdf/sensor/matrix.urdf.xacro.backup @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/sobit_edu_description/urdf/sensor/mic.urdf.xacro b/sobit_edu_description/urdf/sensor/mic.urdf.xacro index c0e1cec..74d77ce 100644 --- a/sobit_edu_description/urdf/sensor/mic.urdf.xacro +++ b/sobit_edu_description/urdf/sensor/mic.urdf.xacro @@ -12,23 +12,24 @@ - + - + + - - - + + + @@ -43,23 +44,25 @@ - + + - + + - - - + + + diff --git a/sobit_edu_description/urdf/sensor/mic.urdf.xacro.backup b/sobit_edu_description/urdf/sensor/mic.urdf.xacro.backup new file mode 100644 index 0000000..e843eb7 --- /dev/null +++ b/sobit_edu_description/urdf/sensor/mic.urdf.xacro.backup @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/sobit_edu_description/urdf/sensor/urg.urdf.xacro.backup b/sobit_edu_description/urdf/sensor/urg.urdf.xacro.backup new file mode 100644 index 0000000..c6ee78e --- /dev/null +++ b/sobit_edu_description/urdf/sensor/urg.urdf.xacro.backup @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sobit_edu_description/urdf/sensor/xtion.urdf.xacro.backup b/sobit_edu_description/urdf/sensor/xtion.urdf.xacro.backup new file mode 100644 index 0000000..45fd893 --- /dev/null +++ b/sobit_edu_description/urdf/sensor/xtion.urdf.xacro.backup @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/sobit_edu_description/urdf/transmission.urdf.xacro b/sobit_edu_description/urdf/transmission.urdf.xacro new file mode 100644 index 0000000..9cf1736 --- /dev/null +++ b/sobit_edu_description/urdf/transmission.urdf.xacro @@ -0,0 +1,45 @@ + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + ${reduction} + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + ${reduction} + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + ${reduction} + + + + + \ No newline at end of file diff --git a/sobit_edu_library/CMakeLists.txt b/sobit_edu_library/CMakeLists.txt index 08db079..5fca34a 100644 --- a/sobit_edu_library/CMakeLists.txt +++ b/sobit_edu_library/CMakeLists.txt @@ -1,107 +1,31 @@ -cmake_minimum_required(VERSION 3.10.2) +cmake_minimum_required(VERSION 3.8) project(sobit_edu_library) -cmake_policy(SET CMP0002 OLD) -set(ALLOW_DUPLICATE_CUSTOM_TARGETS TRUE) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -## Compile as C++14, supported in ROS Noetic and newer -add_compile_options(-std=c++14) +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - dynamixel_sdk - roscpp - rospy - std_msgs - tf2 - tf2_ros - tf2_geometry_msgs - trajectory_msgs - geometry_msgs - nav_msgs - pybind11_catkin - sobits_msgs +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME} ) - - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS - dynamixel_sdk - roscpp - rospy - std_msgs - tf2 - tf2_ros - tf2_geometry_msgs - trajectory_msgs - geometry_msgs - nav_msgs - pybind11_catkin - sobits_msgs -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - - -add_library(${PROJECT_NAME} src/sobit_edu_joint_controller.cpp - src/sobit_edu_wheel_controller.cpp) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - -pybind_add_module(sobit_edu_module MODULE src/sobit_edu_library.cpp - src/sobit_edu_joint_controller.cpp - src/sobit_edu_wheel_controller.cpp) -add_dependencies(sobit_edu_module ${catkin_EXPORTED_TARGETS}) - -add_executable(test_control_arm example/test_control_arm.cpp - src/sobit_edu_joint_controller.cpp - src/sobit_edu_wheel_controller.cpp) -target_link_libraries(test_control_arm ${catkin_LIBRARIES} ${PYTHON_LIBRARIES}) -add_dependencies(test_control_arm ${catkin_EXPORTED_TARGETS}) - -add_executable(test_control_head example/test_control_head.cpp - src/sobit_edu_joint_controller.cpp - src/sobit_edu_wheel_controller.cpp) -target_link_libraries(test_control_head ${catkin_LIBRARIES} ${PYTHON_LIBRARIES}) -add_dependencies(test_control_head ${catkin_EXPORTED_TARGETS}) - -add_executable(test_control_wheel example/test_control_wheel.cpp - src/sobit_edu_joint_controller.cpp - src/sobit_edu_wheel_controller.cpp) -target_link_libraries(test_control_wheel ${catkin_LIBRARIES} ${PYTHON_LIBRARIES}) -add_dependencies(test_control_wheel ${catkin_EXPORTED_TARGETS}) - -add_executable(test_grasp example/test_grasp.cpp - src/sobit_edu_joint_controller.cpp - src/sobit_edu_wheel_controller.cpp) -target_link_libraries(test_grasp ${catkin_LIBRARIES} ${PYTHON_LIBRARIES}) -add_dependencies(test_grasp ${catkin_EXPORTED_TARGETS}) - -add_executable(test_place example/test_place.cpp - src/sobit_edu_joint_controller.cpp - src/sobit_edu_wheel_controller.cpp) -target_link_libraries(test_place ${catkin_LIBRARIES} ${PYTHON_LIBRARIES}) -add_dependencies(test_place ${catkin_EXPORTED_TARGETS}) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/sobit_edu_library/config/pose_list.yaml b/sobit_edu_library/config/pose_list.yaml new file mode 100644 index 0000000..be6ff8a --- /dev/null +++ b/sobit_edu_library/config/pose_list.yaml @@ -0,0 +1,12 @@ +sobit_xxx_pose_list: + ros__parameters: + initial_pose: + arm_shoulder_roll : 0.0 + arm_shoulder_pitch : 0.0 + arm_elbow_pitch : 0.0 + arm_forearm_roll : 0.0 + arm_wrist_pitch : 0.0 + arm_wrist_roll : 0.0 + hand : 0.0 + head_yaw : 0.0 + head_pitch : 0.0 diff --git a/sobit_edu_library/config/sobit_edu_pose.yaml b/sobit_edu_library/config/sobit_edu_pose.yaml deleted file mode 100644 index 94c1d9e..0000000 --- a/sobit_edu_library/config/sobit_edu_pose.yaml +++ /dev/null @@ -1,41 +0,0 @@ -sobit_edu_pose: - - { - pose_name: "initial_pose", - arm_shoulder_pan_joint: 0.00, - arm_shoulder_1_tilt_joint: 1.5708, - arm_elbow_1_tilt_joint: -1.40, - arm_wrist_tilt_joint: -0.17, - hand_joint: -1.00, - head_camera_pan_joint: 0.00, - head_camera_tilt_joint: 0.00 - } - - { - pose_name: "pan_pose", - arm_shoulder_pan_joint: 0.00, - arm_shoulder_1_tilt_joint: 1.5708, - arm_elbow_1_tilt_joint: -1.40, - arm_wrist_tilt_joint: -0.17, - hand_joint: -1.00, - head_camera_pan_joint: -0.00, - head_camera_tilt_joint: -1.00 - } - - { - pose_name: "detecting_pose", - arm_shoulder_pan_joint: 0.00, - arm_shoulder_1_tilt_joint: 0.00, - arm_elbow_1_tilt_joint: 0.00, - arm_wrist_tilt_joint: 0.00, - hand_joint: 0.00, - head_camera_pan_joint: 0.00, - head_camera_tilt_joint: -0.53 - } - - { - pose_name: "following_pose", - arm_shoulder_pan_joint: -1.5708, - arm_shoulder_1_tilt_joint: 1.5708, - arm_elbow_1_tilt_joint: -1.40, - arm_wrist_tilt_joint: -0.17, - hand_joint: -1.00, - head_camera_pan_joint: 0.00, - head_camera_tilt_joint: 0.00 - } \ No newline at end of file diff --git a/sobit_edu_library/example/test_control_arm.cpp b/sobit_edu_library/example/test_control_arm.cpp deleted file mode 100644 index cb3d0ae..0000000 --- a/sobit_edu_library/example/test_control_arm.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include -#include "sobit_edu_library/sobit_edu_joint_controller.hpp" - - -int main( int argc, char *argv[] ){ - ros::init(argc, argv, "sobit_edu_test_control_arm"); - - sobit_edu::SobitEduJointController edu_joint_ctrl; - - // Move all the arm joints - edu_joint_ctrl.moveArm( 0.0, -1.5708, 1.5708, 0.0, 1.0 ); - - // Open the hand - edu_joint_ctrl.moveJoint( sobit_edu::Joint::HAND_JOINT, -1.57, 5.0, true ); - - // Set the initial pose - edu_joint_ctrl.moveToPose( "initial_pose", 5.0); - - return 0; -} \ No newline at end of file diff --git a/sobit_edu_library/example/test_control_arm.py b/sobit_edu_library/example/test_control_arm.py deleted file mode 100755 index 1dfb091..0000000 --- a/sobit_edu_library/example/test_control_arm.py +++ /dev/null @@ -1,31 +0,0 @@ -#!/usr/bin/env python3 -#coding: utf-8 - -import sys -import rospy -from sobit_edu_module import SobitEduJointController -from sobit_edu_module import Joint - -def test_control_arm(): - rospy.init_node('sobit_edu_test_control_arm') - - args = sys.argv - edu_joint_ctrl = SobitEduJointController(args[0]) # args[0] : C++上でros::init()を行うための引数 - - # Move all the arm joints - edu_joint_ctrl.moveArm( 0.0, -1.5708, 1.5708, 0.0, 1.0 ) - - # Close the gripper - edu_joint_ctrl.moveJoint( Joint.HAND_JOINT, 0, 2.0, True ) - - # Set the initial pose - edu_joint_ctrl.moveToPose( "initial_pose", 5.0) - - del edu_joint_ctrl - del args - -if __name__ == '__main__': - try: - test_control_arm() - except rospy.ROSInterruptException: - pass \ No newline at end of file diff --git a/sobit_edu_library/example/test_control_head.cpp b/sobit_edu_library/example/test_control_head.cpp deleted file mode 100644 index f825ebd..0000000 --- a/sobit_edu_library/example/test_control_head.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include -#include "sobit_edu_library/sobit_edu_joint_controller.hpp" - - -int main( int argc, char *argv[] ){ - ros::init(argc, argv, "sobit_edu_test_control_head"); - - sobit_edu::SobitEduJointController edu_joint_ctrl; - - const double MAX_ANGLE = 1.57; - const double MIN_ANGLE = -1.57; - double target_angle = 0.0; - double increment = 0.05; - - while( ros::ok() ){ - target_angle += increment; - if (target_angle > MAX_ANGLE || target_angle < MIN_ANGLE) increment *= -1.0; - - // Option 1: Move the head joints simultaneously - edu_joint_ctrl.moveHeadPanTilt( target_angle, target_angle, 0.5, false ); - ros::Duration(0.5).sleep(); - - /*** - // Option 2: Move the head joints individually - edu_joint_ctrl.moveJoint( sobit_edu::Joint::HEAD_PAN_JOINT , target_angle, 0.5, false ); - edu_joint_ctrl.moveJoint( sobit_edu::Joint::HEAD_TILT_JOINT, target_angle, 0.5, false ); - ***/ - } - - - return 0; -} \ No newline at end of file diff --git a/sobit_edu_library/example/test_control_head.py b/sobit_edu_library/example/test_control_head.py deleted file mode 100755 index 635eab6..0000000 --- a/sobit_edu_library/example/test_control_head.py +++ /dev/null @@ -1,56 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - -import sys -import rospy -from sobit_edu_module import SobitEduJointController -from sobit_edu_module import Joint - - -def test_control_head(): - rospy.init_node('sobit_edu_test_control_head') - - args = sys.argv - edu_joint_ctrl = SobitEduJointController(args[0]) - - # r = rospy.Rate(1) # 10hz - # ang = 0.8 - # args = sys.argv - # edu_pantilt_ctr = SobitEduController(args[0]) # args[0] : C++上でros::init()を行うための引数 - - # while not rospy.is_shutdown(): - # ang = -1.0 * ang - - # # カメラパンチルトを動かす - # edu_pantilt_ctr.moveJoint( Joint.HEAD_CAMERA_PAN_JOINT, ang, 2.0, False ) - # r.sleep() - - MAX_ANGLE = 1.57 - MIN_ANGLE = -1.57 - target_angle = 0.0 - increment = 0.05 - - while not rospy.is_shutdown(): - print(target_angle) - target_angle += increment - if target_angle > MAX_ANGLE or target_angle < MIN_ANGLE: - increment *= -1.0 - - # Option 1: Move the head joints simultaneously - edu_joint_ctrl.moveHeadPanTilt( target_angle, target_angle, 0.5, False ) - rospy.sleep(0.5) - - """ - # Option 2: Move the head joints individually - edu_joint_ctrl.moveJoint( Joint.HEAD_CAMERA_PAN_JOINT , target_angle, 0.5, True ) - edu_joint_ctrl.moveJoint( Joint.HEAD_CAMERA_TILT_JOINT, target_angle, 0.5, True ) - """ - - del pro_joint_ctrl - del args - -if __name__ == '__main__': - try: - test_control_head() - except rospy.ROSInterruptException: - pass \ No newline at end of file diff --git a/sobit_edu_library/example/test_control_wheel.cpp b/sobit_edu_library/example/test_control_wheel.cpp deleted file mode 100644 index 18a166b..0000000 --- a/sobit_edu_library/example/test_control_wheel.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#include -#include "sobit_edu_library/sobit_edu_wheel_controller.hpp" - -int main( int argc, char *argv[] ){ - ros::init(argc, argv, "sobit_edu_test_control_wheel"); - - sobit_edu::SobitEduWheelController edu_wheel_ctrl; - - // Move the wheels (linear motion) - edu_wheel_ctrl.controlWheelLinear(1.5); - ros::Duration(3.0).sleep(); - - // Move the wheels (rotational motion: Radian) - edu_wheel_ctrl.controlWheelRotateRad(1.5708); - ros::Duration(3.0).sleep(); - - // Move the wheels (rotational motion: Degree) - edu_wheel_ctrl.controlWheelRotateDeg(-90); - ros::Duration(3.0).sleep(); - - // Move the wheels (linear motion) - edu_wheel_ctrl.controlWheelLinear(-1.5); - ros::Duration(3.0).sleep(); - - return 0; -} - - - - - - - - - diff --git a/sobit_edu_library/example/test_control_wheel.py b/sobit_edu_library/example/test_control_wheel.py deleted file mode 100755 index aad3004..0000000 --- a/sobit_edu_library/example/test_control_wheel.py +++ /dev/null @@ -1,58 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - -import sys -import numpy as np -import math -import rospy -from sobit_edu_module import SobitEduWheelController -from geometry_msgs.msg import Twist - -def test_control_wheel(): - rospy.init_node('sobit_edu_test_control_wheel') - - pub = rospy.Publisher("/mobile_base/commands/velocity",Twist,queue_size=1) - vel = Twist() - vel.linear.x = 0.1 - - for angular in np.arange(-math.pi/2, math.pi/2, 0.01): - vel.angular.z = angular - print("linear = " + str(vel.linear.x) + "\tangular = " + str(vel.angular.z)) - pub.publish(vel) - rospy.sleep(0.1) - - for angular in np.arange(math.pi/2, -math.pi/2, 0.01): - vel.angular.z = angular - print("linear = " + str(vel.linear.x) + "\tangular = " + str(vel.angular.z)) - pub.publish(vel) - rospy.sleep(0.1) - - zero_vel = Twist() - pub.publish(zero_vel) - - """ - args = sys.argv - edu_wheel_ctrl = SobitEduWheelController(args[0]) # args[0] : C++上でros::init()を行うための引数 - - # Move the wheels (linear motion) - edu_wheel_ctrl.controlWheelLinear(1.5) - - # Move the wheels (rotational motion: Radian) - edu_wheel_ctrl.controlWheelRotateRad(1.5708) - - # Move the wheels (rotational motion: Degree) - edu_wheel_ctrl.controlWheelRotateDeg(-90) - - # Move the wheels (linear motion) - edu_wheel_ctrl.controlWheelLinear(-1.5) - - del edu_wheel_ctrl - del args - """ - - -if __name__ == '__main__': - try: - test_control_wheel() - except rospy.ROSInterruptException: - pass diff --git a/sobit_edu_library/example/test_grasp.cpp b/sobit_edu_library/example/test_grasp.cpp deleted file mode 100644 index 296ba10..0000000 --- a/sobit_edu_library/example/test_grasp.cpp +++ /dev/null @@ -1,50 +0,0 @@ -#include -#include "sobit_edu_library/sobit_edu_joint_controller.hpp" - - -int main( int argc, char *argv[] ){ - ros::init(argc, argv, "sobit_edu_test_grasp_on_floor"); - - sobit_edu::SobitEduJointController edu_joint_ctrl; - - std::string target_name = "potato_chips"; - bool is_done = false; - - // Set the detecting_pose - edu_joint_ctrl.moveToPose("detecting_pose", 5.0); - - // Open the hand - edu_joint_ctrl.moveJoint( sobit_edu::Joint::HAND_JOINT, -1.57, 5.0, true ); - - // Option 1: Grasp the target on the given TF position - // Move the hand to the target position - is_done = (edu_joint_ctrl.moveHandToTargetTF( target_name, -0.15,0.0,0.05 )) - // Close the hand - && (edu_joint_ctrl.moveJoint( sobit_edu::Joint::HAND_JOINT, 0.0, 5.0, true )) - // Check if grasped based on the force sensor - && (edu_joint_ctrl.graspDecision( 300, 1000 )); - - /*** - // Option 2: Grasp the target on the given coordinates (x,y,z) position - // Check if grasped based on the force sensor - is_done = (edu_joint_ctrl.moveHandToTargetCoord( 0.0,0.0,0.0, -0.15,0.0,0.05 )) - // Close the hand - && (edu_joint_ctrl.moveJoint( sobit_edu::Joint::HAND_JOINT, 0.0, 5.0, true )) - // Check if grasped based on the force sensor - && (edu_joint_ctrl.graspDecision( 300, 1000 )); - ***/ - - std::cout << "Is "<< target_name << " grasped? " << is_done << std::endl; - - if( is_done ){ - // Set the put_high_pose pose to avoid collision - edu_joint_ctrl.moveToPose("detecting_pose", 5.0); - } else { - ROS_ERROR("Failed to grasp the object"); - } - - // Set the initial pose - edu_joint_ctrl.moveToPose("initial_pose", 5.0); - - return 0; -} \ No newline at end of file diff --git a/sobit_edu_library/example/test_grasp.py b/sobit_edu_library/example/test_grasp.py deleted file mode 100755 index c606d90..0000000 --- a/sobit_edu_library/example/test_grasp.py +++ /dev/null @@ -1,63 +0,0 @@ -#!/usr/bin/env python3 -#coding: utf-8 - -import sys -import rospy -from sobit_edu_module import SobitEduJointController -from sobit_edu_module import SobitEduWheelController -from sobit_edu_module import Joint - - -def test_grasp_on_floor(): - rospy.init_node('sobit_edu_test_grasp_on_floor') - - args = sys.argv - edu_joint_ctrl = SobitEduJointController(args[0]) - edu_wheel_ctrl = SobitEduWheelController(args[0]) # args[0] : C++上でros::init()を行うための引数 - - # Set the detecting pose - edu_joint_ctrl.moveToPose( "detecting_pose" ) - - # Open the gripper - edu_joint_ctrl.moveJoint( Joint.HAND_JOINT, 1.5708, 2.0, True ) - - # Option 1: Grasp the target on the given TF position - # Move the arm (inverse kinematics) - # is_done = edu_joint_ctrl.moveGripperToTargetTF(target_name, -0.3, 0.0, 0.0) - - # """ - # Option 2: Grasp the target on the given coordinates (x,y,z) position - # Move the arm (forward kinematics) - is_done = edu_joint_ctrl.moveHandToTargetCoord(0.2, 0.2, 0.75, -0.3, 0.0, 0.0) - # """ - - # Move the wheels (linear motion) - edu_wheel_ctrl.controlWheelLinear(0.3) - - # Close the gripper - edu_joint_ctrl.moveJoint( Joint.HAND_JOINT, 0.0, 2.0, True ) - - # Move the wheels (linear motion) - edu_wheel_ctrl.controlWheelLinear(-0.3) - - # Check if grasped based on the force sensor - is_done *= edu_joint_ctrl.graspDecision( 300, 1000 ) - - if( is_done ): - # Set the put_high_pose pose to avoid collision - edu_joint_ctrl.moveToPose( "detecting_pose" ) - else: - rospy.logerr("Failed to grasp the object") - - # Set the initial pose - edu_joint_ctrl.moveToPose( "initial_pose" ) - - del edu_joint_ctrl - del edu_wheel_ctrl - del args - -if __name__ == '__main__': - try: - test_grasp_on_floor() - except rospy.ROSInterruptException: - pass \ No newline at end of file diff --git a/sobit_edu_library/example/test_place.cpp b/sobit_edu_library/example/test_place.cpp deleted file mode 100644 index 43f02f5..0000000 --- a/sobit_edu_library/example/test_place.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include - -#include -#include "sobit_edu_library/sobit_edu_joint_controller.hpp" - - -int main( int argc, char *argv[] ){ - ros::init(argc, argv, "sobit_edu_test_put_on_table"); - - sobit_edu::SobitEduJointController edu_joint_ctrl; - - std::string target_name = "placeable_point"; - bool is_done = false; - - // Set the detecting_pose - edu_joint_ctrl.moveToPose( "detecting_pose", 5.0); - - // Lauch the placeable_position_estimator node - std::string package_name = "placeable_position_estimator"; - std::string node_name = "placeable_position_estimator"; - std::string launch_name = node_name+".launch"; - std::string command = "roslaunch " + package_name + " " + launch_name; - int is_launch = system(command.c_str()); - - if( is_launch == -1 ){ - ROS_ERROR("Failed to execute roslaunch command"); - return 1; - } - - ros::Duration(10.0).sleep(); - - // Option 1: Place object on the given TF position - // Arm will move down until it touches the placeable_point - is_done = edu_joint_ctrl.moveHandToPlaceTF( target_name, -0.15, 0.0, 0.2 ); - - // Option 2: Place object on the given coordinates (x,y,z) position - // Arm will move down until it touches the placeable_point - // bool res = edu_joint_ctrl.moveHandToPlaceCoord( 0.0, 0.0, 0.0, -0.15, 0.0, 0.2 ); - - if( is_done ){ - // Open the hand - edu_joint_ctrl.moveJoint( sobit_edu::Joint::HAND_JOINT, -1.57, 2.0, true ); - - // Set the put_high_pose pose to avoid collision - edu_joint_ctrl.moveToPose( "detecting_pose", 5.0); - } else{ - ROS_ERROR("Failed to place the object"); - } - - // Set the initial pose - edu_joint_ctrl.moveToPose( "initial_pose", 5.0); - - // Kill the placeable_position_estimator node - command = "rosnode kill /" + node_name + "/" + node_name + "_node"; - is_launch = system(command.c_str()); - ros::Duration(5.0).sleep(); - - if( is_launch == -1 ){ - ROS_ERROR("Failed to kill the placeable_position_estimator node"); - return 1; - } - - return 0; -} \ No newline at end of file diff --git a/sobit_edu_library/example/test_place.py b/sobit_edu_library/example/test_place.py deleted file mode 100755 index d406b8a..0000000 --- a/sobit_edu_library/example/test_place.py +++ /dev/null @@ -1,86 +0,0 @@ -#!/usr/bin/env python3 -#coding: utf-8 - -import sys -import rospy -from sobit_edu_module import SobitEduJointController -from sobit_edu_module import SobitEduWheelController -from sobit_edu_module import Joint - - -def test_place_on_table(): - rospy.init_node('sobit_edu_test_place_on_table') - - args = sys.argv - edu_joint_ctrl = SobitEduJointController(args[0]) - edu_wheel_ctrl = SobitEduWheelController(args[0]) # args[0] : C++上でros::init()を行うための引数 - - # Set the detecting_pose - edu_joint_ctrl.moveToPose( "initial_pose" ) - - """ - # Lauch the placeable_position_estimator node - package_name = "sobit_pro_bringup" - node_name = "placeable_position_estimator" - launch_name = node_name + ".launch" - comand = "roslaunch " + package_name + " " + launch_name - p = Popen(comand.split(), shell=True) - p.wait() - - if( not p.returncode ){ - rospy.logerr("Failed to launch the placeable_position_estimator node"); - } - - # Turn on detection service - rospy.wait_for_service('placeable_position_estimator/execute_ctrl') - try: - client = rospy.ServiceProxy('placeable_position_estimator/execute_ctrl', SetBool) - client(True) - except rospy.ServiceException as e: - rospy.logerr( "Failed to call service placeable_position_estimator : %s" % e ) - - rospy.sleep(10.0) - """ - - # Option 1: Place object on the given TF position - # Arm will move down until it touches the placeable_point - # is_done = edu_joint_ctrl.moveGripperToPlaceTF( "placeable_point", -0.15, 0.0, 0.2 ) - - # Option 2: Place object on the given coordinates (x,y,z) position - # Arm will move down until it touches the placeable_point - is_done = edu_joint_ctrl.moveHandToPlaceCoord( 0.0, 0.0, 0.3, -0.15, 0.0, 0.2 ) - rospy.sleep(3.0) - - if( is_done ): - # Open the gripper - edu_joint_ctrl.moveJoint( Joint.HAND_JOINT, -1.57, 2.0, True ) - - # Set the put_high_pose pose to avoid collision - edu_joint_ctrl.moveToPose( "detecting_pose") - else: - rospy.logerr( "Failed to place the object" ) - - # Set the initial pose - edu_joint_ctrl.moveToPose( "initial_pose") - - del edu_joint_ctrl - del edu_wheel_ctrl - del args - - """ - # Kill the placeable_position_estimator node - command = "rosnode kill /" + node_name + "/" + node_name + "_node" - p = Popen(command.split(), shell=True) - p.wait() - - if( not p.returncode ){ - rospy.logerr("Failed to kill the placeable_position_estimator node"); - } - - """ - -if __name__ == '__main__': - try: - test_place_on_table() - except rospy.ROSInterruptException: - pass diff --git a/sobit_edu_library/include/sobit_edu_library/.gitkeep b/sobit_edu_library/include/sobit_edu_library/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/sobit_edu_library/include/sobit_edu_library/sobit_edu_joint_controller.hpp b/sobit_edu_library/include/sobit_edu_library/sobit_edu_joint_controller.hpp deleted file mode 100644 index 0876d02..0000000 --- a/sobit_edu_library/include/sobit_edu_library/sobit_edu_joint_controller.hpp +++ /dev/null @@ -1,175 +0,0 @@ -#ifndef SOBIT_EDU_JOINT_CONTROLLER_H -#define SOBIT_EDU_JOINT_CONTROLLER_H - -#include - -// #include -#include -#include -#include -#include - -#include "sobit_edu_library/sobit_edu_library.h" -#include "sobits_msgs/current_state_array.h" -// #include "sobits_msgs/current_state.h" - -namespace sobit_edu { - enum Joint { ARM_SHOULDER_PAN_JOINT = 0, - ARM_SHOULDER_1_TILT_JOINT, - ARM_SHOULDER_2_TILT_JOINT, - ARM_ELBOW_1_TILT_JOINT, - ARM_ELBOW_2_TILT_JOINT, - ARM_WRIST_TILT_JOINT, - HAND_JOINT, - HEAD_CAMERA_PAN_JOINT, - HEAD_CAMERA_TILT_JOINT, - JOINT_NUM }; - - typedef struct { - std::string pose_name; - std::vector joint_val; - } Pose; - - class SobitEduJointController : private ROSCommonNode { - private: - - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - - ros::Publisher pub_arm_control_; - ros::Publisher pub_head_camera_control_; - ros::Subscriber sub_curr_arm; - - tf2_ros::Buffer tfBuffer_; - tf2_ros::TransformListener tfListener_; - - const std::vector joint_names_ = { "arm_shoulder_pan_joint", - "arm_shoulder_1_tilt_joint", - "arm_shoulder_2_tilt_joint", - "arm_elbow_1_tilt_joint", - "arm_elbow_2_tilt_joint", - "arm_wrist_tilt_joint", - "hand_joint", - "head_camera_pan_joint", - "head_camera_tilt_joint" }; - std::vector pose_list_; - - double arm_wrist_tilt_current_ = 0.; - double hand_current_ = 0.; - - static constexpr const double base_to_shoulder_flex_joint_z_cm = 52.2; - static constexpr const double base_to_shoulder_flex_joint_x_cm = 12.2; - static constexpr const double arm_upper_link_x_cm = 14.8; - static constexpr const double arm_upper_link_z_cm = 2.4; - static constexpr const double arm_outer_link_x_cm = 15.0; - static constexpr const double grasp_min_z_cm = 35.0; - static constexpr const double grasp_max_z_cm = 80.0; - - void setJointTrajectory( const std::string& joint_name, const double rad, const double sec, trajectory_msgs::JointTrajectory* jt ); - void addJointTrajectory( const std::string& joint_name, const double rad, const double sec, trajectory_msgs::JointTrajectory* jt ); - void checkPublishersConnection( const ros::Publisher& pub ); - void callbackCurrArm( const sobits_msgs::current_state_array& msg ); - void loadPose( ); - - public: - SobitEduJointController( const std::string &name ); - SobitEduJointController( ); - - bool moveToPose( const std::string &pose_name, - const double sec = 5.0 ); - bool moveAllJoint( const double arm_shoulder_pan, - const double arm_shoulder_tilt, - const double arm_elbow_tilt, - const double arm_wrist_tilt, - const double hand, - const double head_camera_pan, - const double head_camera_tilt, - const double sec, bool is_sleep = true ); - bool moveJoint( const Joint joint_num, - const double rad, - const double sec = 5.0, bool is_sleep = true ); - bool moveArm( const double arm_shoulder_pan, - const double arm_shoulder_tilt, - const double arm_elbow_tilt, - const double arm_wrist_tilt, - const double hand, - const double sec = 5.0, bool is_sleep = true ); - bool moveHeadPanTilt( const double pan_rad, - const double tilt_rad, - const double sec = 5.0, bool is_sleep = true ); - bool moveHandToTargetCoord( const double target_pos_x, const double target_pos_y, const double target_pos_z, - const double shift_x , const double shift_y , const double shift_z, - const double sec = 5.0, bool is_sleep = true ); - bool moveHandToTargetTF( const std::string &target_name, - const double shift_x, const double shift_y, const double shift_z, - const double sec = 5.0, bool is_sleep = true); - bool moveHandToPlaceCoord( const double target_pos_x, const double target_pos_y, const double target_pos_z, - const double shift_x , const double shift_y , const double shift_z, - const double sec = 5.0, bool is_sleep = true ); - bool moveHandToPlaceTF( const std::string& target_name, - const double shift_x, const double shift_y, const double shift_z, - const double sec = 5.0, bool is_sleep = true ); - bool graspDecision( const int min_curr = 300, const int max_curr = 1000 ); - bool placeDecision( const int min_curr = 500, const int max_curr = 1000 ); - }; -} // namespace sobit_edu - -inline void sobit_edu::SobitEduJointController::setJointTrajectory( const std::string& joint_name, - const double rad, - const double sec, - trajectory_msgs::JointTrajectory* jt ) { - trajectory_msgs::JointTrajectory joint_trajectory; - trajectory_msgs::JointTrajectoryPoint joint_trajectory_point; - - joint_trajectory.joint_names.push_back( joint_name ); - joint_trajectory_point.positions.push_back( rad ); - joint_trajectory_point.velocities.push_back( 0.0 ); - joint_trajectory_point.accelerations.push_back( 0.0 ); - joint_trajectory_point.effort.push_back( 0.0 ); - joint_trajectory_point.time_from_start = ros::Duration( sec ); - joint_trajectory.points.push_back( joint_trajectory_point ); - - *jt = joint_trajectory; - - return; -} - -inline void sobit_edu::SobitEduJointController::addJointTrajectory( const std::string& joint_name, - const double rad, - const double sec, - trajectory_msgs::JointTrajectory* jt ) { - trajectory_msgs::JointTrajectory joint_trajectory = *jt; - - joint_trajectory.joint_names.push_back( joint_name ); - joint_trajectory.points[0].positions.push_back( rad ); - joint_trajectory.points[0].velocities.push_back( 0.0 ); - joint_trajectory.points[0].accelerations.push_back( 0.0 ); - joint_trajectory.points[0].effort.push_back( 0.0 ); - joint_trajectory.points[0].time_from_start = ros::Duration( sec ); - - *jt = joint_trajectory; - - return; -} - -inline void sobit_edu::SobitEduJointController::checkPublishersConnection ( const ros::Publisher& pub ) { - - ros::Rate loop_rate( 10 ); - while ( pub.getNumSubscribers() == 0 && ros::ok() ) { - try { loop_rate.sleep(); - } catch ( const std::exception& ex ) { break; } - } - return; -} - -// Check! -inline void sobit_edu::SobitEduJointController::callbackCurrArm( const sobits_msgs::current_state_array& msg ){ - // ros::spinOnce(); - - for( const auto actuator : msg.current_state_array ){ - if( actuator.joint_name == joint_names_[ARM_WRIST_TILT_JOINT] ) arm_wrist_tilt_current_ = actuator.current_ma; - if( actuator.joint_name == joint_names_[HAND_JOINT] ) hand_current_ = actuator.current_ma; - } -} - -#endif /* SOBIT_EDU_JOINT_CONTROLLER_H */ \ No newline at end of file diff --git a/sobit_edu_library/include/sobit_edu_library/sobit_edu_library.h b/sobit_edu_library/include/sobit_edu_library/sobit_edu_library.h deleted file mode 100644 index 0b284ba..0000000 --- a/sobit_edu_library/include/sobit_edu_library/sobit_edu_library.h +++ /dev/null @@ -1,22 +0,0 @@ -#ifndef _SOBIT_EDU_LIBRARY_SOBIT_EDU_LIBRARY_H_ -#define _SOBIT_EDU_LIBRARY_SOBIT_EDU_LIBRARY_H_ - -#include -#include - -class ROSCommonNode { - public: - ROSCommonNode( const std::string& name ) { - char* cstr = new char[name.size() + 1]; - std::strcpy( cstr, name.c_str() ); - char** argv = &cstr; - int argc = 0; - delete[] cstr; - - ros::init( argc, argv, "sobit_edu_library_node" ); - } - - ROSCommonNode() {} -}; - -#endif /* _SOBIT_EDU_LIBRARY_SOBIT_EDU_LIBRARY_H_ */ \ No newline at end of file diff --git a/sobit_edu_library/include/sobit_edu_library/sobit_edu_wheel_controller.hpp b/sobit_edu_library/include/sobit_edu_library/sobit_edu_wheel_controller.hpp deleted file mode 100644 index 99ee987..0000000 --- a/sobit_edu_library/include/sobit_edu_library/sobit_edu_wheel_controller.hpp +++ /dev/null @@ -1,83 +0,0 @@ -#ifndef SOBIT_EDU_WHEEL_CONTROLLER_H_ -#define SOBIT_EDU_WHEEL_CONTROLLER_H_ - -#include -#include - -#include -// #include - -#include -#include -#include -#include -#include -#include - -#include "sobit_edu_library/sobit_edu_library.h" - - -namespace sobit_edu { - class SobitEduWheelController : private ROSCommonNode { - private : - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - - ros::Publisher pub_cmd_vel_; - ros::Subscriber sub_odom_; - - nav_msgs::Odometry curt_odom_; - - void checkPublishersConnection( const ros::Publisher& pub ); - void callbackOdometry( const nav_msgs::OdometryConstPtr &odom_msg ); - double geometryQuat2Yaw( const geometry_msgs::Quaternion& geometry_quat ); - - - public : - SobitEduWheelController( const std::string &name ); - SobitEduWheelController( ); - - bool controlWheelLinear( const double distance ); - bool controlWheelRotateRad( const double angle_rad ); - bool controlWheelRotateDeg( const double angle_deg ); - - double rad2Deg( const double rad ); - double deg2Rad( const double deg ); - - }; -} - -inline void sobit_edu::SobitEduWheelController::checkPublishersConnection ( const ros::Publisher& pub ) { - ros::Rate loop_rate( 10 ); - - while ( pub.getNumSubscribers() == 0 && ros::ok() ) { - try { loop_rate.sleep(); - } catch ( const std::exception& ex ) { break; } - } - - return; -} - -inline void sobit_edu::SobitEduWheelController::callbackOdometry ( const nav_msgs::OdometryConstPtr &odom_msg ) { curt_odom_ = *odom_msg; } - -inline double sobit_edu::SobitEduWheelController::geometryQuat2Yaw( const geometry_msgs::Quaternion& geometry_quat ) { - // tf::Quaternion quat; - tf2::Quaternion quat_tf; - double roll, pitch, yaw; - - tf2::fromMsg(geometry_quat, quat_tf); - quat_tf.normalize(); - tf2::Matrix3x3(quat_tf).getRPY(roll, pitch, yaw); - - // quaternionMsgToTF( geometry_quat, quat ); - // quat.normalize(); - // tf::Matrix3x3( quat ).getRPY( roll, pitch, yaw ); - - return yaw; -} - -inline double sobit_edu::SobitEduWheelController::rad2Deg ( const double rad ) { return rad * 180.0 / M_PI; } - -inline double sobit_edu::SobitEduWheelController::deg2Rad ( const double deg ) { return deg * M_PI / 180.0; } - -#endif /* SOBIT_EDU_WHEEL_CONTROLLER_H_ */ \ No newline at end of file diff --git a/sobit_edu_library/launch/load_sobit_edu_pose.launch b/sobit_edu_library/launch/load_sobit_edu_pose.launch deleted file mode 100644 index 77a3685..0000000 --- a/sobit_edu_library/launch/load_sobit_edu_pose.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - \ No newline at end of file diff --git a/sobit_edu_library/package.xml b/sobit_edu_library/package.xml index caea49c..af10bcf 100644 --- a/sobit_edu_library/package.xml +++ b/sobit_edu_library/package.xml @@ -1,40 +1,18 @@ - + sobit_edu_library 0.0.0 - - This package allows shared configuration with SOBITS robots - - Yuki Okuma + TODO: Package description + sobits + TODO: License declaration - BSD + ament_cmake - https://home.soka.ac.jp/~choi/index.html - https://github.com/TeamSOBITS/sobit_common - https://github.com/TeamSOBITS/sobit_common/issues - Team SOBITS + ament_lint_auto + ament_lint_common - catkin - - dynamixel_sdk - roscpp - rospy - tf2 - tf2_ros - tf2_geometry_msgs - std_msgs - geometry_msgs - trajectory_msgs - nav_msgs - pybind11_catkin - sobits_msgs - - - - + ament_cmake diff --git a/sobit_edu_library/src/.gitkeep b/sobit_edu_library/src/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/sobit_edu_library/src/sobit_edu_joint_controller.cpp b/sobit_edu_library/src/sobit_edu_joint_controller.cpp deleted file mode 100644 index 43fc263..0000000 --- a/sobit_edu_library/src/sobit_edu_joint_controller.cpp +++ /dev/null @@ -1,440 +0,0 @@ -#include "sobit_edu_library/sobit_edu_joint_controller.hpp" -#include "sobit_edu_library/sobit_edu_wheel_controller.hpp" - - -using namespace sobit_edu; - -SobitEduJointController::SobitEduJointController( const std::string &name ) : ROSCommonNode(name), nh_(), pnh_("~"), tfBuffer_(), tfListener_(tfBuffer_) { - pub_arm_control_ = nh_.advertise( "/arm_trajectory_controller/command", 1) ; - pub_head_camera_control_ = nh_.advertise( "/head_camera_trajectory_controller/command", 1 ); - - sub_curr_arm = nh_.subscribe( "/current_state_array", 1, &SobitEduJointController::callbackCurrArm, this ); - - loadPose(); -} - -SobitEduJointController::SobitEduJointController( ) : ROSCommonNode(), nh_(), pnh_("~"), tfBuffer_(), tfListener_(tfBuffer_) { - pub_arm_control_ = nh_.advertise( "/arm_trajectory_controller/command", 1 ); - pub_head_camera_control_ = nh_.advertise( "/head_camera_trajectory_controller/command", 1 ); - - sub_curr_arm = nh_.subscribe( "/current_state_array", 1, &SobitEduJointController::callbackCurrArm, this ); - - loadPose(); -} - -void SobitEduJointController::loadPose( ) { - XmlRpc::XmlRpcValue pose_val; - - if ( !nh_.hasParam("/sobit_edu_pose") ) return; - nh_.getParam( "/sobit_edu_pose", pose_val ); - - int pose_num = pose_val.size(); - pose_list_.clear(); - - for ( int i = 0; i < pose_num; i++ ) { - Pose pose; - std::vector joint_val(Joint::JOINT_NUM, 0.0); - - pose.pose_name = static_cast(pose_val[i]["pose_name"]); - joint_val[Joint::ARM_SHOULDER_PAN_JOINT] = static_cast(pose_val[i][joint_names_[Joint::ARM_SHOULDER_PAN_JOINT]]); - joint_val[Joint::ARM_SHOULDER_1_TILT_JOINT] = static_cast(pose_val[i][joint_names_[Joint::ARM_SHOULDER_1_TILT_JOINT]]); - joint_val[Joint::ARM_SHOULDER_2_TILT_JOINT] = - static_cast(pose_val[i][joint_names_[Joint::ARM_SHOULDER_1_TILT_JOINT]]); - joint_val[Joint::ARM_ELBOW_1_TILT_JOINT] = static_cast(pose_val[i][joint_names_[Joint::ARM_ELBOW_1_TILT_JOINT]]); - joint_val[Joint::ARM_ELBOW_2_TILT_JOINT] = - static_cast(pose_val[i][joint_names_[Joint::ARM_ELBOW_1_TILT_JOINT]]); - joint_val[Joint::ARM_WRIST_TILT_JOINT] = static_cast(pose_val[i][joint_names_[Joint::ARM_WRIST_TILT_JOINT]]); - joint_val[Joint::HAND_JOINT] = static_cast(pose_val[i][joint_names_[Joint::HAND_JOINT]]); - joint_val[Joint::HEAD_CAMERA_PAN_JOINT] = static_cast(pose_val[i][joint_names_[Joint::HEAD_CAMERA_PAN_JOINT]]); - joint_val[Joint::HEAD_CAMERA_TILT_JOINT] = static_cast(pose_val[i][joint_names_[Joint::HEAD_CAMERA_TILT_JOINT]]); - - pose.joint_val = joint_val; - - pose_list_.push_back( pose ); - } - - return; -} - -bool SobitEduJointController::moveToPose( const std::string &pose_name, const double sec ) { - bool is_find = false; - std::vector joint_val; - for ( auto& pose : pose_list_ ) { - if ( pose_name != pose.pose_name ) continue; - is_find = true; - joint_val = pose.joint_val; - break; - } - if ( is_find ) { - ROS_INFO( "I found a '%s'", pose_name.c_str() ); - return moveAllJoint( joint_val[Joint::ARM_SHOULDER_PAN_JOINT], - joint_val[Joint::ARM_SHOULDER_1_TILT_JOINT], - joint_val[Joint::ARM_ELBOW_1_TILT_JOINT], - joint_val[Joint::ARM_WRIST_TILT_JOINT], - joint_val[Joint::HAND_JOINT], - joint_val[Joint::HEAD_CAMERA_PAN_JOINT], - joint_val[Joint::HEAD_CAMERA_TILT_JOINT], - sec ); - } else { - ROS_ERROR( "'%s' doesn't exist.", pose_name.c_str() ); - return false; - } -} - -bool SobitEduJointController::moveAllJoint( const double arm_shoulder_pan, - const double arm_shoulder_tilt, - const double arm_elbow_tilt, - const double arm_wrist_tilt, - const double hand, - const double head_camera_pan, - const double head_camera_tilt, - const double sec, bool is_sleep ) { - try { - trajectory_msgs::JointTrajectory arm_joint_trajectory; - trajectory_msgs::JointTrajectory head_camera_joint_trajectory; - - setJointTrajectory( joint_names_[Joint::ARM_SHOULDER_PAN_JOINT] , arm_shoulder_pan , sec, &arm_joint_trajectory ); - addJointTrajectory( joint_names_[Joint::ARM_SHOULDER_1_TILT_JOINT], arm_shoulder_tilt, sec, &arm_joint_trajectory ); - addJointTrajectory( joint_names_[Joint::ARM_SHOULDER_2_TILT_JOINT], -arm_shoulder_tilt, sec, &arm_joint_trajectory ); - addJointTrajectory( joint_names_[Joint::ARM_ELBOW_1_TILT_JOINT] , arm_elbow_tilt , sec, &arm_joint_trajectory ); - addJointTrajectory( joint_names_[Joint::ARM_ELBOW_2_TILT_JOINT] , -arm_elbow_tilt , sec, &arm_joint_trajectory ); - addJointTrajectory( joint_names_[Joint::ARM_WRIST_TILT_JOINT] , arm_wrist_tilt , sec, &arm_joint_trajectory ); - addJointTrajectory( joint_names_[Joint::HAND_JOINT] , -hand , sec, &arm_joint_trajectory ); - - setJointTrajectory( joint_names_[Joint::HEAD_CAMERA_PAN_JOINT] , head_camera_pan , sec, &head_camera_joint_trajectory ); - addJointTrajectory( joint_names_[Joint::HEAD_CAMERA_TILT_JOINT] , head_camera_tilt , sec, &head_camera_joint_trajectory ); - - checkPublishersConnection( pub_arm_control_ ); - checkPublishersConnection( pub_head_camera_control_ ); - pub_arm_control_.publish( arm_joint_trajectory ); - pub_head_camera_control_.publish( head_camera_joint_trajectory ); - - if ( is_sleep ) ros::Duration( sec ).sleep(); - - return true; - - } catch ( const std::exception& ex ) { - ROS_ERROR( "%s", ex.what() ); - return false; - } -} - -bool SobitEduJointController::moveJoint( const Joint joint_num, - const double rad, - const double sec, bool is_sleep ) { - try { - trajectory_msgs::JointTrajectory joint_trajectory; - - // ARM_SHOULDER_1_TILT_JOINT : joint_num = 1 - // ARM_SHOULDER_1_TILT_JOINT : joint_num = 2 - // ARM_ELBOW_1_TILT_JOINT : joint_num = 3 - // ARM_ELBOW_1_TILT_JOINT : joint_num = 4 - if ( joint_num == 1 || joint_num == 3 ) { - setJointTrajectory( joint_names_[joint_num] , rad, sec, &joint_trajectory ); - addJointTrajectory( joint_names_[joint_num + 1], -rad, sec, &joint_trajectory ); - - } else if ( joint_num == 2 || joint_num == 4 ) { - setJointTrajectory( joint_names_[joint_num] , -rad, sec, &joint_trajectory ); - addJointTrajectory( joint_names_[joint_num - 1], rad, sec, &joint_trajectory ); - - } else if ( joint_num == 6 ) { - setJointTrajectory( joint_names_[joint_num] , -rad, sec, &joint_trajectory ); - - } else { - setJointTrajectory( joint_names_[joint_num] , rad, sec, &joint_trajectory ); - } - - if ( joint_num < Joint::HEAD_CAMERA_PAN_JOINT ) { - checkPublishersConnection( pub_arm_control_ ); - pub_arm_control_.publish( joint_trajectory ); - } else { - checkPublishersConnection( pub_head_camera_control_ ); - pub_head_camera_control_.publish( joint_trajectory ); - } - - if ( is_sleep ) ros::Duration( sec ).sleep(); - - return true; - - } catch ( const std::exception& ex ) { - ROS_ERROR( "%s", ex.what() ); - return false; - } -} - -bool SobitEduJointController::moveHeadPanTilt( const double pan_rad, - const double tilt_rad, - const double sec, bool is_sleep ) { - try { - trajectory_msgs::JointTrajectory joint_trajectory; - - setJointTrajectory( joint_names_[Joint::HEAD_CAMERA_PAN_JOINT] , pan_rad , sec, &joint_trajectory ); - addJointTrajectory( joint_names_[Joint::HEAD_CAMERA_TILT_JOINT], tilt_rad, sec, &joint_trajectory ); - - checkPublishersConnection( pub_head_camera_control_ ); - pub_head_camera_control_.publish( joint_trajectory ); - - if ( is_sleep ) ros::Duration( sec ).sleep(); - - return true; - - } catch ( const std::exception& ex ) { - ROS_ERROR( "%s", ex.what() ); - return false; - } -} - -bool SobitEduJointController::moveArm ( const double arm_shoulder_pan, - const double arm_shoulder_tilt, - const double arm_elbow_tilt, - const double arm_wrist_tilt, - const double hand, - const double sec, bool is_sleep ) { - try { - trajectory_msgs::JointTrajectory arm_joint_trajectory; - - setJointTrajectory( joint_names_[Joint::ARM_SHOULDER_PAN_JOINT] , arm_shoulder_pan , sec, &arm_joint_trajectory ); - addJointTrajectory( joint_names_[Joint::ARM_SHOULDER_1_TILT_JOINT], arm_shoulder_tilt, sec, &arm_joint_trajectory ); - addJointTrajectory( joint_names_[Joint::ARM_SHOULDER_2_TILT_JOINT], -arm_shoulder_tilt, sec, &arm_joint_trajectory ); - addJointTrajectory( joint_names_[Joint::ARM_ELBOW_1_TILT_JOINT] , arm_elbow_tilt , sec, &arm_joint_trajectory ); - addJointTrajectory( joint_names_[Joint::ARM_ELBOW_2_TILT_JOINT] , -arm_elbow_tilt , sec, &arm_joint_trajectory ); - addJointTrajectory( joint_names_[Joint::ARM_WRIST_TILT_JOINT] , arm_wrist_tilt , sec, &arm_joint_trajectory ); - addJointTrajectory( joint_names_[Joint::HAND_JOINT] , -hand , sec, &arm_joint_trajectory ); - - checkPublishersConnection ( pub_arm_control_ ); - pub_arm_control_.publish( arm_joint_trajectory ); - - if ( is_sleep ) ros::Duration( sec ).sleep(); - - return true; - - } catch ( const std::exception& ex ) { - ROS_ERROR( "%s", ex.what() ); - return false; - } -} - -bool SobitEduJointController::moveHandToTargetCoord( const double target_pos_x, const double target_pos_y, const double target_pos_z, - const double shift_x , const double shift_y , const double shift_z, - const double sec, bool is_sleep ){ - sobit_edu::SobitEduWheelController wheel_ctrl; - - // // Calculate goal_position_pos + difference(gap) - const double base_to_target_x = target_pos_x + shift_x; - const double base_to_target_y = target_pos_y + shift_y; - const double base_to_target_z = target_pos_z + shift_z; - bool is_reached = false; - - // Calculate angle between footbase_pos and the sifted goal_position_pos (XY平面) - double tan_rad = std::atan2( base_to_target_y, base_to_target_x ); - - // Change goal_position_pos units (m->cm) - const double goal_position_pos_x_cm = base_to_target_x * 100.; - const double goal_position_pos_y_cm = base_to_target_y * 100.; - const double goal_position_pos_z_cm = base_to_target_z * 100.; - - // Check if the object is graspable - if ( goal_position_pos_z_cm > grasp_max_z_cm ) { - std::cout << "The target is located too tall (" << goal_position_pos_z_cm << ">80.0)" << std::endl; - return is_reached; - - } else if ( goal_position_pos_z_cm < grasp_min_z_cm ) { - std::cout << "The target is located too low (" << goal_position_pos_z_cm << "<35.0) " << std::endl; - return is_reached; - } - - double shoulder_roll_joint_rad = 0.0; - double shoulder_flex_joint_rad = 0.0; - double arm_elbow_tilt_joint_rad = 0.0; - double arm_wrist_tilt_joint_rad = 0.0; - double hand_rad = 0.0; - double base_to_arm_wrist_tilt_joint_x_cm = 0.0; - - // Target is above arm_elbow_tilt_join - if ( (base_to_shoulder_flex_joint_z_cm + arm_upper_link_z_cm) < goal_position_pos_z_cm ) { - std::cout << "Target (z:" << goal_position_pos_z_cm << ") is above arm_elbow_tilt_joint" << std::endl; - ROS_INFO( "Target (z:%f) is above arm_elbow_tilt_joint", goal_position_pos_z_cm ); - - // Caution: Calculating until arm_wrist_tilt_joint_x_cm (not target) - double arm_elbow_tilt_joint_sin = (goal_position_pos_z_cm - (base_to_shoulder_flex_joint_z_cm + arm_upper_link_x_cm)) / arm_outer_link_x_cm; - arm_elbow_tilt_joint_rad = std::asin(arm_elbow_tilt_joint_sin); - arm_wrist_tilt_joint_rad = -arm_elbow_tilt_joint_rad; - shoulder_flex_joint_rad = 0.0; - - base_to_arm_wrist_tilt_joint_x_cm = base_to_shoulder_flex_joint_x_cm + arm_upper_link_z_cm + arm_outer_link_x_cm * std::cos(arm_elbow_tilt_joint_rad); - } - - // Target is below arm_elbow_tilt_join and above shoulder_flex_joint - else if ( base_to_shoulder_flex_joint_z_cm <= goal_position_pos_z_cm && goal_position_pos_z_cm <= (base_to_shoulder_flex_joint_z_cm + arm_upper_link_x_cm) ) { - std::cout << "Target (z:" << goal_position_pos_z_cm << ") is below arm_elbow_tilt_join and above shoulder_flex_joint" << std::endl; - ROS_INFO( "Target (z:%f) is below arm_elbow_tilt_join and above shoulder_flex_joint", goal_position_pos_z_cm ); - - // Caution: Calculating until arm_wrist_tilt_joint_x_cm (not target) - double arm_elbow_tilt_joint_sin = (base_to_shoulder_flex_joint_z_cm + arm_upper_link_x_cm - goal_position_pos_z_cm) / arm_outer_link_x_cm; - arm_elbow_tilt_joint_rad = -std::asin(arm_elbow_tilt_joint_sin); - arm_wrist_tilt_joint_rad = -arm_elbow_tilt_joint_rad; - shoulder_flex_joint_rad = 0.0; - - base_to_arm_wrist_tilt_joint_x_cm = base_to_shoulder_flex_joint_x_cm + arm_upper_link_z_cm + arm_outer_link_x_cm * std::cos(arm_elbow_tilt_joint_rad); - } - - // Target is below shoulder_flex_joint - else if ( goal_position_pos_z_cm < base_to_shoulder_flex_joint_z_cm ) { - std::cout << "Target (z:" << goal_position_pos_z_cm << ") is below shoulder_flex_joint" << std::endl; - ROS_INFO( "Target (z:%f) is below shoulder_flex_joint", goal_position_pos_z_cm ); - - // Caution: Calculating until arm_wrist_tilt_joint_x_cm (not target) - double arm_elbow_tilt_joint_cos = (base_to_shoulder_flex_joint_z_cm - arm_upper_link_z_cm - goal_position_pos_z_cm) / arm_outer_link_x_cm; - arm_elbow_tilt_joint_rad = std::acos(arm_elbow_tilt_joint_cos); - arm_wrist_tilt_joint_rad = std::asin(arm_elbow_tilt_joint_cos); - shoulder_flex_joint_rad = -wheel_ctrl.deg2Rad(90.0); - - base_to_arm_wrist_tilt_joint_x_cm = base_to_shoulder_flex_joint_x_cm + arm_upper_link_z_cm + arm_outer_link_x_cm * std::cos(arm_elbow_tilt_joint_rad); - } - - // Calculate wheel movement (diagonal) - // - Rotate the robot - const double rot_rad = std::atan2( goal_position_pos_y_cm, goal_position_pos_x_cm ); - // ROS_INFO( "rot_rad = %f(deg:%f)", rot_rad, SobitEduWheelController::rad2Deg(rot_rad) ); - wheel_ctrl.controlWheelRotateRad( rot_rad ); - ros::Duration(1.0).sleep(); - - // - Move forward the robot - const double linear_m = std::sqrt(std::pow(goal_position_pos_x_cm, 2) + std::pow(goal_position_pos_y_cm, 2)) / 100.0 - base_to_arm_wrist_tilt_joint_x_cm / 100.0; - // double linear_m = std::sqrt(std::pow(transform_base_to_target.getOrigin().x(), 2) + std::pow(transform_base_to_target.getOrigin().y(), 2)) - base_to_arm_wrist_tilt_joint_x_cm / 100. + shift_x; - ROS_INFO( "linear_m = %f", linear_m ); - wheel_ctrl.controlWheelLinear( linear_m ); - ros::Duration(1.0).sleep(); - - // // Calculate wheel movement (+-90->x_pos->-+90->y_pos) NEEDS CONFIRMATION - // // - Rotate the robot - // const double rot_deg = goal_position_pos_x_cm > 0.0 ? 90.0:-90.0; - // ROS_INFO("rot_deg:%f)", rot_deg); - // wheel_ctrl.controlWheelRotateDeg(rot_deg); - // ros::Duration(3.0).sleep(); - - // // - Move forward the robot - // ROS_INFO("linear_m = %f", goal_position_pos_x_cm/100.0); - // wheel_ctrl.controlWheelLinear(goal_position_pos_x_cm/100.0); - // ros::Duration(3.0).sleep(); - - // // - Rotate the robot - // ROS_INFO("rot_deg:%f)", -rot_deg); - // wheel_ctrl.controlWheelRotateDeg(-rot_deg); - // ros::Duration(3.0).sleep(); - - // // - Move forward the robot - // ROS_INFO("linear_m = %f", goal_position_pos_y_cm/100.0); - // wheel_ctrl.controlWheelLinear(goal_position_pos_y_cm/100.0); - // ros::Duration(3.0).sleep(); - - // - Move arm (OPEN) - hand_rad = wheel_ctrl.deg2Rad( 90.0 ); - is_reached = moveArm( shoulder_roll_joint_rad, shoulder_flex_joint_rad, arm_elbow_tilt_joint_rad, arm_wrist_tilt_joint_rad, hand_rad, - sec, is_sleep ); - double shoulder_roll_joint_deg = wheel_ctrl.rad2Deg( shoulder_roll_joint_rad ); - double shoulder_flex_joint_deg = wheel_ctrl.rad2Deg( shoulder_flex_joint_rad ); - double arm_elbow_tilt_joint_deg = wheel_ctrl.rad2Deg( arm_elbow_tilt_joint_rad ); - double arm_wrist_tilt_joint_deg = wheel_ctrl.rad2Deg( arm_wrist_tilt_joint_rad ); - double hand_deg = wheel_ctrl.rad2Deg( hand_rad ); - printf( "ARM RAD: %f\t%f\t%f\t%f\t%f\n", shoulder_roll_joint_rad, shoulder_flex_joint_rad, arm_elbow_tilt_joint_rad, arm_wrist_tilt_joint_rad, hand_rad ); - printf( "ARM DEG: %f\t%f\t%f\t%f\t%f\n", shoulder_roll_joint_deg, shoulder_flex_joint_deg, arm_elbow_tilt_joint_deg, arm_wrist_tilt_joint_deg, hand_deg ); - ROS_INFO( "goal_position_pos = (%f, %f, %f)", goal_position_pos_x_cm, goal_position_pos_y_cm, goal_position_pos_z_cm ); - // ROS_INFO("result_pos = (%f, %f, %f)", for_kinematics_x, for_kinematics_z, for_kinematics_z); - // ros::Duration(2.0).sleep(); - - return is_reached; -} - -bool SobitEduJointController::moveHandToTargetTF( const std::string& target_name, - const double shift_x, const double shift_y, const double shift_z, - const double sec, bool is_sleep ){ - geometry_msgs::TransformStamped transformStamped; - bool is_reached = false; - - try{ - tfBuffer_.canTransform("base_footprint", target_name, ros::Time(0), ros::Duration(0.5)); - transformStamped = tfBuffer_.lookupTransform ("base_footprint", target_name, ros::Time(0)); - } catch( tf2::TransformException &ex ){ - ROS_ERROR("%s", ex.what()); - return false; - } - - auto& tf_target_to_arm = transformStamped.transform.translation; - is_reached = moveHandToTargetCoord( tf_target_to_arm.x, tf_target_to_arm.y, tf_target_to_arm.z, - shift_x, shift_y, shift_z, - sec, is_sleep ); - - return is_reached; -} - -bool SobitEduJointController::moveHandToPlaceCoord( const double target_pos_x, const double target_pos_y, const double target_pos_z, - const double shift_x , const double shift_y , const double shift_z, - const double sec, bool is_sleep ){ - geometry_msgs::Point shift; - - double target_z = 0.; - bool is_reached = false; - - // Reduce the target_pos_z by 0.05[m], until expected collision is detected - while( !(is_reached && placeDecision(500, 1000)) ) { - is_reached = moveHandToTargetCoord( target_pos_x, target_pos_y, target_pos_z, - shift_x, shift_y, shift_z+target_z, - sec, is_sleep ); - - if( !is_reached ) return is_reached; - - // Accumulate the target_z - target_z -= 0.05; - } - - return is_reached; -} - -bool SobitEduJointController::moveHandToPlaceTF( const std::string& target_name, - const double shift_x, const double shift_y, const double shift_z, - const double sec, bool is_sleep ){ - geometry_msgs::Point shift; - - geometry_msgs::TransformStamped transform_base_to_target; - try { - tfBuffer_.canTransform("base_footprint", target_name, ros::Time(0), ros::Duration(2.0)); - transform_base_to_target = tfBuffer_.lookupTransform("base_footprint", target_name, ros::Time(0)); - } catch ( tf2::TransformException ex ) { - ROS_ERROR("%s", ex.what()); - return false; - } - - auto& goal_position = transform_base_to_target.transform.translation; - - moveHandToPlaceCoord( goal_position.x, goal_position.y, goal_position.z, - shift_x, shift_y, shift_z, - sec, is_sleep ); - return true; -} - -bool SobitEduJointController::graspDecision( const int min_curr, const int max_curr ) { - bool is_grasped = false; - - // Spin until the current value is obtained - while( hand_current_ == 0. ) ros::spinOnce(); - - // ros::spinOnce(); - std::cout << "hand_current_ = " << hand_current_ << std::endl; - - is_grasped = (min_curr <= hand_current_ && hand_current_ <= max_curr) ? true : false; - - return is_grasped; -} - -bool SobitEduJointController::placeDecision( const int min_curr, const int max_curr ){ - bool is_placed = false; - - // Spin until the current value is obtained - while( arm_wrist_tilt_current_ == 0. ) ros::spinOnce(); - - // ros::spinOnce(); - std::cout << "arm_wrist_tilt_joint_curr_ = " << arm_wrist_tilt_current_ << std::endl; - - is_placed = (min_curr <= arm_wrist_tilt_current_ && arm_wrist_tilt_current_ <= max_curr) ? true : false; - - return is_placed; -} \ No newline at end of file diff --git a/sobit_edu_library/src/sobit_edu_library.cpp b/sobit_edu_library/src/sobit_edu_library.cpp deleted file mode 100644 index e40b4a7..0000000 --- a/sobit_edu_library/src/sobit_edu_library.cpp +++ /dev/null @@ -1,87 +0,0 @@ -#include "sobit_edu_library/sobit_edu_library.h" -#include "sobit_edu_library/sobit_edu_joint_controller.hpp" -#include "sobit_edu_library/sobit_edu_wheel_controller.hpp" - -using namespace sobit_edu; -// namespace pybind11 = pybind11bind11; - -PYBIND11_MODULE( sobit_edu_module, m ) { - pybind11::enum_( m, "Joint" ) - .value( "ARM_SHOULDER_PAN_JOINT", Joint::ARM_SHOULDER_PAN_JOINT ) - .value( "ARM_SHOULDER_1_TILT_JOINT", Joint::ARM_SHOULDER_1_TILT_JOINT ) - .value( "ARM_SHOULDER_2_TILT_JOINT", Joint::ARM_SHOULDER_2_TILT_JOINT ) - .value( "ARM_ELBOW_1_TILT_JOINT", Joint::ARM_ELBOW_1_TILT_JOINT ) - .value( "ARM_ELBOW_2_TILT_JOINT", Joint::ARM_ELBOW_2_TILT_JOINT ) - .value( "ARM_WRIST_TILT_JOINT", Joint::ARM_WRIST_TILT_JOINT ) - .value( "HAND_JOINT", Joint::HAND_JOINT ) - .value( "HEAD_CAMERA_PAN_JOINT", Joint:: HEAD_CAMERA_PAN_JOINT ) - .value( "HEAD_CAMERA_TILT_JOINT", Joint::HEAD_CAMERA_TILT_JOINT ) - .value( "JOINT_NUM", Joint::JOINT_NUM ) - .export_values( ); - - pybind11::class_( m, "SobitEduJointController" ) - .def( pybind11::init< const std::string& >() ) - .def( "moveToPose", &SobitEduJointController::moveToPose, "move Pose", - pybind11::arg( "pose_name" ), - pybind11::arg( "sec" ) = 5.0 ) - .def( "moveAllJoint", &SobitEduJointController::moveAllJoint, "move all Joint", - pybind11::arg( "arm_shoulder_pan" ), - pybind11::arg( "arm_shoulder_tilt" ), - pybind11::arg( "arm_elbow_tilt" ), - pybind11::arg( "arm_wrist_tilt" ), - pybind11::arg( "hand" ), - pybind11::arg( "head_camera_pan" ), - pybind11::arg( "head_camera_tilt" ), - pybind11::arg( "sec" ) = 5.0, pybind11::arg( "is_sleep" ) = true) - .def( "moveJoint", &SobitEduJointController::moveJoint, "moveJoint", - pybind11::arg( "joint_num" ), - pybind11::arg( "rad" ), - pybind11::arg( "sec" ) = 5.0, pybind11::arg( "is_sleep" ) = true ) - .def( "moveHeadPanTilt", &SobitEduJointController::moveHeadPanTilt, "move Head PanTilt", - pybind11::arg( "pan_rad" ), - pybind11::arg( "tilt_rad" ), - pybind11::arg( "sec" ) = 5.0, pybind11::arg( "is_sleep" ) = true ) - .def( "moveArm", &SobitEduJointController::moveArm, "move Arm", - pybind11::arg( "arm_shoulder_pan" ), - pybind11::arg( "arm_shoulder_tilt" ), - pybind11::arg( "arm_elbow_tilt" ), - pybind11::arg( "arm_wrist_tilt" ), - pybind11::arg( "hand" ), - pybind11::arg( "sec" ) = 5.0, pybind11::arg( "is_sleep" ) = true ) - .def( "moveHandToTargetCoord", &SobitEduJointController::moveHandToTargetCoord, "moveHandToTargetCoord", - pybind11::arg( "target_pos_x" ), pybind11::arg( "target_pos_y" ), pybind11::arg( "target_pos_z" ), - pybind11::arg( "shift_x" ), pybind11::arg( "shift_y" ), pybind11::arg( "shift_z" ), - pybind11::arg( "sec" ) = 5.0, pybind11::arg( "is_sleep" ) = true ) - .def( "moveHandToTargetTF", &SobitEduJointController::moveHandToTargetTF, "moveHandToTargetTF", - pybind11::arg( "target_name" ), - pybind11::arg( "shift_x" ), pybind11::arg( "shift_y" ), pybind11::arg( "shift_z" ), - pybind11::arg( "sec" ) = 5.0, pybind11::arg( "is_sleep" ) = true ) - .def( "moveHandToPlaceCoord", &SobitEduJointController::moveHandToPlaceCoord, "move Hand To Placeable Position Coordinate", - pybind11::arg( "target_pos_x" ), pybind11::arg( "target_pos_y" ), pybind11::arg( "target_pos_z" ), - pybind11::arg( "shift_x" ), pybind11::arg( "shift_y" ), pybind11::arg( "shift_z" ), - pybind11::arg( "sec" ) = 5.0, pybind11::arg( "is_sleep" ) = true ) - .def( "moveHandToPlaceTF", &SobitEduJointController::moveHandToPlaceTF, "move Hand To Placeable Position TF", - pybind11::arg( "target_name" ), - pybind11::arg( "shift_x" ), pybind11::arg( "shift_y" ), pybind11::arg( "shift_z" ), - pybind11::arg( "sec" ) = 5.0, pybind11::arg( "is_sleep" ) = true ) - .def( "graspDecision", &SobitEduJointController::graspDecision, "grasp Decision", - pybind11::arg( "min_curr" ) = 300, - pybind11::arg( "max_curr" ) = 1000) - .def( "placeDecision", &SobitEduJointController::placeDecision, "place Decision", - pybind11::arg( "min_curr" ) = 500, - pybind11::arg( "max_curr" ) = 1000); - - pybind11::class_( m, "SobitEduWheelController" ) - .def( pybind11::init< const std::string& >() ) - .def( "controlWheelLinear", &SobitEduWheelController::controlWheelLinear, "control Wheel Linear", - pybind11::arg( "distance" ) ) - .def( "controlWheelRotateRad", &SobitEduWheelController::controlWheelRotateRad, "control Wheel Rotate Rad", - pybind11::arg( "angle_rad" ) ) - .def( "controlWheelRotateDeg", &SobitEduWheelController::controlWheelRotateDeg, "control Wheel Rotate Deg", - pybind11::arg( "angle_deg" ) ) - .def( "rad2Deg", &SobitEduWheelController::rad2Deg, "rad 2 Deg", - pybind11::arg( "rad" ) ) - .def( "deg2Rad", &SobitEduWheelController::deg2Rad, "deg 2 Rad", - pybind11::arg( "deg" ) ); - -} \ No newline at end of file diff --git a/sobit_edu_library/src/sobit_edu_wheel_controller.cpp b/sobit_edu_library/src/sobit_edu_wheel_controller.cpp deleted file mode 100644 index 6c22e15..0000000 --- a/sobit_edu_library/src/sobit_edu_wheel_controller.cpp +++ /dev/null @@ -1,158 +0,0 @@ -#include "sobit_edu_library/sobit_edu_wheel_controller.hpp" - -using namespace sobit_edu; - -SobitEduWheelController::SobitEduWheelController ( const std::string &name ) : ROSCommonNode( name ), nh_(), pnh_("~") { - sub_odom_ = nh_.subscribe( "/odom", 1, &SobitEduWheelController::callbackOdometry, this ); - pub_cmd_vel_ = nh_.advertise< geometry_msgs::Twist >( "/cmd_vel_mux/input/teleop", 1 ); - - ros::spinOnce(); - ros::Duration(3.0).sleep(); -} - -SobitEduWheelController::SobitEduWheelController ( ) : ROSCommonNode( ), nh_(), pnh_("~") { - sub_odom_ = nh_.subscribe( "/odom", 1, &SobitEduWheelController::callbackOdometry, this ); - pub_cmd_vel_ = nh_.advertise< geometry_msgs::Twist >( "/cmd_vel_mux/input/teleop", 1 ); - - ros::spinOnce(); - ros::Duration(3.0).sleep(); -} - -bool SobitEduWheelController::controlWheelLinear( const double distance ) { - try { - double start_time = ros::Time::now().toSec(); - geometry_msgs::Twist output_vel; - - while ( curt_odom_.pose.pose.position.x == 0 & - curt_odom_.pose.pose.position.y == 0 & - curt_odom_.pose.pose.position.z == 0 & - curt_odom_.pose.pose.orientation.w == 0 ) { - - ros::spinOnce(); - } - nav_msgs::Odometry init_odom = curt_odom_; - - double moving_distance = 0.0; - double target_distance = std::fabs( distance ); - - double Kp = 0.1; - double Ki = 0.4; - double Kd = 0.8; - - double velocity_differential = Kp * distance; - ros::Rate loop_rate(20); - - while ( moving_distance < target_distance ) { - ros::spinOnce(); - - double end_time = ros::Time::now().toSec(); - double elapsed_time = end_time - start_time; - - double vel_linear = 0.0; - - // PID - if ( target_distance <= 0.1 ) { - vel_linear = Kp * ( target_distance + 0.001 - moving_distance ) - Kd * velocity_differential + Ki / 0.8 * ( target_distance + 0.001 - moving_distance ) * std::pow( elapsed_time, 2 ); - } else { - vel_linear = Kp * ( target_distance + 0.001 - moving_distance ) - Kd * velocity_differential + Ki / ( 8.0 / target_distance ) * ( target_distance + 0.001 - moving_distance ) * std::pow( elapsed_time, 2 ); - } - output_vel.linear.x = ( distance > 0 ) ? vel_linear : -vel_linear; - velocity_differential = vel_linear; - - pub_cmd_vel_.publish( output_vel ); - - double x_diif = curt_odom_.pose.pose.position.x - init_odom.pose.pose.position.x; - double y_diif = curt_odom_.pose.pose.position.y - init_odom.pose.pose.position.y; - moving_distance = std::hypotf( x_diif, y_diif ); - - // ROS_INFO("target_distance = %f\tmoving_distance = %f", target_distance, moving_distance ); - ROS_INFO("x_diif = %f\tx2_diif = %f\ty_diif = %f\ty2_diif = %f", init_odom.pose.pose.position.x, curt_odom_.pose.pose.position.x, init_odom.pose.pose.position.y, curt_odom_.pose.pose.position.y ); - loop_rate.sleep(); - } - - return true; - } catch ( const std::exception& ex ) { - ROS_ERROR("%s", ex.what()); - return false; - } -} - -bool SobitEduWheelController::controlWheelRotateRad( const double angle_rad ) { - try { - while ( curt_odom_.pose.pose.position.x == 0 & - curt_odom_.pose.pose.position.y == 0 & - curt_odom_.pose.pose.position.z == 0 & - curt_odom_.pose.pose.orientation.w == 0 ) { - - ros::spinOnce(); - } - - double start_time = ros::Time::now().toSec(); - - geometry_msgs::Twist output_vel; - double init_yaw = geometryQuat2Yaw( curt_odom_.pose.pose.orientation ); - double moving_angle_rad = 0.0; - double abs_angle_rad = std::fabs( angle_rad ); - double abs_angle_deg = rad2Deg( abs_angle_rad ); - - double Kp = 0.1; - double Ki = 0.4; - double Kd = 0.8; - - double velocity_differential = Kp * angle_rad; - int loop_cnt = 1; - ros::Rate loop_rate(20); - - while ( moving_angle_rad < abs_angle_rad ) { - ros::spinOnce(); - - double end_time = ros::Time::now().toSec(); - double elapsed_time = end_time - start_time; - double vel_angular = 0.0; - - // PID - if ( abs_angle_deg <= 30 ) { - vel_angular = Kp * ( abs_angle_rad + 0.001 - moving_angle_rad ) - - Kd * velocity_differential - + Ki * ( abs_angle_rad + 0.001 - moving_angle_rad ) * std::pow( elapsed_time, 2 ); - } else { - vel_angular = Kp * ( abs_angle_rad + 0.001 - moving_angle_rad ) - - Kd * velocity_differential - + Ki * ( abs_angle_rad + 0.001 - moving_angle_rad ) * std::pow( elapsed_time, 2 ) * 0.75 * 30 / abs_angle_deg; - } - output_vel.angular.z = ( angle_rad > 0 ) ? vel_angular : - vel_angular; - velocity_differential = vel_angular; - - pub_cmd_vel_.publish( output_vel ); - - double curt_yaw = geometryQuat2Yaw( curt_odom_.pose.pose.orientation ); - double pre_move_ang_rad = moving_angle_rad; - - if ( -0.00314 < curt_yaw - init_yaw && curt_yaw - init_yaw < 0 && 0 < angle_rad ) continue; - else if ( 0 < curt_yaw - init_yaw && curt_yaw - init_yaw < 0.00314 && angle_rad < 0 ) continue; - - if ( curt_yaw - init_yaw < 0 && 0 < angle_rad ) moving_angle_rad = abs(curt_yaw - init_yaw + deg2Rad(360 * loop_cnt)); - else if ( 0 < curt_yaw - init_yaw && angle_rad < 0 ) moving_angle_rad = abs(curt_yaw - init_yaw - deg2Rad(360 * loop_cnt)); - else if ( 0 < angle_rad ) moving_angle_rad = abs(curt_yaw - init_yaw + deg2Rad(360 * (loop_cnt-1))); - else moving_angle_rad = abs(curt_yaw - init_yaw - deg2Rad(360 * (loop_cnt-1))); - - if ( rad2Deg(moving_angle_rad) < (rad2Deg(pre_move_ang_rad)-0.0314) ) { - loop_cnt++; - if ( 0 < angle_rad ) moving_angle_rad = abs(curt_yaw - init_yaw + deg2Rad(360 * (loop_cnt-1))); - else moving_angle_rad = abs(curt_yaw - init_yaw - deg2Rad(360 * (loop_cnt-1))); - } - - ROS_INFO( "target_angle = %f\tmoving_angle = %f", abs_angle_rad, moving_angle_rad ); - - loop_rate.sleep(); - } - return true; - } catch ( const std::exception& ex ) { - ROS_ERROR( "%s", ex.what() ); - return false; - } -} - -bool SobitEduWheelController::controlWheelRotateDeg( const double angle_deg ) { - return controlWheelRotateRad( deg2Rad(angle_deg) ); -} \ No newline at end of file