diff --git a/CMakeLists.txt b/CMakeLists.txt index efe3f9d..52a4140 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,9 +1,21 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(pybullet_ros) -find_package(catkin REQUIRED COMPONENTS - std_msgs - cv_bridge +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(ament_index_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(tf2_ros REQUIRED) + +ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR pybullet_ros/${PROJECT_NAME}) + +install(DIRECTORY config launch common resource + DESTINATION share/${PROJECT_NAME} ) -catkin_python_setup() +ament_package() diff --git a/README.md b/README.md index 6ddfad5..72f7571 100644 --- a/README.md +++ b/README.md @@ -1,243 +1,98 @@ # pybullet_ros -A bridge between [ROS1](https://www.ros.org/) and [PyBullet](https://pybullet.org/wordpress/) +A bridge between [ROS 2](https://docs.ros.org/en/jazzy/) and [PyBullet](https://pybullet.org/wordpress/). drawing -# Project status +## Project status -This project is in a medium stage and presents with the following features: +This project exposes the pybullet simulator through a ROS 2 node and a set of plugins that implement common robot interfaces: -- body velocity control - Subscription to cmd_vel topic and apply desired speed to the robot (without noise) -- joint control: Joint Position, velocity and effort control for all revolute joints on the robot -- sensors: Odometry, joint states (joint position, velocity and effort feedback), laser scanner, RGB camera image +- Body velocity control – subscribe to `cmd_vel` and apply the desired speed to the robot. +- Joint control – position, velocity and effort control for all revolute joints on the robot. +- Sensors – odometry, joint states, laser scanner and RGB camera image publishers. -Missing: - -- sensors: Depth information (pointcloud) -- [sdf](http://sdformat.org) support - -Main implementation is done [here](https://github.com/oscar-lima/pybullet_ros/blob/noetic/ros/src/pybullet_ros/pybullet_ros.py) +Planned features include point cloud support and SDF world loading. ## Installation -The following instructions have been tested under ubuntu 20.04 and [ROS noetic](http://wiki.ros.org/noetic/Installation/Ubuntu). - -This wrapper requires that you have pybullet installed, you can do so by executing: - - sudo -H pip3 install pybullet - -Additionally clone this repository inside your [catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace), -compile (catkin build) and source your devel workspace (as you would normally do with any ROS pkg). +The package targets ROS 2 Jazzy on Ubuntu 24.04. The following steps assume you have sourced your ROS 2 environment and created a colcon workspace (`~/ros2_ws` in the example below): -In case you need to simulate RGBD sensor then install opencv for python3 and ros cv bridge: +```bash +sudo apt install python3-pip ros-jazzy-xacro ros-jazzy-cv-bridge ros-jazzy-joint-state-publisher-gui +python3 -m pip install --user --upgrade pybullet opencv-python - sudo -H pip3 install opencv-python - sudo apt install ros-noetic-cv-bridge +cd ~/ros2_ws/src +git clone https://github.com/oscar-lima/pybullet_ros.git +cd .. +colcon build --packages-select pybullet_ros +source install/setup.bash +``` ## Test the simulator -We provide with 2 robots for testing purposes: acrobat and r2d2, they can be found [here](https://github.com/oscar-lima/pybullet_ros/tree/noetic/common/test/urdf). - -### Bringup r2d2 robot - -This code is shipped with a simple URDF robot for testing purposes (r2d2), you can run it by executing: - - roslaunch pybullet_ros bringup_robot_example.launch - -You should now be able to visualise the robot in a gui. - -### Send position control commands to the robot. - -Publish a float msg to the following topic: - - rostopic pub /head_swivel_position_controller/command std_msgs/Float64 "data: 1.0" --once - -Move in position control with convenient gui: - - roslaunch pybullet_ros position_cmd_gui.launch - -A gui will pop up, use the slides to control the angle of your robot joints in position control. - -NOTE: This gui should not be active while sending velocity of effort commands! - -### Send joint velocity or effort (torque) control commands to the robot. - -NOTE: r2d2 robot has a revolute joint in the neck that can be used to test - -position, velocity or effort commands as described below in the following lines: - -Before sending commands, make sure position control interface gui publisher is not running! - -Publish a float msg to the following topics: - -velocity controller interface: - - rostopic pub /head_swivel_velocity_controller/command std_msgs/Float64 "data: 2.0" --once - -effort controller interface: - - rostopic pub /head_swivel_effort_controller/command std_msgs/Float64 "data: -2000.0" --once - -Done. The robot should now move in velocity or effort control mode with the desired speed/torque. - -## Visualize tf data and robot model in rviz - -A convenient configuration file is provided for the visualization of the example robot, run it with: - - rosrun rviz rviz --display-config `rospack find pybullet_ros`/ros/config/pybullet_config.rviz - -## Topics you can use to interact with this node - -```/joint_states``` (sensor_msgs/JointState) this topic is published at the ```pybullet_ros/loop_rate``` -parameter frequency (see parameters section for more detail). -This topic gets listened by the robot state publisher which in turn publishes tf data to the ROS ecosystem. - -```/tf``` - This wrapper broadcats all robot transformations to tf, using the robot state publisher and custom plugins. - -```/scan```- Using the lidar plugin you get laser scanner readings of type sensor_msgs/LaserScan. - -```/odom``` - Using the odometry plugin, robot body odometry gets published (nav_msgs/Odometry). - -```/cmd_vel``` - Using the body_vel_control plugin, the robot will subscribe to cmd_vel and exert the desired velocity to the robot. - -```__controller/command``` - replace "joint_name" with actual joint name and "xtype" -with [position, velocity, effort] - Using the control plugin, you can publish a joint command on this topic -and the robot will forward the instruction to the robot joint. - -```/rgb_image``` - The camera image of type (sensor_msgs/Image) - -## Services offered by this node - -reset simulation, of type std_srvs/Empty, which means it takes no arguments as input, it calls pybullet.resetSimulation() method. - - rosservice call /pybullet_ros/reset_simulation - -pause or unpause physics, empty args, prevents the wrapper to call stepSimulation() - - rosservice call /pybullet_ros/pause_physics - rosservice call /pybullet_ros/unpause_physics - -## Parameters - -The following parameters can be used to customize the behavior of the simulator. - -~ refers to the name of the node (because private nodehandle is used), e.g. pybullet_ros - -```~loop_rate``` - Sleep to control the frequency of how often to call pybullet.stepSimulation(), default : 10.0 (hz) - -```~pybullet_gui``` - Whether you want to visualize the simulation in a gui or not, default : True - -```~robot_urdf_path``` - The path to load a robot at startup, default : None - -```~pause_simulation``` - Specify if simulation must start paused (true) or unpaused (false), default : False - -```~gravity``` - The desired value of gravity for your simulation physics engine, default : -9.81 - -```~max_effort``` - The max effort (torque) to apply to the joint while in position or velocity control mode, default: 100.0 - NOTE: max_effort parameter is ignored when effort commands are given. - -```~max_effort_vel_mode``` - Deprecated parameter, use max_effort instead, backwards compatibility is provided, however please change your code asap - -```~use_intertia_from_file``` - If True pybullet will compute the inertia tensor based on mass and volume of the collision shape, default: False - -```~robot_pose_x``` - The position where to spawn the robot in the world in m, default: 0.0 - -```~robot_pose_y``` - The position where to spawn the robot in the world in m, default: 0.0 - -```~robot_pose_z``` - The position where to spawn the robot in the world in m, default: 1.0 - -```~robot_pose_yaw``` - The orientation where to spawn the robot in the world, default: 0.0 - -```~fixed_base``` - If true, the first link of the robot will be fixed to the center of the world, useful for non movable robots default: False - -```~use_deformable_world``` - Set this paramter to true in case you require soft body simulation, default: False - -```~environment``` - The name of the python file (has to be placed inside plugins folder) without the .py extension that implements the necessary - custom functions to load an environment via python code, e.g using functions like self.pb.loadURDF(...) - See "environment" section below for more details. - -```~plugin_import_prefix``` - Allow environment plugins located in external packages to be recognized by pybullet ros. The line executed would look like this: - from my_external_ros_pkg. import Environment # default: pybullet_ros.plugins - -```~gui_options``` - Expose gui options to the user, for example to be able to maximize screen -> options="--width=2560 --height=1440". - The line of code in pybullet for the presented example would look like this: ```physicsClient = p.connect(p.GUI, options="--width=2560 --height=1440")``` - -# pybullet ros plugins - -What is a pybullet ros plugin? - -At the core of pybullet ros, we have the following workflow: - -drawing - -Basically we iterate over all registered plugins, run their execute function, and after doing it for all plugins we step the simulation one time step. - -# Plugin creation - -This section shows you how you can create your own plugin, to extend this library with your own needs. - -NOTE: Before creating a pybullet_ros plugin, make sure your plugin does not exist already -[check available pybullet_ros plugins here](https://github.com/oscar-lima/pybullet_ros/tree/noetic/ros/src/pybullet_ros/plugins). - -To ease the process, we provide with a template [here](https://github.com/oscar-lima/pybullet_ros/blob/noetic/ros/src/pybullet_ros/plugins/plugin_template.py). - -Copy the template and follow this instructions: - -1. roscd pybullet_ros/ros/src/pybullet_ros/plugins && cp plugin_template.py my_awesome_plugin.py +Two sample robots are provided for testing: **acrobat** and **r2d2**. Their URDF models live under `common/test/urdf`. -2. add it to param server +### Bring up the r2d2 robot - roscd pybullet_ros/ros/config && gedit pybullet_params_example.yaml +```bash +ros2 launch pybullet_ros bringup_robot_example.launch.py +``` -Extend "plugins" param to add yours, e.g: +A pybullet window should appear with the robot loaded. The launch file also starts `robot_state_publisher`, so TF data is available out of the box. - plugins: { pybullet_ros.plugins.body_vel_control: cmdVelCtrl, - pybullet_ros.plugins.odometry: simpleOdometry, - pybullet_ros.plugins.control: Control} +### Send position control commands to the robot - plugins: { pybullet_ros.plugins.body_vel_control: cmdVelCtrl, - pybullet_ros.plugins.odometry: simpleOdometry, - pybullet_ros.plugins.control: Control, - pybullet_ros.plugins.plugin_template: pluginTemplate} +You can command the joints directly by publishing `std_msgs/msg/Float64` messages. For example, to move the head swivel joint: -3. Thats all! you receive a pointer to the robot and to the import of pybullet itself, e.g. +```bash +ros2 topic pub --once /head_swivel_position_controller/command std_msgs/msg/Float64 "{data: 1.0}" +``` - import pybullet as pb -> self.pb - self.robot -> self.pb.loadURDF(urdf_path, basePosition=[0,0,0], baseOrientation=[0,0,0,1], ...) +A convenience GUI is also provided: -Using the pybullet [documentation](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#) you should be able -to access all the functionality that the pybullet api provides. +```bash +ros2 launch pybullet_ros tools/position_cmd_gui.launch.py +``` -# Environment plugin +Use the sliders to command joint angles in position control mode. -To load an environment (URDF, SDF, etc) we provide with a particular one time plugin under "plugins/environment.py". +### Send velocity or effort control commands -This is loaded during runtime via importlib based upon the value of the ```~environment``` parameter. +The control plugin exposes velocity and effort interfaces as well: -The recommended way is to set the "environment" parameter to point to a python file which has to be placed under "plugins" folder (just as any other plugin). +```bash +ros2 topic pub --once /head_swivel_velocity_controller/command std_msgs/msg/Float64 "{data: 2.0}" +ros2 topic pub --once /head_swivel_effort_controller/command std_msgs/msg/Float64 "{data: -2000.0}" +``` -Then set the environment parameter to be a string with the name of your python file, e.g. ```my_env.py``` but without the .py, therefore only : ```my_env```. +## Visualise the robot in RViz -Then inside my_env.py inherit from Environment class provided in plugins/environment.py and override the ```load_environment_via_code``` method. +A ready-to-use configuration file is provided: -A template is provided under plugins/environment_template.py to ease the process. +```bash +ros2 run rviz2 rviz2 --display-config $(ros2 pkg prefix pybullet_ros)/share/pybullet_ros/config/pybullet_config.rviz +``` -As mentioned before, the code inside the method "load_environment_via_code" will be called one time only during puybullet startup sequence. +## Topics -## NOTE about the multiple r2d2 urdf models in the web +- `/joint_states` (`sensor_msgs/msg/JointState`) – published at the configured loop rate. +- `/scan` (`sensor_msgs/msg/LaserScan`) – laser scanner readings. +- `/odom` (`nav_msgs/msg/Odometry`) – base odometry. +- `/cmd_vel` (`geometry_msgs/msg/Twist`) – body velocity commands. +- `/rgb_image` (`sensor_msgs/msg/Image`) – RGB camera output. +- `__controller/command` (`std_msgs/msg/Float64`) – joint control commands (`type` is `position`, `velocity` or `effort`). -As you might have already noticed, there are multiple r2d2 urdf models in the web, for instance the one that -ROS [uses](http://wiki.ros.org/urdf/Tutorials/Building%20a%20Visual%20Robot%20Model%20with%20URDF%20from%20Scratch) to -teach new users about URDF, however is missing collision and inertia tags. Another one can be found under pybullet repo -[data folder](https://github.com/bulletphysics/bullet3/blob/master/data/r2d2.urdf) but that model does not follow -[ROS conventions](https://www.ros.org/reps/rep-0103.html#axis-orientation), in particular "x" axis moves the robot forward and "y" axis moves it to the left. -We have created our own r2d2 and included a lidar on its base to be able to test the laser scanner plugin. +## Services -## Wait a second... bullet is already integrated in gazebo. Why do I need this repository at all? +The node provides the following services (all using `std_srvs/srv/Empty`): -Well thats true, bullet is integrated in gazebo, they have much more plugins available and their api runs much faster as it is implemented in C++. +```bash +ros2 service call /pybullet_ros/reset_simulation std_srvs/srv/Empty {} +ros2 service call /pybullet_ros/pause_physics std_srvs/srv/Empty {} +ros2 service call /pybullet_ros/unpause_physics std_srvs/srv/Empty {} +``` -I myself also use gazebo on a daily basis! , however probably some reasons why this repository be useful are because is very easy and fast to configure a rapid prototype. +## Extending with plugins -Your urdf model does not need to be extended with gazebo tags, installation is extremely easy from pypi and there are lots of examples in pybullet available (see [here](https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/examples)). Additionally, its in python! so is easier and faster to develop + the pybullet documentation is better. +Plugins are regular Python classes loaded at runtime. See `pybullet_ros/plugins/plugin_template.py` for a minimal example and `config/pybullet_params_example.yaml` for how plugins are configured. diff --git a/ros/config/pybullet_config.rviz b/config/pybullet_config.rviz similarity index 100% rename from ros/config/pybullet_config.rviz rename to config/pybullet_config.rviz diff --git a/ros/config/pybullet_params_example.yaml b/config/pybullet_params_example.yaml similarity index 100% rename from ros/config/pybullet_params_example.yaml rename to config/pybullet_params_example.yaml diff --git a/config/robots/acrobat_robot_config.yaml b/config/robots/acrobat_robot_config.yaml new file mode 100644 index 0000000..1032883 --- /dev/null +++ b/config/robots/acrobat_robot_config.yaml @@ -0,0 +1,21 @@ +plugins: + - + module: pybullet_ros.plugins.control + class: Control + - + module: pybullet_ros.plugins.joint_state_pub + class: joinStatePub + - + module: pybullet_ros.plugins.rgbd_camera + class: RGBDCamera + +loop_rate: 80.0 +gravity: -9.81 +max_effort: 5000.0 +use_intertia_from_file: False + +rgbd_camera: + frame_id: tip_link + resolution: + width: 640 + height: 480 diff --git a/launch/bringup_robot_example.launch.py b/launch/bringup_robot_example.launch.py new file mode 100644 index 0000000..ce091a3 --- /dev/null +++ b/launch/bringup_robot_example.launch.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, Command +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +def generate_launch_description(): + pkg_share = get_package_share_directory('pybullet_ros') + default_config = os.path.join(pkg_share, 'config', 'pybullet_params_example.yaml') + default_urdf = os.path.join(pkg_share, 'common', 'test', 'urdf', 'r2d2_robot', 'r2d2.urdf.xacro') + pybullet_launch = os.path.join(pkg_share, 'launch', 'pybullet_ros_example.launch.py') + + robot_description = ParameterValue( + Command(['xacro', LaunchConfiguration('robot_urdf_path')]), + value_type=str, + ) + + return LaunchDescription([ + DeclareLaunchArgument('config_file', default_value=default_config, + description='YAML configuration file for pybullet_ros'), + DeclareLaunchArgument('plugin_import_prefix', default_value='pybullet_ros.plugins', + description='Python module prefix used for plugin imports'), + DeclareLaunchArgument('environment', default_value='environment', + description='Environment plugin name'), + DeclareLaunchArgument('pybullet_gui', default_value='true', + description='Enable pybullet GUI'), + DeclareLaunchArgument('robot_urdf_path', default_value=default_urdf, + description='Robot URDF or xacro file to load'), + DeclareLaunchArgument('pause_simulation', default_value='false', + description='Start simulation paused'), + DeclareLaunchArgument('parallel_plugin_execution', default_value='true', + description='Execute plugins in parallel'), + DeclareLaunchArgument('robot_pose_x', default_value='0.0'), + DeclareLaunchArgument('robot_pose_y', default_value='0.0'), + DeclareLaunchArgument('robot_pose_z', default_value='0.7'), + DeclareLaunchArgument('robot_pose_yaw', default_value='0.0'), + DeclareLaunchArgument('fixed_base', default_value='false'), + DeclareLaunchArgument('use_deformable_world', default_value='false'), + DeclareLaunchArgument('gui_options', default_value=''), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{'robot_description': robot_description}], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(pybullet_launch), + launch_arguments={ + 'config_file': LaunchConfiguration('config_file'), + 'plugin_import_prefix': LaunchConfiguration('plugin_import_prefix'), + 'environment': LaunchConfiguration('environment'), + 'pybullet_gui': LaunchConfiguration('pybullet_gui'), + 'robot_urdf_path': LaunchConfiguration('robot_urdf_path'), + 'pause_simulation': LaunchConfiguration('pause_simulation'), + 'parallel_plugin_execution': LaunchConfiguration('parallel_plugin_execution'), + 'robot_pose_x': LaunchConfiguration('robot_pose_x'), + 'robot_pose_y': LaunchConfiguration('robot_pose_y'), + 'robot_pose_z': LaunchConfiguration('robot_pose_z'), + 'robot_pose_yaw': LaunchConfiguration('robot_pose_yaw'), + 'fixed_base': LaunchConfiguration('fixed_base'), + 'use_deformable_world': LaunchConfiguration('use_deformable_world'), + 'gui_options': LaunchConfiguration('gui_options'), + }.items(), + ), + ]) diff --git a/launch/pybullet_ros_example.launch.py b/launch/pybullet_ros_example.launch.py new file mode 100644 index 0000000..6a6d625 --- /dev/null +++ b/launch/pybullet_ros_example.launch.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +def generate_launch_description(): + pkg_share = get_package_share_directory('pybullet_ros') + default_config = os.path.join(pkg_share, 'config', 'pybullet_params_example.yaml') + default_urdf = os.path.join(pkg_share, 'common', 'test', 'urdf', 'r2d2_robot', 'r2d2.urdf') + + return LaunchDescription([ + DeclareLaunchArgument('config_file', default_value=default_config, + description='YAML configuration file for pybullet_ros'), + DeclareLaunchArgument('plugin_import_prefix', default_value='pybullet_ros.plugins', + description='Python module prefix used for plugin imports'), + DeclareLaunchArgument('environment', default_value='environment', + description='Environment plugin name'), + DeclareLaunchArgument('pybullet_gui', default_value='true', + description='Enable pybullet GUI'), + DeclareLaunchArgument('robot_urdf_path', default_value=default_urdf, + description='Robot URDF file to load'), + DeclareLaunchArgument('pause_simulation', default_value='false', + description='Start simulation paused'), + DeclareLaunchArgument('parallel_plugin_execution', default_value='true', + description='Execute plugins in parallel'), + DeclareLaunchArgument('robot_pose_x', default_value='0.0', + description='Robot spawn position X'), + DeclareLaunchArgument('robot_pose_y', default_value='0.0', + description='Robot spawn position Y'), + DeclareLaunchArgument('robot_pose_z', default_value='1.0', + description='Robot spawn position Z'), + DeclareLaunchArgument('robot_pose_yaw', default_value='0.0', + description='Robot spawn yaw'), + DeclareLaunchArgument('fixed_base', default_value='false', + description='Spawn robot with fixed base'), + DeclareLaunchArgument('use_deformable_world', default_value='false', + description='Enable pybullet soft body simulation'), + DeclareLaunchArgument('gui_options', default_value='', + description='Additional GUI options passed to pybullet'), + Node( + package='pybullet_ros', + executable='pybullet_ros_node', + name='pybullet_ros', + output='screen', + parameters=[ + { + 'config_file': LaunchConfiguration('config_file'), + 'plugin_import_prefix': LaunchConfiguration('plugin_import_prefix'), + 'environment': LaunchConfiguration('environment'), + 'pybullet_gui': ParameterValue(LaunchConfiguration('pybullet_gui'), value_type=bool), + 'robot_urdf_path': LaunchConfiguration('robot_urdf_path'), + 'pause_simulation': ParameterValue(LaunchConfiguration('pause_simulation'), value_type=bool), + 'parallel_plugin_execution': ParameterValue( + LaunchConfiguration('parallel_plugin_execution'), value_type=bool + ), + 'robot_pose_x': ParameterValue(LaunchConfiguration('robot_pose_x'), value_type=float), + 'robot_pose_y': ParameterValue(LaunchConfiguration('robot_pose_y'), value_type=float), + 'robot_pose_z': ParameterValue(LaunchConfiguration('robot_pose_z'), value_type=float), + 'robot_pose_yaw': ParameterValue(LaunchConfiguration('robot_pose_yaw'), value_type=float), + 'fixed_base': ParameterValue(LaunchConfiguration('fixed_base'), value_type=bool), + 'use_deformable_world': ParameterValue( + LaunchConfiguration('use_deformable_world'), value_type=bool + ), + 'gui_options': LaunchConfiguration('gui_options'), + } + ], + ), + ]) diff --git a/launch/robots/acrobat_bringup.launch.py b/launch/robots/acrobat_bringup.launch.py new file mode 100644 index 0000000..ef76412 --- /dev/null +++ b/launch/robots/acrobat_bringup.launch.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + pkg_share = get_package_share_directory('pybullet_ros') + default_config = os.path.join(pkg_share, 'config', 'robots', 'acrobat_robot_config.yaml') + default_urdf = os.path.join(pkg_share, 'common', 'test', 'urdf', 'acrobat_robot', 'acrobat_robot.urdf') + bringup_launch = os.path.join(pkg_share, 'launch', 'bringup_robot_example.launch.py') + tools_launch = os.path.join(pkg_share, 'launch', 'tools', 'position_cmd_gui.launch.py') + + return LaunchDescription([ + DeclareLaunchArgument('config_file', default_value=default_config, + description='Configuration for the acrobat robot'), + DeclareLaunchArgument('robot_urdf_path', default_value=default_urdf, + description='URDF file for the acrobat robot'), + DeclareLaunchArgument('robot_pose_z', default_value='0.0'), + DeclareLaunchArgument('fixed_base', default_value='true'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(bringup_launch), + launch_arguments={ + 'config_file': LaunchConfiguration('config_file'), + 'robot_urdf_path': LaunchConfiguration('robot_urdf_path'), + 'robot_pose_z': LaunchConfiguration('robot_pose_z'), + 'fixed_base': LaunchConfiguration('fixed_base'), + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(tools_launch), + ), + ]) diff --git a/launch/tools/position_cmd_gui.launch.py b/launch/tools/position_cmd_gui.launch.py new file mode 100644 index 0000000..56b770e --- /dev/null +++ b/launch/tools/position_cmd_gui.launch.py @@ -0,0 +1,21 @@ +#!/usr/bin/env python3 + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + name='position_ctrl_cmd_gui', + remappings=[('joint_states', 'fake_joint_states')], + ), + Node( + package='pybullet_ros', + executable='joint_states_to_float', + name='position_ctrl_repub', + output='screen', + ), + ]) diff --git a/package.xml b/package.xml index 0d0290d..b9c3c17 100644 --- a/package.xml +++ b/package.xml @@ -1,19 +1,32 @@ - - pybullet_ros - 2.0.0 - - ROS wrapper for pybullet simulator - + + pybullet_ros + 2.0.0 + ROS wrapper for pybullet simulator - MIT + Oscar Lima + Oscar Lima + MIT - Oscar Lima - Oscar Lima + ament_cmake + ament_cmake_python - catkin - std_msgs - cv_bridge - joint_state_publisher_gui + ament_index_python + geometry_msgs + nav_msgs + rclpy + sensor_msgs + std_msgs + std_srvs + tf2_ros + cv_bridge + python3-yaml + xacro + pybullet + pybullet_data + + + ament_cmake + diff --git a/ros/src/pybullet_ros/__init__.py b/pybullet_ros/__init__.py similarity index 100% rename from ros/src/pybullet_ros/__init__.py rename to pybullet_ros/__init__.py diff --git a/ros/src/pybullet_ros/function_exec_manager.py b/pybullet_ros/function_exec_manager.py similarity index 100% rename from ros/src/pybullet_ros/function_exec_manager.py rename to pybullet_ros/function_exec_manager.py diff --git a/pybullet_ros/plugins/__init__.py b/pybullet_ros/plugins/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pybullet_ros/plugins/body_vel_control.py b/pybullet_ros/plugins/body_vel_control.py new file mode 100644 index 0000000..a315512 --- /dev/null +++ b/pybullet_ros/plugins/body_vel_control.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python3 + +"""Subscribe to cmd_vel and apply desired speed to the robot without any noise.""" + +import math +from typing import Optional + +import numpy as np +from geometry_msgs.msg import Twist, Vector3, Vector3Stamped +from rclpy.duration import Duration + + +class CmdVelCtrl: + def __init__(self, node, pybullet, robot, **kwargs): + self.node = node + self.logger = node.get_logger() + self.pb = pybullet + self.robot = robot + self.cmd_vel_msg: Optional[Twist] = None + self.received_cmd_vel_time = None + + topic = kwargs.get('parameters', {}).get('cmd_vel', 'cmd_vel') + self.subscription = node.create_subscription(Twist, topic, self.cmd_vel_callback, 10) + + def translation_matrix(self, direction): + M = np.identity(4) + M[:3, 3] = direction[:3] + return M + + def quaternion_matrix(self, quaternion): + epsilon = np.finfo(float).eps * 4.0 + q = np.array(quaternion[:4], dtype=np.float64, copy=True) + nq = np.dot(q, q) + if nq < epsilon: + return np.identity(4) + q *= math.sqrt(2.0 / nq) + q = np.outer(q, q) + return np.array( + ( + (1.0 - q[1, 1] - q[2, 2], q[0, 1] - q[2, 3], q[0, 2] + q[1, 3], 0.0), + (q[0, 1] + q[2, 3], 1.0 - q[0, 0] - q[2, 2], q[1, 2] - q[0, 3], 0.0), + (q[0, 2] - q[1, 3], q[1, 2] + q[0, 3], 1.0 - q[0, 0] - q[1, 1], 0.0), + (0.0, 0.0, 0.0, 1.0), + ), + dtype=np.float64, + ) + + def from_translation_rotation(self, translation, rotation): + return np.dot(self.translation_matrix(translation), self.quaternion_matrix(rotation)) + + def lookup_transform(self, target_frame='base_link', source_frame='odom'): + position, orientation = self.pb.getBasePositionAndOrientation(self.robot) + return [position[0], position[1], position[2]], [orientation[0], orientation[1], orientation[2], orientation[3]] + + def as_matrix(self, target_frame, hdr): + translation, rotation = self.lookup_transform(target_frame, hdr.frame_id) + return self.from_translation_rotation(translation, rotation) + + def transform_vector3(self, target_frame, v3s: Vector3Stamped): + mat44 = self.as_matrix(target_frame, v3s.header) + mat44[0, 3] = 0.0 + mat44[1, 3] = 0.0 + mat44[2, 3] = 0.0 + xyz = tuple(np.dot(mat44, np.array([v3s.vector.x, v3s.vector.y, v3s.vector.z, 1.0])))[:3] + result = Vector3Stamped() + result.header.stamp = v3s.header.stamp + result.header.frame_id = target_frame + result.vector = Vector3(*xyz) + return result + + def cmd_vel_callback(self, msg: Twist) -> None: + self.cmd_vel_msg = msg + self.received_cmd_vel_time = self.node.get_clock().now() + + def execute(self) -> None: + if self.cmd_vel_msg is None or self.received_cmd_vel_time is None: + return + if self.node.get_clock().now() - self.received_cmd_vel_time > Duration(seconds=0.5): + return + lin_vec = Vector3Stamped() + lin_vec.header.frame_id = 'base_link' + lin_vec.vector.x = self.cmd_vel_msg.linear.x + lin_vec.vector.y = self.cmd_vel_msg.linear.y + lin_vec.vector.z = self.cmd_vel_msg.linear.z + ang_vec = Vector3Stamped() + ang_vec.header.frame_id = 'base_link' + ang_vec.vector.x = self.cmd_vel_msg.angular.x + ang_vec.vector.y = self.cmd_vel_msg.angular.y + ang_vec.vector.z = self.cmd_vel_msg.angular.z + lin_cmd_vel_in_odom = self.transform_vector3('odom', lin_vec) + ang_cmd_vel_in_odom = self.transform_vector3('odom', ang_vec) + self.pb.resetBaseVelocity( + self.robot, + [lin_cmd_vel_in_odom.vector.x, lin_cmd_vel_in_odom.vector.y, lin_cmd_vel_in_odom.vector.z], + [ang_cmd_vel_in_odom.vector.x, ang_cmd_vel_in_odom.vector.y, ang_cmd_vel_in_odom.vector.z], + ) + + +# Backwards compatibility alias +cmdVelCtrl = CmdVelCtrl diff --git a/pybullet_ros/plugins/control.py b/pybullet_ros/plugins/control.py new file mode 100644 index 0000000..ec2eccc --- /dev/null +++ b/pybullet_ros/plugins/control.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 + +"""Position, velocity and effort control for all controllable joints.""" + +from std_msgs.msg import Float64 + + +class PVEControl: + def __init__(self, node, joint_index, joint_name, controller_type): + assert controller_type in ['position', 'velocity', 'effort'] + topic = f'{joint_name}_{controller_type}_controller/command' + self.subscription = node.create_subscription(Float64, topic, self.pve_control_cb, 10) + self.cmd = 0.0 + self.data_available = False + self.joint_index = joint_index + self.joint_name = joint_name + + def pve_control_cb(self, msg: Float64): + self.data_available = True + self.cmd = msg.data + + def get_last_cmd(self): + self.data_available = False + return self.cmd + + def get_is_data_available(self): + return self.data_available + + def get_joint_index(self): + return self.joint_index + + +class Control: + def __init__(self, node, pybullet, robot, **kwargs): + self.node = node + self.logger = node.get_logger() + self.pb = pybullet + self.robot = robot + + global_params = kwargs.get('global_parameters', {}) + max_effort = global_params.get('max_effort', 100.0) + if 'max_effort_vel_mode' in global_params: + self.logger.warn('max_effort_vel_mode parameter is deprecated, please use max_effort instead') + max_effort = global_params.get('max_effort_vel_mode', max_effort) + + self.joint_index_name_dic = {**kwargs['rev_joints'], **kwargs['prism_joints']} + self.position_joint_commands = [] + self.velocity_joint_commands = [] + self.effort_joint_commands = [] + self.force_commands = [] + self.pc_subscribers = [] + self.vc_subscribers = [] + self.ec_subscribers = [] + self.joint_indices = [] + + for joint_index, joint_name in self.joint_index_name_dic.items(): + self.position_joint_commands.append(0.0) + self.velocity_joint_commands.append(0.0) + self.effort_joint_commands.append(0.0) + self.force_commands.append(max_effort) + self.joint_indices.append(joint_index) + self.pc_subscribers.append(PVEControl(node, joint_index, joint_name, 'position')) + self.vc_subscribers.append(PVEControl(node, joint_index, joint_name, 'velocity')) + self.ec_subscribers.append(PVEControl(node, joint_index, joint_name, 'effort')) + + def execute(self): + position_ctrl_task = False + velocity_ctrl_task = False + effort_ctrl_task = False + + for index, subscriber in enumerate(self.pc_subscribers): + if subscriber.get_is_data_available(): + self.position_joint_commands[index] = subscriber.get_last_cmd() + position_ctrl_task = True + for index, subscriber in enumerate(self.vc_subscribers): + if subscriber.get_is_data_available(): + self.velocity_joint_commands[index] = subscriber.get_last_cmd() + velocity_ctrl_task = True + for index, subscriber in enumerate(self.ec_subscribers): + if subscriber.get_is_data_available(): + self.effort_joint_commands[index] = subscriber.get_last_cmd() + effort_ctrl_task = True + + if position_ctrl_task: + self.pb.setJointMotorControlArray( + bodyUniqueId=self.robot, + jointIndices=self.joint_indices, + controlMode=self.pb.POSITION_CONTROL, + targetPositions=self.position_joint_commands, + forces=self.force_commands, + ) + elif velocity_ctrl_task: + self.pb.setJointMotorControlArray( + bodyUniqueId=self.robot, + jointIndices=self.joint_indices, + controlMode=self.pb.VELOCITY_CONTROL, + targetVelocities=self.velocity_joint_commands, + forces=self.force_commands, + ) + elif effort_ctrl_task: + self.pb.setJointMotorControlArray( + bodyUniqueId=self.robot, + jointIndices=self.joint_indices, + controlMode=self.pb.TORQUE_CONTROL, + forces=self.effort_joint_commands, + ) diff --git a/pybullet_ros/plugins/diff_drive.py b/pybullet_ros/plugins/diff_drive.py new file mode 100644 index 0000000..cfb8995 --- /dev/null +++ b/pybullet_ros/plugins/diff_drive.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 + +from geometry_msgs.msg import Twist + + +class DiffDrive: + def __init__(self, node, pybullet, robot, **kwargs): + self.node = node + self.logger = node.get_logger() + self.pb = pybullet + self.robot = robot + + parameters = kwargs.get('parameters', {}) + + self.revolute_joints_id_name = kwargs['rev_joints'] + self.revolute_joints_name_id = {v: k for k, v in self.revolute_joints_id_name.items()} + + self.no_execution = False + + cmd_vel_topic = parameters.get('cmd_vel', 'cmd_vel') + self.cmd_vel_subscriber = node.create_subscription(Twist, cmd_vel_topic, self.cmd_vel_callback, 10) + + self.wheel_separation = float(parameters.get('wheel_separation', 1.0)) + self.wheel_radius = float(parameters.get('wheel_radius', 1.0)) + + left_joints = parameters.get('left_joints') + right_joints = parameters.get('right_joints') + if left_joints is None or right_joints is None: + self.logger.error('Both left_joints and right_joints must be provided for DiffDrive controller') + self.no_execution = True + return + if isinstance(left_joints, str): + left_joints = [left_joints] + if isinstance(right_joints, str): + right_joints = [right_joints] + + self.joint_indices = {'left': [], 'right': []} + for side, joint_list in (('left', left_joints), ('right', right_joints)): + for joint_name in joint_list: + if joint_name not in self.revolute_joints_name_id: + self.logger.error(f'Could not find joint {joint_name} in URDF') + self.no_execution = True + return + self.joint_indices[side].append(self.revolute_joints_name_id[joint_name]) + + self.cmd_vel = Twist() + + def cmd_vel_callback(self, msg: Twist): + self.cmd_vel = msg + + def get_wheel_speeds(self): + vx = self.cmd_vel.linear.x + wz = self.cmd_vel.angular.z + left = (vx - wz * self.wheel_separation / 2.0) / self.wheel_radius + right = (vx + wz * self.wheel_separation / 2.0) / self.wheel_radius + return {'left': left, 'right': right} + + def execute(self): + if self.no_execution: + return + speeds = self.get_wheel_speeds() + for side in ['left', 'right']: + indices = self.joint_indices[side] + self.pb.setJointMotorControlArray( + bodyUniqueId=self.robot, + jointIndices=indices, + controlMode=self.pb.VELOCITY_CONTROL, + targetVelocities=[speeds[side] for _ in range(len(indices))], + ) diff --git a/pybullet_ros/plugins/environment.py b/pybullet_ros/plugins/environment.py new file mode 100644 index 0000000..93ee620 --- /dev/null +++ b/pybullet_ros/plugins/environment.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python3 + +"""Plugin responsible for loading the simulation environment.""" + +from typing import Any, Dict + + +class Environment: + def __init__(self, node, pybullet, **kwargs): + self.node = node + self.logger = node.get_logger() + self.pb = pybullet + self.config: Dict[str, Any] = kwargs.get('config', {}) + self.use_deformable_world = bool(kwargs.get('use_deformable_world', False)) + + if self.use_deformable_world: + self.logger.info('Using deformable world (soft body simulation)') + self.pb.resetSimulation(self.pb.RESET_USE_DEFORMABLE_WORLD) + + def load_environment(self) -> None: + gravity = float(self.config.get('gravity', -9.81)) + self.pb.setGravity(0, 0, gravity) + self.pb.loadURDF('plane.urdf') + self.load_environment_via_code() + + def load_environment_via_code(self) -> None: + """Override in derived classes to spawn custom objects.""" + # Intentionally left blank to be extended by user plugins. + return diff --git a/pybullet_ros/plugins/environment_template.py b/pybullet_ros/plugins/environment_template.py new file mode 100644 index 0000000..e083469 --- /dev/null +++ b/pybullet_ros/plugins/environment_template.py @@ -0,0 +1,12 @@ +from pybullet_ros.plugins.environment import Environment as DefaultEnv + + +class Environment(DefaultEnv): + def __init__(self, node, pybullet, **kwargs): + super().__init__(node, pybullet, **kwargs) + + def load_environment_via_code(self): + self.logger.warn('Loading custom environment via code!') + # Example: + # self.pb.loadURDF(...) + # self.pb.setTimeStep(0.001) diff --git a/pybullet_ros/plugins/joint_state_pub.py b/pybullet_ros/plugins/joint_state_pub.py new file mode 100644 index 0000000..a4b5362 --- /dev/null +++ b/pybullet_ros/plugins/joint_state_pub.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python3 + +"""Query robot state and publish position, velocity and effort values to /joint_states.""" + +from sensor_msgs.msg import JointState + + +class JoinStatePublisher: + def __init__(self, node, pybullet, robot, **kwargs): + self.node = node + self.logger = node.get_logger() + self.pb = pybullet + self.robot = robot + self.joint_index_name_dic = kwargs['rev_joints'] + self.publisher = node.create_publisher(JointState, 'joint_states', 10) + + def execute(self) -> None: + joint_msg = JointState() + for joint_index in self.joint_index_name_dic: + joint_state = self.pb.getJointState(self.robot, joint_index) + joint_msg.name.append(self.joint_index_name_dic[joint_index]) + joint_msg.position.append(joint_state[0]) + joint_msg.velocity.append(joint_state[1]) + joint_msg.effort.append(joint_state[3]) + joint_msg.header.stamp = self.node.get_clock().now().to_msg() + self.publisher.publish(joint_msg) + + +# Backwards compatibility alias +joinStatePub = JoinStatePublisher diff --git a/pybullet_ros/plugins/laser_scanner.py b/pybullet_ros/plugins/laser_scanner.py new file mode 100644 index 0000000..7959c6a --- /dev/null +++ b/pybullet_ros/plugins/laser_scanner.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python3 + +"""Laser scanner simulation based on pybullet rayTestBatch.""" + +import math +from typing import Dict, List + +import numpy as np +from sensor_msgs.msg import LaserScan + + +class LaserScanner: + def __init__(self, node, pybullet, robot, **kwargs): + self.node = node + self.logger = node.get_logger() + self.pb = pybullet + self.robot = robot + + link_ids: Dict[str, int] = kwargs['link_ids'] + laser_params = kwargs.get('parameters', {}) or kwargs.get('global_parameters', {}).get('laser', {}) + + laser_frame_id = laser_params.get('frame_id') + if not laser_frame_id: + self.logger.error('Parameter laser.frame_id is required to initialise the laser scanner plugin') + self.no_execution = True + return + if laser_frame_id not in link_ids: + self.logger.error(f'Laser frame "{laser_frame_id}" not found in URDF model') + self.logger.warn(f'Available frames are: {list(link_ids.keys())}') + self.no_execution = True + return + self.pb_laser_link_id = link_ids[laser_frame_id] + + angle_min = float(laser_params.get('angle_min', -1.5707963)) + angle_max = float(laser_params.get('angle_max', 1.5707963)) + assert angle_max > angle_min + + self.num_rays = int(laser_params.get('num_beams', 50)) + self.range_min = float(laser_params.get('range_min', 0.03)) + self.range_max = float(laser_params.get('range_max', 5.6)) + self.beam_visualisation = bool(laser_params.get('beam_visualisation', False)) + + self.publisher = node.create_publisher(LaserScan, 'scan', 10) + self.laser_msg = LaserScan() + self.laser_msg.header.frame_id = laser_frame_id + self.laser_msg.angle_min = angle_min + self.laser_msg.angle_max = angle_max + self.laser_msg.angle_increment = (angle_max - angle_min) / self.num_rays + self.laser_msg.range_min = self.range_min + self.laser_msg.range_max = self.range_max + self.laser_msg.time_increment = float(laser_params.get('time_increment', 0.01)) + self.laser_msg.scan_time = float(laser_params.get('scan_time', 0.1)) + + self.ray_hit_color = [1, 0, 0] + self.ray_miss_color = [0, 1, 0] + + self.ray_from, self.ray_to = self.prepare_rays() + self.count = 0 + self.no_execution = False + + def prepare_rays(self): + ray_from: List[List[float]] = [] + ray_to: List[List[float]] = [] + for n in range(0, self.num_rays): + alpha = self.laser_msg.angle_min + n * self.laser_msg.angle_increment + ray_from.append([self.range_min * math.cos(alpha), self.range_min * math.sin(alpha), 0.0]) + ray_to.append([self.range_max * math.cos(alpha), self.range_max * math.sin(alpha), 0.0]) + return ray_from, ray_to + + def transform_rays(self, laser_position, laser_orientation): + laser_position = [laser_position[0], laser_position[1], laser_position[2]] + tf_ray_from: List[List[float]] = [] + tf_ray_to: List[List[float]] = [] + rm = self.pb.getMatrixFromQuaternion(laser_orientation) + rotation_matrix = [[rm[0], rm[1], rm[2]], [rm[3], rm[4], rm[5]], [rm[6], rm[7], rm[8]]] + for ray in self.ray_from: + position = np.dot(rotation_matrix, [ray[0], ray[1], ray[2]]) + laser_position + tf_ray_from.append([position[0], position[1], position[2]]) + for ray in self.ray_to: + position = np.dot(rotation_matrix, [ray[0], ray[1], ray[2]]) + laser_position + tf_ray_to.append([position[0], position[1], position[2]]) + return tf_ray_from, tf_ray_to + + def execute(self) -> None: + if getattr(self, 'no_execution', False): + return + self.count += 1 + if self.count < 100: + return + self.count = 0 + self.laser_msg.ranges = [] + if self.beam_visualisation: + self.pb.removeAllUserDebugItems() + laser_state = self.pb.getLinkState(self.robot, self.pb_laser_link_id) + ray_from, ray_to = self.transform_rays(laser_state[0], laser_state[1]) + results = self.pb.rayTestBatch(ray_from, ray_to, 4) + for i in range(self.num_rays): + if self.beam_visualisation: + hit_object_uid = results[i][0] + if hit_object_uid < 0: + self.pb.addUserDebugLine(ray_from[i], ray_to[i], self.ray_miss_color) + else: + self.pb.addUserDebugLine(ray_from[i], results[i][3], self.ray_hit_color) + self.laser_msg.ranges.append(results[i][2] * self.laser_msg.range_max) + self.laser_msg.header.stamp = self.node.get_clock().now().to_msg() + self.publisher.publish(self.laser_msg) + + +# Backwards compatibility alias +laserScanner = LaserScanner diff --git a/pybullet_ros/plugins/odometry.py b/pybullet_ros/plugins/odometry.py new file mode 100644 index 0000000..1397197 --- /dev/null +++ b/pybullet_ros/plugins/odometry.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 + +"""Query robot base pose and speed from pybullet and publish to /odom.""" + +from geometry_msgs.msg import TransformStamped +from nav_msgs.msg import Odometry +from tf2_ros import TransformBroadcaster + + +class SimpleOdometry: + def __init__(self, node, pybullet, robot, **kwargs): + self.node = node + self.logger = node.get_logger() + self.pb = pybullet + self.robot = robot + self.publisher = node.create_publisher(Odometry, 'odom', 10) + params = kwargs.get('global_parameters', {}) + self.odom_msg = Odometry() + self.odom_msg.header.frame_id = params.get('odom_frame', 'odom') + self.odom_msg.child_frame_id = params.get('robot_base_frame', 'base_link') + self.tf_broadcaster = TransformBroadcaster(node) + + def execute(self): + now = self.node.get_clock().now().to_msg() + self.odom_msg.header.stamp = now + position, orientation = self.pb.getBasePositionAndOrientation(self.robot) + self.odom_msg.pose.pose.position.x = position[0] + self.odom_msg.pose.pose.position.y = position[1] + self.odom_msg.pose.pose.position.z = position[2] + self.odom_msg.pose.pose.orientation.x = orientation[0] + self.odom_msg.pose.pose.orientation.y = orientation[1] + self.odom_msg.pose.pose.orientation.z = orientation[2] + self.odom_msg.pose.pose.orientation.w = orientation[3] + linear, angular = self.pb.getBaseVelocity(self.robot) + self.odom_msg.twist.twist.linear.x = linear[0] + self.odom_msg.twist.twist.linear.y = linear[1] + self.odom_msg.twist.twist.linear.z = linear[2] + self.odom_msg.twist.twist.angular.x = angular[0] + self.odom_msg.twist.twist.angular.y = angular[1] + self.odom_msg.twist.twist.angular.z = angular[2] + self.publisher.publish(self.odom_msg) + + transform = TransformStamped() + transform.header.stamp = now + transform.header.frame_id = self.odom_msg.header.frame_id + transform.child_frame_id = self.odom_msg.child_frame_id + transform.transform.translation.x = position[0] + transform.transform.translation.y = position[1] + transform.transform.translation.z = position[2] + transform.transform.rotation.x = orientation[0] + transform.transform.rotation.y = orientation[1] + transform.transform.rotation.z = orientation[2] + transform.transform.rotation.w = orientation[3] + self.tf_broadcaster.sendTransform(transform) + + +# Backwards compatibility alias +simpleOdometry = SimpleOdometry diff --git a/pybullet_ros/plugins/plugin_template.py b/pybullet_ros/plugins/plugin_template.py new file mode 100644 index 0000000..7ec2e7b --- /dev/null +++ b/pybullet_ros/plugins/plugin_template.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python3 + +"""Example structure for a pybullet_ros plugin.""" + + +class PluginTemplate: + def __init__(self, node, pybullet, robot, **kwargs): + self.node = node + self.logger = node.get_logger() + self.pb = pybullet + self.robot = robot + # TODO: implement plugin initialisation here + + def execute(self): + """Called from the pybullet ros update loop.""" + self.logger.info('My plugin is running!') + + +# Backwards compatibility alias +pluginTemplate = PluginTemplate diff --git a/pybullet_ros/plugins/rgbd_camera.py b/pybullet_ros/plugins/rgbd_camera.py new file mode 100644 index 0000000..8794284 --- /dev/null +++ b/pybullet_ros/plugins/rgbd_camera.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 + +"""RGBD camera sensor simulation for pybullet_ros.""" + +import math +from typing import Dict + +import numpy as np +from cv_bridge import CvBridge +from sensor_msgs.msg import Image + + +class RGBDCamera: + def __init__(self, node, pybullet, robot, **kwargs): + self.node = node + self.logger = node.get_logger() + self.pb = pybullet + self.robot = robot + + link_ids: Dict[str, int] = kwargs['link_ids'] + rgbd_config = kwargs.get('parameters', {}) or kwargs.get('global_parameters', {}).get('rgbd_camera', {}) + + resolution = rgbd_config.get('resolution', {}) + self.image_msg = Image() + self.image_msg.width = int(resolution.get('width', 640)) + self.image_msg.height = int(resolution.get('height', 480)) + if self.image_msg.width < 5 or self.image_msg.height < 5: + raise ValueError('RGBD camera resolution must be greater than 5 pixels') + + cam_frame_id = rgbd_config.get('frame_id') + if not cam_frame_id: + raise RuntimeError('rgbd_camera.frame_id parameter is required') + if cam_frame_id not in link_ids: + raise RuntimeError(f'Camera frame "{cam_frame_id}" not found in URDF model') + self.pb_camera_link_id = link_ids[cam_frame_id] + self.image_msg.header.frame_id = cam_frame_id + + self.publisher = node.create_publisher(Image, 'rgb_image', 10) + self.image_msg.encoding = rgbd_config.get('encoding', 'rgb8') + self.image_msg.is_bigendian = int(rgbd_config.get('is_bigendian', 0)) + self.image_msg.step = int(rgbd_config.get('step', self.image_msg.width * 3)) + + self.hfov = float(rgbd_config.get('hfov', 56.3)) + self.vfov = float(rgbd_config.get('vfov', 43.7)) + self.near_plane = float(rgbd_config.get('near_plane', 0.4)) + self.far_plane = float(rgbd_config.get('far_plane', 8.0)) + self.projection_matrix = self.compute_projection_matrix() + + self.image_bridge = CvBridge() + self.count = 0 + + def compute_projection_matrix(self): + return self.pb.computeProjectionMatrix( + left=-math.tan(math.pi * self.hfov / 360.0) * self.near_plane, + right=math.tan(math.pi * self.hfov / 360.0) * self.near_plane, + bottom=-math.tan(math.pi * self.vfov / 360.0) * self.near_plane, + top=math.tan(math.pi * self.vfov / 360.0) * self.near_plane, + nearVal=self.near_plane, + farVal=self.far_plane, + ) + + def extract_frame(self, camera_image): + bgr_image = np.zeros((self.image_msg.height, self.image_msg.width, 3)) + camera_image = np.reshape(camera_image[2], (camera_image[1], camera_image[0], 4)) + bgr_image[:, :, 2] = ( + (1 - camera_image[:, :, 3]) * camera_image[:, :, 2] + camera_image[:, :, 3] * camera_image[:, :, 2] + ) + bgr_image[:, :, 1] = ( + (1 - camera_image[:, :, 3]) * camera_image[:, :, 1] + camera_image[:, :, 3] * camera_image[:, :, 1] + ) + bgr_image[:, :, 0] = ( + (1 - camera_image[:, :, 3]) * camera_image[:, :, 0] + camera_image[:, :, 3] * camera_image[:, :, 0] + ) + return bgr_image.astype(np.uint8) + + def compute_camera_target(self, camera_position, camera_orientation): + target_point = [5.0, 0, 0] + camera_position = [camera_position[0], camera_position[1], camera_position[2]] + rm = self.pb.getMatrixFromQuaternion(camera_orientation) + rotation_matrix = [[rm[0], rm[1], rm[2]], [rm[3], rm[4], rm[5]], [rm[6], rm[7], rm[8]]] + return np.dot(rotation_matrix, target_point) + camera_position + + def execute(self): + self.count += 1 + if self.count < 100: + return + self.count = 0 + cam_state = self.pb.getLinkState(self.robot, self.pb_camera_link_id) + target = self.compute_camera_target(cam_state[0], cam_state[1]) + view_matrix = self.pb.computeViewMatrix(cam_state[0], target, [0, 0, 1]) + pybullet_cam_resp = self.pb.getCameraImage( + self.image_msg.width, + self.image_msg.height, + view_matrix, + self.projection_matrix, + renderer=self.pb.ER_BULLET_HARDWARE_OPENGL, + flags=self.pb.ER_NO_SEGMENTATION_MASK, + ) + frame = self.extract_frame(pybullet_cam_resp) + self.image_msg.data = self.image_bridge.cv2_to_imgmsg(frame).data + self.image_msg.header.stamp = self.node.get_clock().now().to_msg() + self.publisher.publish(self.image_msg) diff --git a/pybullet_ros/pybullet_ros.py b/pybullet_ros/pybullet_ros.py new file mode 100644 index 0000000..da92e1b --- /dev/null +++ b/pybullet_ros/pybullet_ros.py @@ -0,0 +1,296 @@ +#!/usr/bin/env python3 + +import importlib +import os +from threading import Event, Thread +from typing import Any, Dict, List + +import rclpy +from rclpy.node import Node +from std_srvs.srv import Empty + +from pybullet_ros.function_exec_manager import FuncExecManager + + +class PyBulletRosNode(Node): + """ROS 2 wrapper class for the pybullet simulator.""" + + def __init__(self) -> None: + super().__init__('pybullet_ros') + self.pb = importlib.import_module('pybullet') + + self.declare_parameter('config_file', '') + self.config_data = self._load_configuration() + + self.loop_rate = self._declare_with_config('loop_rate', 80.0) + self.plugin_import_prefix = self._declare_with_config('plugin_import_prefix', 'pybullet_ros.plugins') + self.environment_module = self._declare_with_config('environment', 'environment') + self.pybullet_gui = self._declare_with_config('pybullet_gui', True) + self.pause_simulation = self._declare_with_config('pause_simulation', False) + self.parallel_plugin_execution = self._declare_with_config('parallel_plugin_execution', True) + self.robot_pose_x = self._declare_with_config('robot_pose_x', 0.0) + self.robot_pose_y = self._declare_with_config('robot_pose_y', 0.0) + self.robot_pose_z = self._declare_with_config('robot_pose_z', 1.0) + self.robot_pose_yaw = self._declare_with_config('robot_pose_yaw', 0.0) + self.fixed_base = self._declare_with_config('fixed_base', False) + self.robot_urdf_path = self._declare_with_config('robot_urdf_path', '') + self.use_deformable_world = self._declare_with_config('use_deformable_world', False) + self.gui_options = self._declare_with_config('gui_options', '') + + self.plugins: List[Any] = [] + self.connected_to_physics_server: bool = False + self.physics_client = None + self._temp_urdf_files: List[str] = [] + + self._stop_event = Event() + self._executor_thread: Thread | None = None + self._timer = None + + self.create_service(Empty, 'reset_simulation', self.handle_reset_simulation) + self.create_service(Empty, 'pause_physics', self.handle_pause_physics) + self.create_service(Empty, 'unpause_physics', self.handle_unpause_physics) + + self._initialize_world() + self._load_plugins() + + def _declare_with_config(self, name: str, default: Any) -> Any: + value = self.config_data.get(name, default) + param = self.declare_parameter(name, value) + return param.value + + def _load_configuration(self) -> Dict[str, Any]: + config_file = self.get_parameter('config_file').value + if not config_file: + return {} + config_path = os.path.expanduser(os.path.expandvars(config_file)) + if not os.path.isfile(config_path): + self.get_logger().warning(f'Configuration file {config_file} does not exist') + return {} + try: + import yaml # type: ignore + except ImportError as exc: # pragma: no cover - dependency error is surfaced to the user + self.get_logger().error( + 'Unable to import yaml to parse configuration file. ' + 'Please install python3-yaml or remove the config_file parameter.' + ) + raise exc + with open(config_path, 'r', encoding='utf-8') as config_stream: + data = yaml.safe_load(config_stream) or {} + if not isinstance(data, dict): + self.get_logger().warning('Configuration file did not produce a dictionary, ignoring contents') + return {} + return data + + def _initialize_world(self) -> None: + self.get_logger().info('Starting pybullet...') + gui = bool(self.pybullet_gui) + self.physics_client = self._start_gui(gui=gui) + if self.physics_client < 0: + self.get_logger().error('Failed to connect to pybullet physics server') + return + self.connected_to_physics_server = True + self.pb.setAdditionalSearchPath(importlib.import_module('pybullet_data').getDataPath()) + self.environment = self._load_environment_plugin() + self.robot = self._init_pybullet_robot() + if not self.robot: + self.connected_to_physics_server = False + return + self.rev_joint_index_name_dic, self.prismatic_joint_index_name_dic, \ + self.fixed_joint_index_name_dic, self.link_names_to_ids_dic = self._get_properties() + + def _load_environment_plugin(self): + plugin_module = f'{self.plugin_import_prefix}.{self.environment_module}' + environment_cls = getattr(importlib.import_module(plugin_module), 'Environment') + return environment_cls(self, self.pb, use_deformable_world=self.use_deformable_world, + config=self.config_data) + + def _load_plugins(self) -> None: + plugin_entries = self.config_data.get('plugins', []) + if not plugin_entries: + self.get_logger().warn('No plugins configured; simulation will run without ROS interfaces') + return + for plugin in plugin_entries: + module_name = plugin.get('module') + class_name = plugin.get('class') + if not module_name or not class_name: + self.get_logger().warn('Plugin entry missing module or class; skipping') + continue + try: + module = importlib.import_module(module_name) + plugin_cls = getattr(module, class_name) + except (ImportError, AttributeError) as exc: + self.get_logger().error(f'Failed to import plugin {class_name} from {module_name}: {exc}') + continue + plugin_config = {key: value for key, value in plugin.items() if key not in {'module', 'class'}} + namespace_name = plugin_config.get('name') + namespace_config = self.config_data.get(namespace_name, {}) if namespace_name else {} + self.get_logger().info(f'Loading plugin: {class_name} from {module_name}') + try: + plugin_obj = plugin_cls( + self, + self.pb, + self.robot, + rev_joints=self.rev_joint_index_name_dic, + prism_joints=self.prismatic_joint_index_name_dic, + fixed_joints=self.fixed_joint_index_name_dic, + link_ids=self.link_names_to_ids_dic, + plugin_config=plugin_config, + parameters=namespace_config, + global_parameters=self.config_data, + ) + self.plugins.append(plugin_obj) + except Exception as exc: # pragma: no cover - plugin specific failures are logged + self.get_logger().error(f'Failed to initialise plugin {class_name}: {exc}') + + def start(self) -> None: + if not self.connected_to_physics_server: + self.get_logger().error('Cannot start simulation; not connected to physics server') + return + if self.parallel_plugin_execution: + self._executor_thread = Thread( + target=self._run_parallel_execution, + args=(self.loop_rate,), + daemon=True, + ) + self._executor_thread.start() + else: + self._timer = self.create_timer(1.0 / float(self.loop_rate), self._sequential_step) + + def stop(self) -> None: + self._stop_event.set() + if self._timer is not None: + self._timer.cancel() + self._timer = None + if self._executor_thread is not None: + self._executor_thread.join() + self._executor_thread = None + if self.connected_to_physics_server: + self.pb.disconnect() + self.connected_to_physics_server = False + for temp_file in self._temp_urdf_files: + try: + os.remove(temp_file) + except OSError: + pass + self._temp_urdf_files.clear() + + def _start_gui(self, gui: bool = True): + if gui: + self.get_logger().info('Running pybullet with GUI enabled') + options = self.gui_options if isinstance(self.gui_options, str) else '' + return self.pb.connect(self.pb.GUI, options=options) + self.get_logger().info('Running pybullet without GUI') + return self.pb.connect(self.pb.DIRECT) + + def _init_pybullet_robot(self): + urdf_path = self.robot_urdf_path + if not urdf_path: + raise RuntimeError('Mandatory parameter robot_urdf_path not set') + urdf_path = os.path.expanduser(os.path.expandvars(urdf_path)) + if not os.path.isfile(urdf_path): + raise FileNotFoundError(f'URDF file does not exist: {urdf_path}') + if urdf_path.endswith('.xacro'): + try: + import xacro # type: ignore + except ImportError as exc: + raise RuntimeError('xacro is required to process robot xacro files') from exc + doc = xacro.process_file(urdf_path) + urdf_content = doc.toxml() + from tempfile import NamedTemporaryFile + with NamedTemporaryFile('w', delete=False, suffix='.urdf') as temp_file: + temp_file.write(urdf_content) + urdf_path = temp_file.name + self._temp_urdf_files.append(temp_file.name) + robot_spawn_orientation = self.pb.getQuaternionFromEuler([0.0, 0.0, float(self.robot_pose_yaw)]) + fixed_base = bool(self.fixed_base) + if self.config_data.get('use_intertia_from_file', False): + urdf_flags = self.pb.URDF_USE_INERTIA_FROM_FILE | self.pb.URDF_USE_SELF_COLLISION + else: + urdf_flags = self.pb.URDF_USE_SELF_COLLISION + self.environment.load_environment() + self.pb.setRealTimeSimulation(0) + self.get_logger().info(f'Loading URDF model: {urdf_path}') + return self.pb.loadURDF( + urdf_path, + basePosition=[float(self.robot_pose_x), float(self.robot_pose_y), float(self.robot_pose_z)], + baseOrientation=robot_spawn_orientation, + useFixedBase=fixed_base, + flags=urdf_flags, + ) + + def _get_properties(self): + rev_joint_index_name_dic: Dict[int, str] = {} + fixed_joint_index_name_dic: Dict[int, str] = {} + prismatic_joint_index_name_dic: Dict[int, str] = {} + link_names_to_ids_dic: Dict[str, int] = {} + for joint_index in range(0, self.pb.getNumJoints(self.robot)): + info = self.pb.getJointInfo(self.robot, joint_index) + link_names_to_ids_dic[info[12].decode('utf-8')] = joint_index + if info[2] == self.pb.JOINT_REVOLUTE: + rev_joint_index_name_dic[joint_index] = info[1].decode('utf-8') + elif info[2] == self.pb.JOINT_FIXED: + fixed_joint_index_name_dic[joint_index] = info[1].decode('utf-8') + elif info[2] == self.pb.JOINT_PRISMATIC: + prismatic_joint_index_name_dic[joint_index] = info[1].decode('utf-8') + return rev_joint_index_name_dic, prismatic_joint_index_name_dic, fixed_joint_index_name_dic, link_names_to_ids_dic + + def handle_reset_simulation(self, request, response): + self.get_logger().info('Resetting simulation') + self.pause_simulation = True + self.pb.resetSimulation() + self._init_pybullet_robot() + self.pause_simulation = False + return response + + def handle_pause_physics(self, request, response): + self.get_logger().info('Pausing simulation') + self.pause_simulation = True + return response + + def handle_unpause_physics(self, request, response): + self.get_logger().info('Unpausing simulation') + self.pause_simulation = False + return response + + def _sequential_step(self) -> None: + if self.pause_simulation: + return + for plugin in self.plugins: + execute = getattr(plugin, 'execute', None) + if callable(execute): + execute() + self.pb.stepSimulation() + + def _run_parallel_execution(self, loop_rate: float) -> None: + exec_manager_obj = FuncExecManager( + self.plugins, + self._stop_event.is_set, + self.pb.stepSimulation, + lambda: self.pause_simulation, + log_info=self.get_logger().info, + log_warn=self.get_logger().warning, + log_debug=self.get_logger().debug, + function_name='plugin', + ) + exec_manager_obj.start_synchronous_execution(loop_rate=float(loop_rate)) + + def destroy_node(self) -> bool: + self.stop() + return super().destroy_node() + + +def main(args: List[str] | None = None) -> None: + rclpy.init(args=args) + node = PyBulletRosNode() + try: + node.start() + rclpy.spin(node) + except KeyboardInterrupt: + node.get_logger().info('Keyboard interrupt received, shutting down') + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/pybullet_ros/scripts/__init__.py b/pybullet_ros/scripts/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pybullet_ros/scripts/joint_states_to_float.py b/pybullet_ros/scripts/joint_states_to_float.py new file mode 100755 index 0000000..a168603 --- /dev/null +++ b/pybullet_ros/scripts/joint_states_to_float.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64 +from sensor_msgs.msg import JointState + + +class JointStatesToFloat(Node): + def __init__(self): + super().__init__('joint_states_to_float') + self.declare_parameter('input_topic', 'fake_joint_states') + self.declare_parameter('output_suffix', '_position_controller/command') + input_topic = self.get_parameter('input_topic').value + self.output_suffix = self.get_parameter('output_suffix').value + + self.joint_state_msg = None + self.previous_angles = [] + self.publishers = {} + self.msg_received = False + + self.subscription = self.create_subscription(JointState, input_topic, self.joint_states_cb, 10) + self.timer = self.create_timer(0.1, self.publish_joints_as_floats) + + def joint_states_cb(self, msg: JointState) -> None: + if not self.publishers: + for name in msg.name: + topic_name = f'{name}{self.output_suffix}' + self.publishers[name] = self.create_publisher(Float64, topic_name, 10) + self.previous_angles.append(0.0) + self.joint_state_msg = msg + self.msg_received = True + + def publish_joints_as_floats(self) -> None: + if not self.msg_received or self.joint_state_msg is None: + return + self.msg_received = False + angles = list(self.joint_state_msg.position) + for i, name in enumerate(self.joint_state_msg.name): + if i >= len(self.previous_angles) or angles[i] != self.previous_angles[i]: + publisher = self.publishers.get(name) + if publisher is not None: + msg = Float64() + msg.data = angles[i] + publisher.publish(msg) + self.previous_angles = angles + + +def main(args=None): + rclpy.init(args=args) + node = JointStatesToFloat() + try: + rclpy.spin(node) + except KeyboardInterrupt: + node.get_logger().info('Keyboard interrupt, shutting down') + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/pybullet_ros/sdf/__init__.py b/pybullet_ros/sdf/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros/src/pybullet_ros/sdf/conversions.py b/pybullet_ros/sdf/conversions.py similarity index 100% rename from ros/src/pybullet_ros/sdf/conversions.py rename to pybullet_ros/sdf/conversions.py diff --git a/ros/src/pybullet_ros/sdf/naming.py b/pybullet_ros/sdf/naming.py similarity index 100% rename from ros/src/pybullet_ros/sdf/naming.py rename to pybullet_ros/sdf/naming.py diff --git a/ros/src/pybullet_ros/sdf/sdf_parser.py b/pybullet_ros/sdf/sdf_parser.py similarity index 100% rename from ros/src/pybullet_ros/sdf/sdf_parser.py rename to pybullet_ros/sdf/sdf_parser.py diff --git a/resource/pybullet_ros b/resource/pybullet_ros new file mode 100644 index 0000000..d95628b --- /dev/null +++ b/resource/pybullet_ros @@ -0,0 +1 @@ +pybullet_ros diff --git a/ros/config/robots/acrobat_robot_config.yaml b/ros/config/robots/acrobat_robot_config.yaml deleted file mode 100644 index 63ebec7..0000000 --- a/ros/config/robots/acrobat_robot_config.yaml +++ /dev/null @@ -1,15 +0,0 @@ -# pybullet plugins, will be loaded during runtime -plugins: { pybullet_ros.plugins.control: Control, - pybullet_ros.plugins.joint_state_pub: joinStatePub, - pybullet_ros.plugins.rgbd_camera: RGBDCamera} - -loop_rate: 80.0 -gravity: -9.81 -max_effort: 5000.0 -use_intertia_from_file: False - -rgbd_camera: - frame_id: tip_link - resolution: - width: 640 - height: 480 diff --git a/ros/launch/bringup_robot_example.launch b/ros/launch/bringup_robot_example.launch deleted file mode 100644 index 5109b4b..0000000 --- a/ros/launch/bringup_robot_example.launch +++ /dev/null @@ -1,49 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ros/launch/pybullet_ros_example.launch b/ros/launch/pybullet_ros_example.launch deleted file mode 100644 index 14daa91..0000000 --- a/ros/launch/pybullet_ros_example.launch +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ros/launch/robots/acrobat_bringup.launch b/ros/launch/robots/acrobat_bringup.launch deleted file mode 100644 index 01b49f7..0000000 --- a/ros/launch/robots/acrobat_bringup.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ros/launch/tools/position_cmd_gui.launch b/ros/launch/tools/position_cmd_gui.launch deleted file mode 100644 index 7cd5912..0000000 --- a/ros/launch/tools/position_cmd_gui.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/ros/scripts/joint_states_to_float b/ros/scripts/joint_states_to_float deleted file mode 100755 index 26cfc59..0000000 --- a/ros/scripts/joint_states_to_float +++ /dev/null @@ -1,63 +0,0 @@ -#!/usr/bin/env python3 - -import rospy -from std_msgs.msg import Float64 -from sensor_msgs.msg import JointState - -class JointStatesToFloat: - def __init__(self): - self.rate = rospy.Rate(10) - self.msg_received = False - self.joint_state_msg = None - self.previous_angles = [] - rospy.Subscriber('fake_joint_states', JointState, self.JointStatesCB) - rospy.loginfo('waiting for first joint state msg to be published') - self.wait_for_first_msg() - rospy.loginfo('joint state msg received, proceeding') - # init previous angle list - for i in range(len(self.joint_state_msg.position)): - self.previous_angles.append(0.0) - # create one publisher per joint name - self.pub_list = [] - for name in self.joint_state_msg.name: - self.pub_list.append(rospy.Publisher(name + '_position_controller/command', Float64, queue_size=1)) - - def wait_for_first_msg(self): - while not self.msg_received: - # wait until at least one joint state msg is received - self.rate.sleep() - - def JointStatesCB(self, msg): - self.joint_state_msg = msg - self.msg_received = True - - def publishJointsAsFloats(self): - # extract desired angles from joint state msg - angles = [] - for angle in self.joint_state_msg.position: - angles.append(angle) - # compare previous angles to see if there was any change - changes = [] - for i, previous_angle in enumerate(self.previous_angles): - changes.append(previous_angle != angles[i]) - # publish desired angles to float topics - for i, pub in enumerate(self.pub_list): - if changes[i]: # compute only if angle changed - float_msg = Float64() - float_msg.data = angles[i] - pub.publish(float_msg) - # to keep track if the angle changed or not - self.previous_angles = angles - - def start(self): - while not rospy.is_shutdown(): - if self.msg_received: - # lower flag - self.msg_received = False - self.publishJointsAsFloats() - self.rate.sleep() - -if __name__ == '__main__': - rospy.init_node('joint_states_to_float', anonymous=False) - jstf = JointStatesToFloat() - jstf.start() diff --git a/ros/scripts/pybullet_ros_node b/ros/scripts/pybullet_ros_node deleted file mode 100755 index 8a0f775..0000000 --- a/ros/scripts/pybullet_ros_node +++ /dev/null @@ -1,6 +0,0 @@ -#!/usr/bin/env python3 - -import pybullet_ros.pybullet_ros - -if __name__ == '__main__': - pybullet_ros.pybullet_ros.main() diff --git a/ros/src/pybullet_ros/plugins/body_vel_control.py b/ros/src/pybullet_ros/plugins/body_vel_control.py deleted file mode 100644 index d5a069c..0000000 --- a/ros/src/pybullet_ros/plugins/body_vel_control.py +++ /dev/null @@ -1,140 +0,0 @@ -#!/usr/bin/env python3 - -""" -Subscribe to cmd_vel and apply desired speed to the robot, without any noise - -tf explained: -pybullet requires that velocity of the robot is set w.r.t. world reference frame -however cmd_vel convention required velocity to be expressed w.r.t. robot base frame -therefore a transformation is needed. -""" - -import rospy -import math -import numpy as np - -from geometry_msgs.msg import Twist, Vector3Stamped, Vector3 - -class cmdVelCtrl: - def __init__(self, pybullet, robot, **kargs): - # get "import pybullet as pb" and store in self.pb - self.pb = pybullet - # get robot from parent class - self.robot = robot - # subscribe to robot velocity commands - self.cmd_vel_msg = None - self.received_cmd_vel_time = None - rospy.Subscriber("cmd_vel", Twist, self.cmdVelCB) - - # ---------- tf stuff starts - - def translation_matrix(self, direction): - """copied from tf (listener.py)""" - M = np.identity(4) - M[:3, 3] = direction[:3] - return M - - def quaternion_matrix(self, quaternion): - """copied from tf (listener.py)""" - epsilon = np.finfo(float).eps * 4.0 - q = np.array(quaternion[:4], dtype=np.float64, copy=True) - nq = np.dot(q, q) - if nq < epsilon: - return np.identity(4) - q *= math.sqrt(2.0 / nq) - q = np.outer(q, q) - return np.array(( - (1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3], 0.0), - ( q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3], 0.0), - ( q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], 0.0), - ( 0.0, 0.0, 0.0, 1.0) - ), dtype=np.float64) - - def fromTranslationRotation(self, translation, rotation): - """copied from tf (listener.py)""" - return np.dot(self.translation_matrix(translation), self.quaternion_matrix(rotation)) - - def lookupTransform(self, target_frame='base_link', source_frame='odom'): - """ - copied from tf (listener.py) - source_frame = odom, world_frame, target_frame = base_link, robot_frame - """ - # get robot pose from pybullet - t, r = self.pb.getBasePositionAndOrientation(self.robot) - return [t[0], t[1], t[2]], [r[0], r[1], r[2], r[3]] - - def asMatrix(self, target_frame, hdr): - """copied from tf (listener.py)""" - translation,rotation = self.lookupTransform(target_frame, hdr.frame_id) - return self.fromTranslationRotation(translation, rotation) - - def transformVector3(self, target_frame, v3s): - """copied from tf (listener.py)""" - mat44 = self.asMatrix(target_frame, v3s.header) - mat44[0,3] = 0.0 - mat44[1,3] = 0.0 - mat44[2,3] = 0.0 - xyz = tuple(np.dot(mat44, np.array([v3s.vector.x, v3s.vector.y, v3s.vector.z, 1.0])))[:3] - r = Vector3Stamped() - r.header.stamp = v3s.header.stamp - r.header.frame_id = target_frame - r.vector = Vector3(*xyz) - return r - - # ---------- tf stuff ends - - def cmd_vel_force_ctrler(self): - """ - experimental code to set a torque on the robot model using a PID controller - """ - pass - #self.controller[0].setpoint = self.cmd_vel_msg.linear.x - #self.controller[1].setpoint = self.cmd_vel_msg.linear.y - #self.controller[2].setpoint = self.cmd_vel_msg.linear.z - #self.controller[3].setpoint = self.cmd_vel_msg.angular.x - #self.controller[4].setpoint = self.cmd_vel_msg.angular.y - #self.controller[5].setpoint = self.cmd_vel_msg.angular.z - ## query current robot speed from pybullet - #linear_vel, angular_vel = self.pb.getBaseVelocity(self.robot) - ## compute torque value based on PID library - #output = [] - #for i in range(0, 3): - #output.append(self.controller[i](linear_vel[i])) - #for j in range(0, 3): - #output.append(self.controller[j](angular_vel[j])) - ## apply external force to robot body - #self.z_offset = rospy.get_param('~cmd_vel_ctrl/z_offset', -0.35) - #self.pb.applyExternalForce(self.robot, linkIndex=-1, forceObj=[output[0], output[1], output[2]], - #posObj=[0.0, 0.0, self.z_offset], flags=self.pb.LINK_FRAME) - #self.pb.applyExternalTorque(self.robot, linkIndex=-1, torqueObj=[output[3], output[4], output[5]], - #flags=self.pb.LINK_FRAME) - - def cmdVelCB(self, msg): - """callback to receive vel commands from user""" - self.cmd_vel_msg = msg - self.received_cmd_vel_time = rospy.Time.now() - - def execute(self): - """this function gets called from pybullet ros main update loop""" - if not self.cmd_vel_msg: - return - # check if timestamp is recent - if (rospy.Time.now() - rospy.Duration(0.5)) > self.received_cmd_vel_time: - return - # transform Twist from base_link to odom (pybullet allows to set vel only on world ref frame) - # NOTE: we would normally use tf for this, but there are issues currently between python 2 and 3 in ROS 1 - lin_vec = Vector3Stamped() - lin_vec.header.frame_id = 'base_link' - lin_vec.vector.x = self.cmd_vel_msg.linear.x - lin_vec.vector.y = self.cmd_vel_msg.linear.y - lin_vec.vector.z = self.cmd_vel_msg.linear.z - ang_vec = Vector3Stamped() - ang_vec.header.frame_id = 'base_link' - ang_vec.vector.x = self.cmd_vel_msg.angular.x - ang_vec.vector.y = self.cmd_vel_msg.angular.y - ang_vec.vector.z = self.cmd_vel_msg.angular.z - lin_cmd_vel_in_odom = self.transformVector3('odom', lin_vec) - ang_cmd_vel_in_odom = self.transformVector3('odom', ang_vec) - # set vel directly on robot model - self.pb.resetBaseVelocity(self.robot, [lin_cmd_vel_in_odom.vector.x, lin_cmd_vel_in_odom.vector.y, lin_cmd_vel_in_odom.vector.z], - [ang_cmd_vel_in_odom.vector.x, ang_cmd_vel_in_odom.vector.y, ang_cmd_vel_in_odom.vector.z]) diff --git a/ros/src/pybullet_ros/plugins/control.py b/ros/src/pybullet_ros/plugins/control.py deleted file mode 100644 index c77137e..0000000 --- a/ros/src/pybullet_ros/plugins/control.py +++ /dev/null @@ -1,127 +0,0 @@ -#!/usr/bin/env python3 - -""" -position, velocity and effort control for all revolute joints on the robot -""" - -import rospy -from std_msgs.msg import Float64 - -# NOTE: 2 classes are implemented here, scroll down to the next class (Control) to see the plugin! - -class pveControl: - """helper class to receive position, velocity or effort (pve) control commands""" - def __init__(self, joint_index, joint_name, controller_type): - """constructor - Assumes joint_name is unique, creates multiple subscribers to receive commands - joint_index - stores an integer joint identifier - joint_name - string with the name of the joint as described in urdf model - controller_type - position, velocity or effort - """ - assert(controller_type in ['position', 'velocity', 'effort']) - rospy.Subscriber(joint_name + '_' + controller_type + '_controller/command', - Float64, self.pve_controlCB, queue_size=1) - self.cmd = 0.0 - self.data_available = False - self.joint_index = joint_index - self.joint_name = joint_name - - def pve_controlCB(self, msg): - """position, velocity or effort callback - msg - the msg passed by the ROS network via topic publication - """ - self.data_available = True - self.cmd = msg.data - - def get_last_cmd(self): - """method to fetch the last received command""" - self.data_available = False - return self.cmd - - def get_is_data_available(self): - """method to retrieve flag to indicate that a command has been received""" - return self.data_available - - def get_joint_name(self): - """Unused method provided for completeness (pybullet works based upon joint index, not names)""" - return self.joint_name - - def get_joint_index(self): - """method used to retrieve the joint int index that this class points to""" - return self.joint_index - -# plugin is implemented below -class Control: - def __init__(self, pybullet, robot, **kargs): - # get "import pybullet as pb" and store in self.pb - self.pb = pybullet - # get robot from parent class - self.robot = robot - # lists to recall last received command (useful when controlling multiple joints) - self.position_joint_commands = [] - self.velocity_joint_commands = [] - self.effort_joint_commands = [] - # this parameter will be set for all robot joints - if rospy.has_param('~max_effort_vel_mode'): - rospy.logwarn('max_effort_vel_mode parameter is deprecated, please use max_effort instead') - # kept for backwards compatibility, delete after some time - max_effort = rospy.get_param('~max_effort_vel_mode', 100.0) - else: - max_effort = rospy.get_param('~max_effort', 100.0) - # the max force to apply to the joint, used in velocity control - self.force_commands = [] - # get joints names and store them in dictionary, combine both revolute and prismatic dic - self.joint_index_name_dic = {**kargs['rev_joints'], **kargs['prism_joints']} - # setup subscribers - self.pc_subscribers = [] - self.vc_subscribers = [] - self.ec_subscribers = [] - self.joint_indices = [] - # revolute joints - joint position, velocity and effort control command individual subscribers - for joint_index in self.joint_index_name_dic: - self.position_joint_commands.append(0.0) - self.velocity_joint_commands.append(0.0) - self.effort_joint_commands.append(0.0) - # used only in velocity and position control mode - self.force_commands.append(max_effort) - # get joint name from dictionary, is used for naming the subscriber - joint_name = self.joint_index_name_dic[joint_index] - # create list of joints for later use in pve_ctrl_cmd(...) - self.joint_indices.append(joint_index) - # create position control object - self.pc_subscribers.append(pveControl(joint_index, joint_name, 'position')) - # create position control object - self.vc_subscribers.append(pveControl(joint_index, joint_name, 'velocity')) - # create position control object - self.ec_subscribers.append(pveControl(joint_index, joint_name, 'effort')) - - def execute(self): - """this function gets called from pybullet ros main update loop""" - """check if user has commanded a joint and forward the request to pybullet""" - # flag to indicate there are pending position control tasks - position_ctrl_task = False - velocity_ctrl_task = False - effort_ctrl_task = False - for index, subscriber in enumerate(self.pc_subscribers): - if subscriber.get_is_data_available(): - self.position_joint_commands[index] = subscriber.get_last_cmd() - position_ctrl_task = True - for index, subscriber in enumerate(self.vc_subscribers): - if subscriber.get_is_data_available(): - self.velocity_joint_commands[index] = subscriber.get_last_cmd() - velocity_ctrl_task = True - for index, subscriber in enumerate(self.ec_subscribers): - if subscriber.get_is_data_available(): - self.effort_joint_commands[index] = subscriber.get_last_cmd() - effort_ctrl_task = True - # forward commands to pybullet, give priority to position control cmds, then vel, at last effort - if position_ctrl_task: - self.pb.setJointMotorControlArray(bodyUniqueId=self.robot, jointIndices=self.joint_indices, - controlMode=self.pb.POSITION_CONTROL, targetPositions=self.position_joint_commands, forces=self.force_commands) - elif velocity_ctrl_task: - self.pb.setJointMotorControlArray(bodyUniqueId=self.robot, jointIndices=self.joint_indices, - controlMode=self.pb.VELOCITY_CONTROL, targetVelocities=self.velocity_joint_commands, forces=self.force_commands) - elif effort_ctrl_task: - self.pb.setJointMotorControlArray(bodyUniqueId=self.robot, jointIndices=self.joint_indices, - controlMode=self.pb.TORQUE_CONTROL, forces=self.effort_joint_commands) - diff --git a/ros/src/pybullet_ros/plugins/diff_drive.py b/ros/src/pybullet_ros/plugins/diff_drive.py deleted file mode 100644 index 90198bb..0000000 --- a/ros/src/pybullet_ros/plugins/diff_drive.py +++ /dev/null @@ -1,83 +0,0 @@ -import rospy -from geometry_msgs.msg import Twist - - -class DiffDrive: - def __init__(self, pybullet, robot, **kwargs): - self.pb = pybullet - self.robot = robot - - try: - name = kwargs["name"] - parameters = rospy.get_param("~" + name) - except Exception: - rospy.logerr("Not loading plugin {}".format(self.__class__.__name__)) - import traceback - traceback.print_exc() - self.no_execution = True - return - - # joint information preparation - self.revolute_joints_id_name = kwargs["rev_joints"] - self.revolute_joints_name_id = {v: k for k, v in self.revolute_joints_id_name.items()} - - # a flag to monitor if anything went wrong - self.no_execution = False - - cmd_vel_topic = parameters.get("cmd_vel", "~cmd_vel") - self.cmd_vel_subscriber = rospy.Subscriber(cmd_vel_topic, Twist, self.cmd_vel_callback) - - if (wheel_separation := parameters.get("wheel_separation", None)) is None: - rospy.logwarn("No wheel_separation provided, using 1.0 as default") - wheel_separation = 1.0 - self.wheel_separation = wheel_separation - if (wheel_radius := parameters.get("wheel_radius", None)) is None: - rospy.logwarn("No wheel_radius provided, using 1.0 as default") - wheel_radius = 1.0 - self.wheel_radius = wheel_radius - - if (left_joints := parameters.get("left_joints", None)) is None: - rospy.logerr("No left_joints provided") - self.no_execution = True - return - if isinstance(left_joints, list) is False and type(left_joints) == str: - left_joints = [left_joints] - if (right_joints := parameters.get("right_joints", None)) is None: - rospy.logerr("No right_joints provided") - self.no_execution = True - return - if isinstance(right_joints, list) is False and type(right_joints) == str: - right_joints = [right_joints] - - self.joint_indices = {"left": [], "right": []} - for side in ["left", "right"]: - for joint_name in eval(side + "_joints"): - if joint_name not in self.revolute_joints_name_id: - rospy.logerr("Could not find joint {} in urdf".format(joint_name)) - self.no_execution = True - return - self.joint_indices[side].append(self.revolute_joints_name_id[joint_name]) - - self.cmd_vel: Twist = Twist() - - def cmd_vel_callback(self, msg: Twist): - self.cmd_vel = msg - - def get_wheel_speeds(self): - vx = self.cmd_vel.linear.x - wz = self.cmd_vel.angular.z - left = (vx - wz * self.wheel_separation / 2.0) / self.wheel_radius - right = (vx + wz * self.wheel_separation / 2.0) / self.wheel_radius - return {"left": left, "right": right} - - def execute(self): - if not self.no_execution: - speeds = self.get_wheel_speeds() - for side in ["left", "right"]: - indices = self.joint_indices[side] - self.pb.setJointMotorControlArray( - bodyUniqueId=self.robot, - jointIndices=indices, - controlMode=self.pb.VELOCITY_CONTROL, - targetVelocities=[speeds[side] for _ in range(len(indices))] - ) diff --git a/ros/src/pybullet_ros/plugins/environment.py b/ros/src/pybullet_ros/plugins/environment.py deleted file mode 100644 index eae569a..0000000 --- a/ros/src/pybullet_ros/plugins/environment.py +++ /dev/null @@ -1,74 +0,0 @@ -#!/usr/bin/env python3 - -""" -plugin that is loaded one time only at the beginning -It is meant to be for you to upload your environment -""" - -import rospy -import pybullet_ros.sdf.sdf_parser as sdf_parser - -class Environment: - def __init__(self, pybullet, **kargs): - # get "import pybullet as pb" and store in self.pb - self.pb = pybullet - # enable soft body simulation if needed - if rospy.get_param('~use_deformable_world', False): - rospy.loginfo('Using deformable world (soft body simulation)') - self.pb.resetSimulation(self.pb.RESET_USE_DEFORMABLE_WORLD) - - def load_environment(self): - """ - set gravity, ground plane and load URDF or SDF models as required - """ - # set gravity - gravity = rospy.get_param('~gravity', -9.81) # get gravity from param server - self.pb.setGravity(0, 0, gravity) - # set floor - self.pb.loadURDF('plane.urdf') - # TODO: load world sdf files, (compatible gazebo .world) - ## make sure GAZEBO_MODEL_PATH is set - #if os.environ.get('GAZEBO_MODEL_PATH', None) == None: - ## suggested value: /usr/share/gazebo-9/models - #rospy.logwarn('GAZEBO_MODEL_PATH environment variable not set, models will not be able to load...') - ## continue blue console output - #print('\033[34m') - #world_path = rospy.get_param('~environment', None) - #if not world_path: - #rospy.logwarn('environment param not set, will run pybullet with empty world') - #return - #if not os.path.isfile(world_path): - #rospy.logwarn('environment file not found, world will not be loaded : ' + world_path) - ## continue blue console output - #print('\033[34m') - #else: - #rospy.loginfo('loading world : ' + world_path) - ## get all world models - #world = sdf_parser.SDF(file=world_path).world - #print('succesfully imported %d models'% len(world.models)) - ## spawn sdf models in pybullet one at a time - #for model in world.models: - ## currently not sure how to set the required pose with loadSDF function... - #obj_pose = [float(x) for x in model.simple_pose.split() if not x.isalpha()] - #rospy.loginfo('loading model : ' + model.filename) - #try: - #self.pb.loadSDF(model.filename) - #except: - #rospy.logwarn('failed to load model : ' + model.filename + ', skipping...') - # give control to the user to upload custom world via code - self.load_environment_via_code() - - def load_environment_via_code(self): - """ - This method provides the possibility for the user to define an environment via python code - example: - self.pb.loadURDF(...) - self.pb.loadSDF(...) - self.pb.loadSoftBody(...) - self.pb.setTimeStep(0.001) - self.pb.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25) # ? related to soft bodies - etc... - NOTE: is not advised to directly write code below, instead make new class and inherit from this one - see example: environment_template.py - """ - pass diff --git a/ros/src/pybullet_ros/plugins/environment_template.py b/ros/src/pybullet_ros/plugins/environment_template.py deleted file mode 100644 index 1bcd712..0000000 --- a/ros/src/pybullet_ros/plugins/environment_template.py +++ /dev/null @@ -1,21 +0,0 @@ -#!/usr/bin/env python3 - -from pybullet_ros.plugins.environment import Environment as DefaultEnv - -class Environment(DefaultEnv): - def __init__(self, pybullet, **kargs): - super().__init__(pybullet) - - # override parent method - def load_environment_via_code(self): - """ - This method provides the possibility for the user to define an environment via python code - example: - self.pb.loadURDF(...) - self.pb.loadSDF(...) - self.pb.loadSoftBody(...) - self.pb.setTimeStep(0.001) - self.pb.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25) # ? related to soft bodies - etc... - """ - rospy.logwarn('loading custom environment via code!') diff --git a/ros/src/pybullet_ros/plugins/joint_state_pub.py b/ros/src/pybullet_ros/plugins/joint_state_pub.py deleted file mode 100644 index ceaa772..0000000 --- a/ros/src/pybullet_ros/plugins/joint_state_pub.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python3 - -""" -query robot state and publish position, velocity and effort values to /joint_states -""" - -import rospy -from sensor_msgs.msg import JointState - -class joinStatePub: - def __init__(self, pybullet, robot, **kargs): - # get "import pybullet as pb" and store in self.pb - self.pb = pybullet - # get robot from parent class - self.robot = robot - # get joints names and store them in dictionary - self.joint_index_name_dic = kargs['rev_joints'] - # register this node in the network as a publisher in /joint_states topic - self.pub_joint_states = rospy.Publisher('joint_states', JointState, queue_size=1) - - def execute(self): - """this function gets called from pybullet ros main update loop""" - # setup msg placeholder - joint_msg = JointState() - # get joint states - for joint_index in self.joint_index_name_dic: - # get joint state from pybullet - joint_state = self.pb.getJointState(self.robot, joint_index) - # fill msg - joint_msg.name.append(self.joint_index_name_dic[joint_index]) - joint_msg.position.append(joint_state[0]) - joint_msg.velocity.append(joint_state[1]) - joint_msg.effort.append(joint_state[3]) # applied effort in last sim step - # update msg time using ROS time api - joint_msg.header.stamp = rospy.Time.now() - # publish joint states to ROS - self.pub_joint_states.publish(joint_msg) diff --git a/ros/src/pybullet_ros/plugins/laser_scanner.py b/ros/src/pybullet_ros/plugins/laser_scanner.py deleted file mode 100644 index 6c10be9..0000000 --- a/ros/src/pybullet_ros/plugins/laser_scanner.py +++ /dev/null @@ -1,119 +0,0 @@ -#!/usr/bin/env python3 - -""" -Laser scanner simulation based on pybullet rayTestBatch function -This code does not add noise to the laser scanner readings -""" - -import rospy -import math -import numpy as np -from sensor_msgs.msg import LaserScan - -class laserScanner: - def __init__(self, pybullet, robot, **kargs): - # get "import pybullet as pb" and store in self.pb - self.pb = pybullet - # get robot from parent class - self.robot = robot - # laser params - laser_frame_id = rospy.get_param('~laser/frame_id', None) # laser reference frame, has to be an existing link - if not laser_frame_id: - rospy.logerr('required parameter laser_frame_id not set, will exit now') - rospy.signal_shutdown('required param laser_frame_id not set') - return - # get pybullet laser link id from its name - link_names_to_ids_dic = kargs['link_ids'] - if not laser_frame_id in link_names_to_ids_dic: - rospy.logerr('laser reference frame "{}" not found in URDF model, cannot continue'.format(laser_frame_id)) - rospy.logwarn('Available frames are: {}'.format(link_names_to_ids_dic)) - rospy.signal_shutdown('required param frame id not set properly') - return - self.pb_laser_link_id = link_names_to_ids_dic[laser_frame_id] - # create laser msg placeholder for publication - self.laser_msg = LaserScan() - # laser field of view - angle_min = rospy.get_param('~laser/angle_min', -1.5707963) - angle_max = rospy.get_param('~laser/angle_max', 1.5707963) - assert(angle_max > angle_min) - self.numRays = rospy.get_param('~laser/num_beams', 50) # should be 512 beams but simulation becomes slow - self.laser_msg.range_min = rospy.get_param('~laser/range_min', 0.03) - self.laser_msg.range_max = rospy.get_param('~laser/range_max', 5.6) - self.beam_visualisation = rospy.get_param('~laser/beam_visualisation', False) - self.laser_msg.angle_min = angle_min - self.laser_msg.angle_max = angle_max - self.laser_msg.angle_increment = (angle_max - angle_min) / self.numRays - # register this node in the network as a publisher in /scan topic - self.pub_laser_scanner = rospy.Publisher('scan', LaserScan, queue_size=1) - self.laser_msg.header.frame_id = laser_frame_id - self.laser_msg.time_increment = 0.01 # ? - self.laser_msg.scan_time = 0.1 # 10 hz - # fixed_joint_index_name_dic = kargs['fixed_joints'] - self.rayHitColor = [1, 0, 0] # red color - self.rayMissColor = [0, 1, 0] # green color - # compute rays end beam position - self.rayFrom, self.rayTo = self.prepare_rays() - # variable used to run this plugin at a lower frequency, HACK - self.count = 0 - - def prepare_rays(self): - """assume laser is in the origin and compute its x, y beam end position""" - # prepare raycast origin and end values - rayFrom = [] - rayTo = [] - for n in range(0, self.numRays): - alpha = self.laser_msg.angle_min + n * self.laser_msg.angle_increment - rayFrom.append([self.laser_msg.range_min * math.cos(alpha), - self.laser_msg.range_min * math.sin(alpha), 0.0]) - rayTo.append([self.laser_msg.range_max * math.cos(alpha), - self.laser_msg.range_max * math.sin(alpha), 0.0]) - return rayFrom, rayTo - - def transform_rays(self, laser_position, laser_orientation): - """transform rays from reference frame using pybullet functions (not tf)""" - laser_position = [laser_position[0], laser_position[1], laser_position[2]] - TFrayFrom = [] - TFrayTo = [] - rm = self.pb.getMatrixFromQuaternion(laser_orientation) - rotation_matrix = [[rm[0], rm[1], rm[2]],[rm[3], rm[4], rm[5]],[rm[6], rm[7], rm[8]]] - for ray in self.rayFrom: - position = np.dot(rotation_matrix, [ray[0], ray[1], ray[2]]) + laser_position - TFrayFrom.append([position[0], position[1], position[2]]) - for ray in self.rayTo: - position = np.dot(rotation_matrix, [ray[0], ray[1], ray[2]]) + laser_position - TFrayTo.append([position[0], position[1], position[2]]) - return TFrayFrom, TFrayTo - - def execute(self): - """this function gets called from pybullet ros main update loop""" - # run at lower frequency, laser computations are expensive - self.count += 1 - if self.count < 100: - return - self.count = 0 # reset count - # remove any previous laser data if any - self.laser_msg.ranges = [] - # remove previous beam lines from screen - if self.beam_visualisation: - self.pb.removeAllUserDebugItems() - # get laser link position - laser_state = self.pb.getLinkState(self.robot, self.pb_laser_link_id) - # transform start and end position of the rays which were generated considering laser at the origin - rayFrom, rayTo = self.transform_rays(laser_state[0], laser_state[1]) # position + orientation - # raycast using 4 threads - results = self.pb.rayTestBatch(rayFrom, rayTo, 4) - for i in range(self.numRays): - if self.beam_visualisation: - hitObjectUid = results[i][0] - if hitObjectUid < 0: - # draw a line on pybullet gui for debug purposes in green because it did not hit any obstacle - self.pb.addUserDebugLine(rayFrom[i], rayTo[i], self.rayMissColor) - else: - # draw a line on pybullet gui for debug purposes in red because it hited obstacle, results[i][3] -> hitPosition - self.pb.addUserDebugLine(rayFrom[i], results[i][3], self.rayHitColor) - # compute laser ranges from hitFraction -> results[i][2] - self.laser_msg.ranges.append(results[i][2] * self.laser_msg.range_max) - # update laser time stamp with current time - self.laser_msg.header.stamp = rospy.Time.now() - # publish scan - self.pub_laser_scanner.publish(self.laser_msg) diff --git a/ros/src/pybullet_ros/plugins/odometry.py b/ros/src/pybullet_ros/plugins/odometry.py deleted file mode 100644 index 62ac419..0000000 --- a/ros/src/pybullet_ros/plugins/odometry.py +++ /dev/null @@ -1,48 +0,0 @@ -#!/usr/bin/env python3 - -""" -Query robot base pose and speed from pybullet and publish to /odom topic -This component does not add any noise to it -""" - -import rospy, tf -from nav_msgs.msg import Odometry - -class simpleOdometry: - def __init__(self, pybullet, robot, **kargs): - # get "import pybullet as pb" and store in self.pb - self.pb = pybullet - # get robot from parent class - self.robot = robot - # register this node as a /odom publisher - self.pub_odometry = rospy.Publisher('odom', Odometry, queue_size=1) - # save some overhead by setting some information only once - self.odom_msg = Odometry() - self.odom_msg.header.frame_id = rospy.get_param('~odom_frame', 'odom') - self.odom_msg.child_frame_id = rospy.get_param('~robot_base_frame', 'base_link') - self.br = tf.TransformBroadcaster() - - def execute(self): - """this function gets called from pybullet ros main update loop""" - # set msg timestamp based on current time - self.odom_msg.header.stamp = rospy.Time.now() - # query base pose from pybullet and store in odom msg - position, orientation = self.pb.getBasePositionAndOrientation(self.robot) - [self.odom_msg.pose.pose.position.x,\ - self.odom_msg.pose.pose.position.y,\ - self.odom_msg.pose.pose.position.z] = position - [self.odom_msg.pose.pose.orientation.x,\ - self.odom_msg.pose.pose.orientation.y,\ - self.odom_msg.pose.pose.orientation.z,\ - self.odom_msg.pose.pose.orientation.w] = orientation - # query base velocity from pybullet and store it in msg - [self.odom_msg.twist.twist.linear.x,\ - self.odom_msg.twist.twist.linear.y,\ - self.odom_msg.twist.twist.linear.z],\ - [self.odom_msg.twist.twist.angular.x,\ - self.odom_msg.twist.twist.angular.y,\ - self.odom_msg.twist.twist.angular.z] = self.pb.getBaseVelocity(self.robot) - self.pub_odometry.publish(self.odom_msg) - # tf broadcast (odom to base_link) - self.br.sendTransform(position, orientation, rospy.Time.now(), \ - self.odom_msg.child_frame_id, self.odom_msg.header.frame_id) diff --git a/ros/src/pybullet_ros/plugins/plugin_template.py b/ros/src/pybullet_ros/plugins/plugin_template.py deleted file mode 100644 index 01a8fe8..0000000 --- a/ros/src/pybullet_ros/plugins/plugin_template.py +++ /dev/null @@ -1,19 +0,0 @@ -#!/usr/bin/env python3 - -""" -TODO: briefly describe your plugin here -""" - -import rospy - -class pluginTemplate: - def __init__(self, pybullet, robot, **kargs): - # get "import pybullet as pb" and store in self.pb - self.pb = pybullet - # get robot from parent class - self.robot = robot - # TODO: implement here... - - def execute(self): - """this function gets called from pybullet ros main update loop""" - rospy.loginfo('my plugin is running!') diff --git a/ros/src/pybullet_ros/plugins/rgbd_camera.py b/ros/src/pybullet_ros/plugins/rgbd_camera.py deleted file mode 100644 index b607fae..0000000 --- a/ros/src/pybullet_ros/plugins/rgbd_camera.py +++ /dev/null @@ -1,124 +0,0 @@ -#!/usr/bin/env python3 - -""" -RGBD camera sensor simulation for pybullet_ros base on pybullet.getCameraImage() -""" - -import math -import numpy as np - -import rospy -from cv_bridge import CvBridge -from sensor_msgs.msg import Image - -class RGBDCamera: - def __init__(self, pybullet, robot, **kargs): - # get "import pybullet as pb" and store in self.pb - self.pb = pybullet - # get robot from parent class - self.robot = robot - # create image msg placeholder for publication - self.image_msg = Image() - # get RGBD camera parameters from ROS param server - self.image_msg.width = rospy.get_param('~rgbd_camera/resolution/width', 640) - self.image_msg.height = rospy.get_param('~rgbd_camera/resolution/height', 480) - assert(self.image_msg.width > 5) - assert(self.image_msg.height > 5) - cam_frame_id = rospy.get_param('~rgbd_camera/frame_id', None) - if not cam_frame_id: - rospy.logerr('Required parameter rgbd_camera/frame_id not set, will exit now...') - rospy.signal_shutdown('Required parameter rgbd_camera/frame_id not set') - return - # get pybullet camera link id from its name - link_names_to_ids_dic = kargs['link_ids'] - if not cam_frame_id in link_names_to_ids_dic: - rospy.logerr('Camera reference frame "{}" not found in URDF model'.format(cam_frame_id)) - rospy.logwarn('Available frames are: {}'.format(link_names_to_ids_dic)) - rospy.signal_shutdown('required param rgbd_camera/frame_id not set properly') - return - self.pb_camera_link_id = link_names_to_ids_dic[cam_frame_id] - self.image_msg.header.frame_id = cam_frame_id - # create publisher - self.pub_image = rospy.Publisher('rgb_image', Image, queue_size=1) - self.image_msg.encoding = rospy.get_param('~rgbd_camera/resolution/encoding', 'rgb8') - self.image_msg.is_bigendian = rospy.get_param('~rgbd_camera/resolution/encoding', 0) - self.image_msg.step = rospy.get_param('~rgbd_camera/resolution/encoding', 1920) - # projection matrix - self.hfov = rospy.get_param('~rgbd_camera/hfov', 56.3) - self.vfov = rospy.get_param('~rgbd_camera/vfov', 43.7) - self.near_plane = rospy.get_param('~rgbd_camera/near_plane', 0.4) - self.far_plane = rospy.get_param('~rgbd_camera/far_plane', 8) - self.projection_matrix = self.compute_projection_matrix() - # use cv_bridge ros to convert cv matrix to ros format - self.image_bridge = CvBridge() - # variable used to run this plugin at a lower frequency, HACK - self.count = 0 - - def compute_projection_matrix(self): - return self.pb.computeProjectionMatrix( - left=-math.tan(math.pi * self.hfov / 360.0) * self.near_plane, - right=math.tan(math.pi * self.hfov / 360.0) * self.near_plane, - bottom=-math.tan(math.pi * self.vfov / 360.0) * self.near_plane, - top=math.tan(math.pi * self.vfov / 360.0) * self.near_plane, - nearVal=self.near_plane, - farVal=self.far_plane) - - def extract_frame(self, camera_image): - bgr_image = np.zeros((self.image_msg.height, self.image_msg.width, 3)) - - camera_image = np.reshape(camera_image[2], (camera_image[1], camera_image[0], 4)) - - bgr_image[:, :, 2] =\ - (1 - camera_image[:, :, 3]) * camera_image[:, :, 2] +\ - camera_image[:, :, 3] * camera_image[:, :, 2] - - bgr_image[:, :, 1] =\ - (1 - camera_image[:, :, 3]) * camera_image[:, :, 1] +\ - camera_image[:, :, 3] * camera_image[:, :, 1] - - bgr_image[:, :, 0] =\ - (1 - camera_image[:, :, 3]) * camera_image[:, :, 0] +\ - camera_image[:, :, 3] * camera_image[:, :, 0] - - # return frame - return bgr_image.astype(np.uint8) - - def compute_camera_target(self, camera_position, camera_orientation): - """ - camera target is a point 5m in front of the robot camera - This method is used to tranform it to the world reference frame - NOTE: this method uses pybullet functions and not tf - """ - target_point = [5.0, 0, 0] # expressed w.r.t camera reference frame - camera_position = [camera_position[0], camera_position[1], camera_position[2]] - rm = self.pb.getMatrixFromQuaternion(camera_orientation) - rotation_matrix = [[rm[0], rm[1], rm[2]],[rm[3], rm[4], rm[5]],[rm[6], rm[7], rm[8]]] - return np.dot(rotation_matrix, target_point) + camera_position - - def execute(self): - """this function gets called from pybullet ros main update loop""" - # run at lower frequency, camera computations are expensive - self.count += 1 - if self.count < 100: - return - self.count = 0 # reset count - # get camera pose - cam_state = self.pb.getLinkState(self.robot, self.pb_camera_link_id) - # target is a point 5m ahead of the robot camera expressed w.r.t world reference frame - target = self.compute_camera_target(cam_state[0], cam_state[1]) - view_matrix = self.pb.computeViewMatrix(cam_state[0], target, [0, 0, 1]) - # get camera image from pybullet - pybullet_cam_resp = self.pb.getCameraImage(self.image_msg.width, - self.image_msg.height, - view_matrix, - self.projection_matrix, - renderer=self.pb.ER_BULLET_HARDWARE_OPENGL, - flags=self.pb.ER_NO_SEGMENTATION_MASK) - # frame extraction function from qibullet - frame = self.extract_frame(pybullet_cam_resp) - # fill pixel data array - self.image_msg.data = self.image_bridge.cv2_to_imgmsg(frame).data - # update msg time stamp - self.image_msg.header.stamp = rospy.Time.now() - # publish camera image to ROS network - self.pub_image.publish(self.image_msg) diff --git a/ros/src/pybullet_ros/pybullet_ros.py b/ros/src/pybullet_ros/pybullet_ros.py deleted file mode 100644 index 8a6c2f3..0000000 --- a/ros/src/pybullet_ros/pybullet_ros.py +++ /dev/null @@ -1,241 +0,0 @@ -#!/usr/bin/env python3 - -import os -import importlib -import rospy -import pybullet_data - -from std_srvs.srv import Empty -from pybullet_ros.function_exec_manager import FuncExecManager - -class pyBulletRosWrapper(object): - """ROS wrapper class for pybullet simulator""" - def __init__(self): - # import pybullet - self.pb = importlib.import_module('pybullet') - # get from param server the frequency at which to run the simulation - self.loop_rate = rospy.get_param('~loop_rate', 80.0) - # query from param server if gui is needed - is_gui_needed = rospy.get_param('~pybullet_gui', True) - # get from param server if user wants to pause simulation at startup - self.pause_simulation = rospy.get_param('~pause_simulation', False) - print('\033[34m') - # print pybullet stuff in blue - physicsClient = self.start_gui(gui=is_gui_needed) # we dont need to store the physics client for now... - # setup service to restart simulation - rospy.Service('~reset_simulation', Empty, self.handle_reset_simulation) - # setup services for pausing/unpausing simulation - rospy.Service('~pause_physics', Empty, self.handle_pause_physics) - rospy.Service('~unpause_physics', Empty, self.handle_unpause_physics) - # get pybullet path in your system and store it internally for future use, e.g. to set floor - self.pb.setAdditionalSearchPath(pybullet_data.getDataPath()) - # create object of environment class for later use - env_plugin = rospy.get_param('~environment', 'environment') # default : plugins/environment.py - plugin_import_prefix = rospy.get_param('~plugin_import_prefix', 'pybullet_ros.plugins') - self.environment = getattr(importlib.import_module(f'{plugin_import_prefix}.{env_plugin}'), 'Environment')(self.pb) - # load robot URDF model, set gravity, and ground plane - self.robot = self.init_pybullet_robot() - self.connected_to_physics_server = None - if not self.robot: - self.connected_to_physics_server = False - return # Error while loading urdf file - else: - self.connected_to_physics_server = True - # get all revolute joint names and pybullet index - rev_joint_index_name_dic, prismatic_joint_index_name_dic, fixed_joint_index_name_dic, link_names_to_ids_dic = self.get_properties() - # import plugins dynamically - self.plugins = [] - plugins = rospy.get_param('~plugins', []) - if not plugins: - rospy.logwarn('No plugins found, forgot to set param ~plugins?') - # return to normal shell color - print('\033[0m') - # load plugins - for plugin in plugins: - module_ = plugin.pop("module") - class_ = plugin.pop("class") - params_ = plugin.copy() - rospy.loginfo('loading plugin: {} class from {}'.format(class_, module_)) - # create object of the imported file class - obj = getattr(importlib.import_module(module_), class_)(self.pb, self.robot, - rev_joints=rev_joint_index_name_dic, - prism_joints=prismatic_joint_index_name_dic, - fixed_joints=fixed_joint_index_name_dic, - link_ids=link_names_to_ids_dic, - **params_) - # store objects in member variable for future use - self.plugins.append(obj) - rospy.loginfo('pybullet ROS wrapper initialized') - - def get_properties(self): - """ - construct 3 dictionaries: - - joint index to joint name x2 (1 for revolute, 1 for fixed joints) - - link name to link index dictionary - """ - rev_joint_index_name_dic = {} - fixed_joint_index_name_dic = {} - prismatic_joint_index_name_dic = {} - link_names_to_ids_dic = {} - for joint_index in range(0, self.pb.getNumJoints(self.robot)): - info = self.pb.getJointInfo(self.robot, joint_index) - # build a dictionary of link names to ids - link_names_to_ids_dic[info[12].decode('utf-8')] = joint_index - # ensure we are dealing with a revolute joint - if info[2] == self.pb.JOINT_REVOLUTE: - # insert key, value in dictionary (joint index, joint name) - rev_joint_index_name_dic[joint_index] = info[1].decode('utf-8') # info[1] refers to joint name - elif info[2] == self.pb.JOINT_FIXED: - # insert key, value in dictionary (joint index, joint name) - fixed_joint_index_name_dic[joint_index] = info[1].decode('utf-8') # info[1] refers to joint name - elif info[2] == self.pb.JOINT_PRISMATIC: - prismatic_joint_index_name_dic[joint_index] = info[1].decode('utf-8') # info[1] refers to joint name - return rev_joint_index_name_dic, prismatic_joint_index_name_dic, fixed_joint_index_name_dic, link_names_to_ids_dic - - def handle_reset_simulation(self, req): - """Callback to handle the service offered by this node to reset the simulation""" - rospy.loginfo('reseting simulation now') - self.pb.resetSimulation() - return Empty() - - def start_gui(self, gui=True): - """start physics engine (client) with or without gui""" - if(gui): - # start simulation with gui - rospy.loginfo('Running pybullet with gui') - rospy.loginfo('-------------------------') - gui_options = rospy.get_param('~gui_options', '') # e.g. to maximize screen: options="--width=2560 --height=1440" - return self.pb.connect(self.pb.GUI, options=gui_options) - else: - # start simulation without gui (non-graphical version) - rospy.loginfo('Running pybullet without gui') - # hide console output from pybullet - rospy.loginfo('-------------------------') - return self.pb.connect(self.pb.DIRECT) - - def init_pybullet_robot(self): - """load robot URDF model, set gravity, ground plane and environment""" - # get from param server the path to the URDF robot model to load at startup - urdf_path = rospy.get_param('~robot_urdf_path', None) - if urdf_path == None: - rospy.signal_shutdown('mandatory param robot_urdf_path not set, will exit now') - # test urdf file existance - if not os.path.isfile(urdf_path): - rospy.logerr('param robot_urdf_path is set, but file does not exist : ' + urdf_path) - rospy.signal_shutdown('required robot urdf file not found') - return None - # ensure urdf is not xacro, but if it is then make urdf file version out of it - if 'xacro' in urdf_path: - robot_description = rospy.get_param('robot_description', None) - if not robot_description: - rospy.logerr('required robot_description param not set') - return None - # remove xacro from name - urdf_path_without_xacro = urdf_path[0:urdf_path.find('.xacro')]+urdf_path[urdf_path.find('.xacro')+len('.xacro'):] - rospy.loginfo('generating urdf model from xacro from robot_description param server under: {0}'.format(urdf_path_without_xacro)) - try: - urdf_file = open(urdf_path_without_xacro,'w') - except: - rospy.logerr('Failed to create urdf file from xacro, cannot write into destination: {0}'.format(urdf_path_without_xacro)) - return None - urdf_file.write(robot_description) - urdf_file.close() - urdf_path = urdf_path_without_xacro - # get robot spawn pose from parameter server - robot_pose_x = rospy.get_param('~robot_pose_x', 0.0) - robot_pose_y = rospy.get_param('~robot_pose_y', 0.0) - robot_pose_z = rospy.get_param('~robot_pose_z', 1.0) - robot_pose_yaw = rospy.get_param('~robot_pose_yaw', 0.0) - robot_spawn_orientation = self.pb.getQuaternionFromEuler([0.0, 0.0, robot_pose_yaw]) - fixed_base = rospy.get_param('~fixed_base', False) - # load robot from URDF model - # user decides if inertia is computed automatically by pybullet or custom - if rospy.get_param('~use_intertia_from_file', False): - # combining several boolean flags using "or" according to pybullet documentation - urdf_flags = self.pb.URDF_USE_INERTIA_FROM_FILE | self.pb.URDF_USE_SELF_COLLISION - else: - urdf_flags = self.pb.URDF_USE_SELF_COLLISION - # load environment - rospy.loginfo('loading environment') - self.environment.load_environment() - # set no realtime simulation, NOTE: no need to stepSimulation if setRealTimeSimulation is set to 1 - self.pb.setRealTimeSimulation(0) # NOTE: does not currently work with effort controller, thats why is left as 0 - rospy.loginfo('loading urdf model: ' + urdf_path) - # NOTE: self collision enabled by default - return self.pb.loadURDF(urdf_path, basePosition=[robot_pose_x, robot_pose_y, robot_pose_z], - baseOrientation=robot_spawn_orientation, - useFixedBase=fixed_base, flags=urdf_flags) - - def handle_reset_simulation(self, req): - """Callback to handle the service offered by this node to reset the simulation""" - rospy.loginfo('reseting simulation now') - # pause simulation to prevent reading joint values with an empty world - self.pause_simulation = True - # remove all objects from the world and reset the world to initial conditions - self.pb.resetSimulation() - # load URDF model again, set gravity and floor - self.init_pybullet_robot() - # resume simulation control cycle now that a new robot is in place - self.pause_simulation = False - return [] - - def handle_pause_physics(self, req): - """pause simulation, raise flag to prevent pybullet to execute self.pb.stepSimulation()""" - rospy.loginfo('pausing simulation') - self.pause_simulation = False - return [] - - def handle_unpause_physics(self, req): - """unpause simulation, lower flag to allow pybullet to execute self.pb.stepSimulation()""" - rospy.loginfo('unpausing simulation') - self.pause_simulation = True - return [] - - def pause_simulation_function(self): - return self.pause_simulation - - def start_pybullet_ros_wrapper_sequential(self): - """ - This function is deprecated, we recommend the use of parallel plugin execution - """ - rate = rospy.Rate(self.loop_rate) - while not rospy.is_shutdown(): - if not self.pause_simulation: - # run x plugins - for task in self.plugins: - task.execute() - # perform all the actions in a single forward dynamics simulation step such - # as collision detection, constraint solving and integration - self.pb.stepSimulation() - rate.sleep() - rospy.logwarn('killing node now...') - # if node is killed, disconnect - if self.connected_to_physics_server: - self.pb.disconnect() - - def start_pybullet_ros_wrapper_parallel(self): - """ - Execute plugins in parallel, however watch their execution time and warn if exceeds the deadline (loop rate) - """ - # create object of our parallel execution manager - exec_manager_obj = FuncExecManager(self.plugins, rospy.is_shutdown, self.pb.stepSimulation, self.pause_simulation_function, - log_info=rospy.loginfo, log_warn=rospy.logwarn, log_debug=rospy.logdebug, function_name='plugin') - # start parallel execution of all "execute" class methods in a synchronous way - exec_manager_obj.start_synchronous_execution(loop_rate=self.loop_rate) - # ctrl + c was pressed, exit - rospy.logwarn('killing node now...') - # if node is killed, disconnect - if self.connected_to_physics_server: - self.pb.disconnect() - - def start_pybullet_ros_wrapper(self): - if rospy.get_param('~parallel_plugin_execution', True): - self.start_pybullet_ros_wrapper_parallel() - else: - self.start_pybullet_ros_wrapper_sequential() - -def main(): - """function called by pybullet_ros_node script""" - rospy.init_node('pybullet_ros', anonymous=False) # node name gets overrided if launched by a launch file - pybullet_ros_interface = pyBulletRosWrapper() - pybullet_ros_interface.start_pybullet_ros_wrapper() diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..5f2d31e --- /dev/null +++ b/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/pybullet_ros +[install] +install_scripts=$base/lib/pybullet_ros diff --git a/setup.py b/setup.py index c03af95..166445a 100644 --- a/setup.py +++ b/setup.py @@ -1,12 +1,38 @@ #!/usr/bin/env python3 -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +import os +from glob import glob +from setuptools import find_packages, setup -# for your packages to be recognized by python -d = generate_distutils_setup( - packages=['pybullet_ros'], - package_dir={'pybullet_ros': 'ros/src/pybullet_ros'} -) +package_name = 'pybullet_ros' + +common_files = [f for f in glob('common/**/*', recursive=True) if os.path.isfile(f)] +config_files = [f for f in glob('config/**/*', recursive=True) if os.path.isfile(f)] +launch_files = [f for f in glob('launch/**/*', recursive=True) if os.path.isfile(f)] -setup(**d) +setup( + name=package_name, + version='2.0.0', + packages=find_packages(where='pybullet_ros'), + package_dir={'': 'pybullet_ros'}, + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/pybullet_ros']), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', launch_files), + ('share/' + package_name + '/config', config_files), + ('share/' + package_name + '/common', common_files), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Oscar Lima', + maintainer_email='oscar.lima@dfki.de', + description='ROS wrapper for pybullet simulator', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'pybullet_ros_node = pybullet_ros.pybullet_ros:main', + 'joint_states_to_float = pybullet_ros.scripts.joint_states_to_float:main', + ], + }, +)