diff --git a/astrobee/launch/astrobee.launch.py b/astrobee/launch/astrobee.launch.py index cd8065f57f..7805faf1c7 100644 --- a/astrobee/launch/astrobee.launch.py +++ b/astrobee/launch/astrobee.launch.py @@ -113,7 +113,7 @@ def generate_launch_description(): "output" : LaunchConfiguration("output"), "gtloc" : LaunchConfiguration("gtloc"), # Use Ground Truth Localizer }.items(), - condition=LaunchConfigurationNotEquals("llp", "disabled"), + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("llp"), "disabled")), ) ] ), @@ -137,7 +137,7 @@ def generate_launch_description(): "output" : LaunchConfiguration("output"), "gtloc" : LaunchConfiguration("gtloc"), # Use Ground Truth Localizer }.items(), - condition=LaunchConfigurationNotEquals("llp", "disabled"), + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("mlp"), "disabled")), ) ] ) diff --git a/astrobee/launch/controller/descriptions.launch.py b/astrobee/launch/controller/descriptions.launch.py index 01ce90d129..0c43d97f89 100644 --- a/astrobee/launch/controller/descriptions.launch.py +++ b/astrobee/launch/controller/descriptions.launch.py @@ -40,7 +40,7 @@ def generate_launch_description(): name="astrobee_state_publisher", parameters=[{'robot_description': ParameterValue(granite_robot_description) }], arguments=[granite_urdf], - condition=LaunchConfigurationEquals("world", "granite") + condition=IfCondition(EqualsSubstitution(LaunchConfiguration("world"), "granite")) ), # Granite robot description Node( @@ -50,7 +50,7 @@ def generate_launch_description(): name="astrobee_state_publisher", parameters=[{'robot_description': ParameterValue(iss_robot_description) }], arguments=[iss_urdf], - condition=LaunchConfigurationEquals("world", "iss") + condition=IfCondition(EqualsSubstitution(LaunchConfiguration("world"), "iss")) ), # Granite robot description Node( diff --git a/astrobee/launch/controller/sim_start.launch.py b/astrobee/launch/controller/sim_start.launch.py index 54f9e42512..d1e8da6dc1 100644 --- a/astrobee/launch/controller/sim_start.launch.py +++ b/astrobee/launch/controller/sim_start.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument("world", default_value="iss"), # Robot namespace - DeclareLaunchArgument("sviz", default_value="true"), # Robot pose + DeclareLaunchArgument("sviz", default_value="false"), # Robot pose DeclareLaunchArgument("vmware", default_value="true"), # Robot description DeclareLaunchArgument("speed", default_value="1"), # Robot description DeclareLaunchArgument("debug", default_value="false"), # Robot description diff --git a/astrobee/launch/controller/synthetic.launch.py b/astrobee/launch/controller/synthetic.launch.py index 5f1a0747a8..53f51cb5aa 100644 --- a/astrobee/launch/controller/synthetic.launch.py +++ b/astrobee/launch/controller/synthetic.launch.py @@ -25,7 +25,7 @@ def generate_launch_description(): # # - if LaunchConfigurationNotEquals("bag", ""): + if IfCondition(EqualsSubstitution(LaunchConfiguration("bag"), "")): return LaunchDescription([ DeclareLaunchArgument("ns"), # Robot namespace DeclareLaunchArgument("pose"), # Robot pose @@ -33,17 +33,17 @@ def generate_launch_description(): # SetEnvironmentVariable(name="ASTROBEE_CONFIG_DIR", value="/home/astrobee/native/config", - condition=LaunchConfigurationNotEquals("sim", "local")), + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))), SetEnvironmentVariable(name="ASTROBEE_RESOURCE_DIR", value=os.getenv("ASTROBEE_RESOURCE_DIR", get_path("/resources")), - condition=LaunchConfigurationNotEquals("sim", "local")), + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))), SetEnvironmentVariable(name="ROSCONSOLE_CONFIG_FILE", value=os.getenv("ROSCONSOLE_CONFIG_FILE", get_path("/resources/logging.config")), - condition=LaunchConfigurationNotEquals("sim", "local")), + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))), SetEnvironmentVariable(name="DISPLAY", value=":0", - condition=LaunchConfigurationNotEquals("sim", "local")), + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))), SetEnvironmentVariable(name="ROS_IP", value=LaunchConfiguration("sim"), - condition=LaunchConfigurationNotEquals("sim", "local")), + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))), IncludeLaunchDescription( get_launch_file("launch/spawn_astrobee.launch.py", "astrobee_gazebo"), diff --git a/astrobee/launch/robot/LLP.launch.py b/astrobee/launch/robot/LLP.launch.py index 59959404ae..6a2b711a46 100644 --- a/astrobee/launch/robot/LLP.launch.py +++ b/astrobee/launch/robot/LLP.launch.py @@ -22,19 +22,19 @@ def generate_launch_description(): return LaunchDescription([ - SetEnvironmentVariable(name="ASTROBEE_ROBOT", condition=LaunchConfigurationNotEquals("llp", "local"), + SetEnvironmentVariable(name="ASTROBEE_ROBOT", condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("llp"), "local")), value=os.getenv("ASTROBEE_ROBOT", LaunchConfiguration("robot"))), - SetEnvironmentVariable(name="ASTROBEE_WORLD", condition=LaunchConfigurationNotEquals("llp", "local"), + SetEnvironmentVariable(name="ASTROBEE_WORLD", condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("llp"), "local")), value=os.getenv("ASTROBEE_WORLD", LaunchConfiguration("world"))), - SetEnvironmentVariable(name="ASTROBEE_CONFIG_DIR", condition=LaunchConfigurationNotEquals("llp", "local"), + SetEnvironmentVariable(name="ASTROBEE_CONFIG_DIR", condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("llp"), "local")), value=os.getenv("ASTROBEE_CONFIG_DIR", "/opt/astrobee/config")), - SetEnvironmentVariable(name="ASTROBEE_RESOURCE_DIR", condition=LaunchConfigurationNotEquals("llp", "local"), + SetEnvironmentVariable(name="ASTROBEE_RESOURCE_DIR", condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("llp"), "local")), value=os.getenv("ASTROBEE_RESOURCE_DIR", "/res")), - SetEnvironmentVariable(name="ROSCONSOLE_CONFIG_FILE", condition=LaunchConfigurationNotEquals("llp", "local"), + SetEnvironmentVariable(name="ROSCONSOLE_CONFIG_FILE", condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("llp"), "local")), value=os.getenv("ROSCONSOLE_CONFIG_FILE", "/res/logging.config")), - SetEnvironmentVariable(name="ROS_HOSTNAME", condition=LaunchConfigurationNotEquals("llp", "local"), + SetEnvironmentVariable(name="ROS_HOSTNAME", condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("llp"), "local")), value=LaunchConfiguration("llp")), diff --git a/astrobee/launch/robot/MLP.launch.py b/astrobee/launch/robot/MLP.launch.py index 31259375ea..24eba15345 100644 --- a/astrobee/launch/robot/MLP.launch.py +++ b/astrobee/launch/robot/MLP.launch.py @@ -23,18 +23,18 @@ def generate_launch_description(): return LaunchDescription([ # Update the environment variables relating to absolute paths - SetEnvironmentVariable(name="ASTROBEE_ROBOT", condition=LaunchConfigurationNotEquals("mlp", "local"), + SetEnvironmentVariable(name="ASTROBEE_ROBOT", condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("mlp"), "local")), value=os.getenv("ASTROBEE_ROBOT", LaunchConfiguration("robot"))), - SetEnvironmentVariable(name="ASTROBEE_WORLD", condition=LaunchConfigurationNotEquals("mlp", "local"), + SetEnvironmentVariable(name="ASTROBEE_WORLD", condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("mlp"), "local")), value=os.getenv("ASTROBEE_WORLD", LaunchConfiguration("world"))), - SetEnvironmentVariable(name="ASTROBEE_CONFIG_DIR", condition=LaunchConfigurationNotEquals("mlp", "local"), + SetEnvironmentVariable(name="ASTROBEE_CONFIG_DIR", condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("mlp"), "local")), value=os.getenv("ASTROBEE_CONFIG_DIR", "/opt/astrobee/config")), - SetEnvironmentVariable(name="ASTROBEE_RESOURCE_DIR", condition=LaunchConfigurationNotEquals("mlp", "local"), + SetEnvironmentVariable(name="ASTROBEE_RESOURCE_DIR", condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("mlp"), "local")), value=os.getenv("ASTROBEE_RESOURCE_DIR", "/res")), - SetEnvironmentVariable(name="ROSCONSOLE_CONFIG_FILE", condition=LaunchConfigurationNotEquals("mlp", "local"), + SetEnvironmentVariable(name="ROSCONSOLE_CONFIG_FILE", condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("mlp"), "local")), value=os.getenv("ROSCONSOLE_CONFIG_FILE", "/res/logging.config")), - SetEnvironmentVariable(name="ROS_HOSTNAME", condition=LaunchConfigurationNotEquals("mlp", "local"), + SetEnvironmentVariable(name="ROS_HOSTNAME", condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("mlp"), "local")), value=LaunchConfiguration("mlp")), @@ -74,7 +74,8 @@ def generate_launch_description(): # remappings=[('/image', '/burgerimage')], # parameters=[{'history': 'keep_last'}], # extra_arguments=[{'use_intra_process_comms': True}]) - ] + ], + output=LaunchConfiguration("output") ), ComposableNodeContainer( name='mlp_localization', @@ -96,7 +97,8 @@ def generate_launch_description(): # remappings=[('/image', '/burgerimage')], # parameters=[{'history': 'keep_last'}], # extra_arguments=[{'use_intra_process_comms': True}]) - ] + ], + output=LaunchConfiguration("output") ), ComposableNodeContainer( name='mlp_graph_localization', @@ -231,7 +233,8 @@ def generate_launch_description(): name='mapper', parameters=[{'use_sim_time': True}], extra_arguments=[{'use_intra_process_comms': False}]), - ] + ], + output=LaunchConfiguration("output") ), ComposableNodeContainer( name='mlp_management', diff --git a/astrobee/launch/sim.launch.py b/astrobee/launch/sim.launch.py index e051588b85..c4de60be81 100644 --- a/astrobee/launch/sim.launch.py +++ b/astrobee/launch/sim.launch.py @@ -61,9 +61,9 @@ def generate_launch_description(): condition=IfCondition(LaunchConfiguration("perch"))), # Default is using JPM Berth 1, for Berth 2 use: '9.817 -10.312 4.293 1 0 0 0' launch_arg("pose", default_value="9.816 -9.806 4.293 0 0 0", - condition=LaunchConfigurationEquals("world", "iss")), + condition=IfCondition(EqualsSubstitution(LaunchConfiguration("world"), "iss"))), launch_arg("pose", default_value="0 0 -0.7 0 0 0", - condition=LaunchConfigurationEquals("world", "granite")), + condition=IfCondition(EqualsSubstitution(LaunchConfiguration("world"), "granite"))), # Multi-robot simulation launch_arg("honey", default_value="false", description="Insert honey robot"), launch_arg("bumble", default_value="false", description="Insert bumble robot"), @@ -125,7 +125,7 @@ def generate_launch_description(): # Launch a recorder for this robot IncludeLaunchDescription( get_launch_file("launch/controller/bagrecord.launch.py"), - condition=LaunchConfigurationNotEquals("rec", ""), + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("rec"), "")), launch_arguments={"bag": LaunchConfiguration("rec")}.items(), ), # @@ -135,18 +135,18 @@ def generate_launch_description(): # Update the environment variables relating to absolute paths SetEnvironmentVariable(name="ASTROBEE_CONFIG_DIR", value=os.getenv("ASTROBEE_CONFIG_DIR", "/home/astrobee/native/config"), - condition=LaunchConfigurationNotEquals("sim", "local")), + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))), SetEnvironmentVariable(name="ASTROBEE_RESOURCE_DIR", value=os.getenv("ASTROBEE_RESOURCE_DIR", "/home/astrobee/native/resources"), - condition=LaunchConfigurationNotEquals("sim", "local")), + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))), SetEnvironmentVariable(name="ROSCONSOLE_CONFIG_FILE", value=os.getenv("ROSCONSOLE_CONFIG_FILE", "/home/astrobee/native/resources/logging.config"), - condition=LaunchConfigurationNotEquals("sim", "local")), + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))), SetEnvironmentVariable(name="DISPLAY", value=":0", - condition=LaunchConfigurationNotEquals("sim", "local")), + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))), SetEnvironmentVariable(name="ROS_IP", value=LaunchConfiguration("sim"), - condition=LaunchConfigurationNotEquals("sim", "local")), + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration("sim"), "local"))), # Start the simulator IncludeLaunchDescription( get_launch_file("launch/controller/sim_start.launch.py"), diff --git a/astrobee/launch/spawn.launch.py b/astrobee/launch/spawn.launch.py index 5e349d0a5b..422852a359 100644 --- a/astrobee/launch/spawn.launch.py +++ b/astrobee/launch/spawn.launch.py @@ -25,9 +25,9 @@ def generate_launch_description(): DeclareLaunchArgument("robot", default_value=os.getenv("ASTROBEE_ROBOT", "sim")), DeclareLaunchArgument("world", default_value=os.getenv("ASTROBEE_WORLD", "iss")), DeclareLaunchArgument("pose", default_value="9.92 -9.54 4.50 0 0 0", - condition=LaunchConfigurationEquals("world", "iss")), + condition=IfCondition(EqualsSubstitution(LaunchConfiguration("world"), "iss"))), DeclareLaunchArgument("pose", default_value="9.92 -9.54 4.50 0 0 0", - condition=LaunchConfigurationEquals("world", "granite")), + condition=IfCondition(EqualsSubstitution(LaunchConfiguration("world"), "granite"))), # Make sure all environment variables are set for controller # Override the robot and world environment variables all the time. The diff --git a/astrobee/utilities/utilities.py b/astrobee/utilities/utilities.py index 3068140de7..5b7efcd491 100755 --- a/astrobee/utilities/utilities.py +++ b/astrobee/utilities/utilities.py @@ -29,8 +29,6 @@ ) from launch.conditions import ( IfCondition, - LaunchConfigurationEquals, - LaunchConfigurationNotEquals, UnlessCondition, ) from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -41,6 +39,8 @@ LaunchConfiguration, PythonExpression, TextSubstitution, + EqualsSubstitution, + NotEqualsSubstitution ) from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode, ParameterValue diff --git a/description/description/urdf/macro_perching_arm.urdf.xacro b/description/description/urdf/macro_perching_arm.urdf.xacro index 2244b649ba..246eca317c 100644 --- a/description/description/urdf/macro_perching_arm.urdf.xacro +++ b/description/description/urdf/macro_perching_arm.urdf.xacro @@ -58,7 +58,7 @@ - - + - - + diff --git a/description/description/urdf/model.urdf.xacro b/description/description/urdf/model.urdf.xacro index 4e872adaa8..1d7913fb06 100644 --- a/description/description/urdf/model.urdf.xacro +++ b/description/description/urdf/model.urdf.xacro @@ -114,23 +114,23 @@ - + - + - + diff --git a/description/description/urdf/model_drag.urdf.xacro b/description/description/urdf/model_drag.urdf.xacro index 79ff9a080b..b1ffbe4023 100644 --- a/description/description/urdf/model_drag.urdf.xacro +++ b/description/description/urdf/model_drag.urdf.xacro @@ -19,10 +19,13 @@ - > + + drag_plugin 1.05 0.092903 1.225 - \ No newline at end of file + diff --git a/description/description/urdf/model_eps.urdf.xacro b/description/description/urdf/model_eps.urdf.xacro index eb08d0a726..c455253d50 100644 --- a/description/description/urdf/model_eps.urdf.xacro +++ b/description/description/urdf/model_eps.urdf.xacro @@ -19,7 +19,10 @@ - + + eps_plugin 10.0 0.04 0.05 @@ -29,5 +32,14 @@ false false + + + body + dock + body + + diff --git a/description/description/urdf/model_monitors.urdf.xacro b/description/description/urdf/model_monitors.urdf.xacro index 01fb17757a..bd0a020d0c 100644 --- a/description/description/urdf/model_monitors.urdf.xacro +++ b/description/description/urdf/model_monitors.urdf.xacro @@ -19,16 +19,24 @@ - + llp_disk_monitor - + mlp_disk_monitor - + llp_cpu_mem_monitor - + mlp_cpu_mem_monitor diff --git a/description/description/urdf/model_truth.urdf.xacro b/description/description/urdf/model_truth.urdf.xacro index 64c0cc6a90..c312967335 100644 --- a/description/description/urdf/model_truth.urdf.xacro +++ b/description/description/urdf/model_truth.urdf.xacro @@ -19,7 +19,10 @@ - + + plugin_truth 62.5 world truth diff --git a/description/description/urdf/sensor_imu.urdf.xacro b/description/description/urdf/sensor_imu.urdf.xacro index ef10554261..1e9e023a7c 100644 --- a/description/description/urdf/sensor_imu.urdf.xacro +++ b/description/description/urdf/sensor_imu.urdf.xacro @@ -24,7 +24,9 @@ true 62.5 0 - + diff --git a/mobility/mapper/CMakeLists.txt b/mobility/mapper/CMakeLists.txt index da6810c5b6..528f699b2d 100644 --- a/mobility/mapper/CMakeLists.txt +++ b/mobility/mapper/CMakeLists.txt @@ -20,7 +20,7 @@ project(mapper) # Default to C++14 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -O3 -fPIC" ) @@ -76,7 +76,7 @@ add_library(${PROJECT_NAME} SHARED ) target_compile_definitions(${PROJECT_NAME} PRIVATE "COMPOSITION_BUILDING_DLL") -target_link_libraries(${PROJECT_NAME} ${OCTOMAP_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${OCTOMAP_LIBRARIES} ${PCL_LIBRARIES}) ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_components std_srvs ff_msgs sensor_msgs pcl_msgs ff_util visualization_msgs octomap) rclcpp_components_register_nodes(${PROJECT_NAME} "mapper::MapperComponent") diff --git a/simulation/CMakeLists.txt b/simulation/CMakeLists.txt index a22129f757..dac78a3dcf 100644 --- a/simulation/CMakeLists.txt +++ b/simulation/CMakeLists.txt @@ -98,29 +98,27 @@ target_link_libraries(astrobee_gazebo gz-sensors::gz-sensors gz-rendering::gz-re ament_target_dependencies(astrobee_gazebo rclcpp ff_common ff_util ros_gz) # Create a model plugin for the EPS system -#add_library(gazebo_model_plugin_eps SHARED -# src/gazebo_model_plugin_eps/gazebo_model_plugin_eps.cc -#) -#target_link_libraries(gazebo_model_plugin_eps astrobee_gazebo) -#ament_target_dependencies(gazebo_model_plugin_eps rclcpp ff_hw_msgs tf2 tf2_ros ros_gz ) -#ament_export_libraries(gazebo_model_plugin_eps) +add_library(gazebo_model_plugin_eps SHARED + src/gazebo_model_plugin_eps/gazebo_model_plugin_eps.cc +) +target_link_libraries(gazebo_model_plugin_eps astrobee_gazebo) +ament_target_dependencies(gazebo_model_plugin_eps ff_hw_msgs tf2 tf2_ros ) # Create a model plugin for aerodynamic drag -#add_library(gazebo_model_plugin_drag SHARED -# src/gazebo_model_plugin_drag/gazebo_model_plugin_drag.cc -#) -#target_link_libraries(gazebo_model_plugin_drag astrobee_gazebo) -#ament_target_dependencies(gazebo_model_plugin_drag rclcpp ros_gz ) -#ament_export_libraries(gazebo_model_plugin_drag) +add_library(gazebo_model_plugin_drag SHARED + src/gazebo_model_plugin_drag/gazebo_model_plugin_drag.cc +) +target_link_libraries(gazebo_model_plugin_drag astrobee_gazebo) # Create a model plugin for the ground truth -#add_library(gazebo_model_plugin_truth SHARED -# src/gazebo_model_plugin_truth/gazebo_model_plugin_truth.cc -#) -#target_link_libraries(gazebo_model_plugin_truth astrobee_gazebo) +add_library(gazebo_model_plugin_truth SHARED + src/gazebo_model_plugin_truth/gazebo_model_plugin_truth.cc +) +target_link_libraries(gazebo_model_plugin_truth astrobee_gazebo) #ament_target_dependencies(gazebo_model_plugin_truth rclcpp tf2 tf2_ros ros_gz ) #ament_export_libraries(gazebo_model_plugin_truth) + # Create a model plugin for the propulsion system #add_library(gazebo_model_plugin_pmc SHARED # src/gazebo_model_plugin_pmc/gazebo_model_plugin_pmc.cc @@ -134,8 +132,7 @@ ament_target_dependencies(astrobee_gazebo rclcpp ff_common ff_util ros_gz) # src/gazebo_model_plugin_perching_arm/gazebo_model_plugin_perching_arm.cc #) #target_link_libraries(gazebo_model_plugin_perching_arm astrobee_gazebo) -#ament_target_dependencies(gazebo_model_plugin_perching_arm rclcpp ff_msgs ff_hw_msgs ros_gz ) -#ament_export_libraries(gazebo_model_plugin_perching_arm) +#ament_target_dependencies(gazebo_model_plugin_perching_arm rclcpp ff_msgs ff_hw_msgs ) # Create a model plugin for the front flashlight #add_library(gazebo_model_plugin_flashlight SHARED @@ -154,12 +151,10 @@ ament_target_dependencies(astrobee_gazebo rclcpp ff_common ff_util ros_gz) #ament_export_libraries(gazebo_model_plugin_laser) # Create a model plugin to publish heartbeats -#add_library(gazebo_model_plugin_heartbeat SHARED -# src/gazebo_model_plugin_heartbeat/gazebo_model_plugin_heartbeat.cc -#) -#target_link_libraries(gazebo_model_plugin_heartbeat astrobee_gazebo) -#ament_target_dependencies(gazebo_model_plugin_heartbeat rclcpp ros_gz ) -#ament_export_libraries(gazebo_model_plugin_heartbeat) +add_library(gazebo_model_plugin_heartbeat SHARED + src/gazebo_model_plugin_heartbeat/gazebo_model_plugin_heartbeat.cc +) +target_link_libraries(gazebo_model_plugin_heartbeat astrobee_gazebo) # Create a model plugin for the signal lights #add_library(gazebo_model_plugin_signal_lights SHARED @@ -176,6 +171,9 @@ ament_target_dependencies(astrobee_gazebo rclcpp ff_common ff_util ros_gz) #target_link_libraries(gazebo_sensor_plugin_imu astrobee_gazebo) #ament_target_dependencies(gazebo_sensor_plugin_imu rclcpp ros_gz ) #ament_export_libraries(gazebo_sensor_plugin_imu) +#target_include_directories( +# gazebo_sensor_plugin_imu PRIVATE include +#) # Create a sensor plugin for the handrail detection algorithm #add_library(gazebo_sensor_plugin_handrail_detect SHARED @@ -239,11 +237,6 @@ add_library(gazebo_world_plugin_speed SHARED ) target_link_libraries(gazebo_world_plugin_speed gz-sim::gz-sim) -#target_include_directories( -# gazebo_world_plugin_speed PRIVATE include -#) - - # Create a system plugin for the client gui #add_library(gazebo_system_plugin_client SHARED # src/gazebo_system_plugin_client/gazebo_system_plugin_client.cc @@ -268,14 +261,14 @@ ament_export_include_directories(include) ## Install install(TARGETS astrobee_gazebo -# gazebo_model_plugin_eps -# gazebo_model_plugin_drag -# gazebo_model_plugin_truth + gazebo_model_plugin_eps + gazebo_model_plugin_drag + gazebo_model_plugin_truth # gazebo_model_plugin_pmc # gazebo_model_plugin_perching_arm # gazebo_model_plugin_flashlight # gazebo_model_plugin_laser -# gazebo_model_plugin_heartbeat + gazebo_model_plugin_heartbeat # gazebo_model_plugin_signal_lights # gazebo_sensor_plugin_imu # gazebo_sensor_plugin_depth_cam diff --git a/simulation/config/ros_gz_astrobee_bridge.yaml b/simulation/config/ros_gz_astrobee_bridge.yaml new file mode 100644 index 0000000000..e48c37fba0 --- /dev/null +++ b/simulation/config/ros_gz_astrobee_bridge.yaml @@ -0,0 +1,6 @@ +--- +- ros_topic_name: "/clock" + gz_topic_name: "/clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "gz.msgs.Clock" + direction: GZ_TO_ROS diff --git a/simulation/hooks/astrobee_gazebo.dsv.in b/simulation/hooks/astrobee_gazebo.dsv.in index b14eb21687..20259a879b 100644 --- a/simulation/hooks/astrobee_gazebo.dsv.in +++ b/simulation/hooks/astrobee_gazebo.dsv.in @@ -1 +1,2 @@ prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share/@PROJECT_NAME@/worlds +prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib/@PROJECT_NAME@/ diff --git a/simulation/hooks/astrobee_gazebo.sh.in b/simulation/hooks/astrobee_gazebo.sh.in index 3d270f301b..32a6154430 100644 --- a/simulation/hooks/astrobee_gazebo.sh.in +++ b/simulation/hooks/astrobee_gazebo.sh.in @@ -1 +1,2 @@ -prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib/@PROJECT_NAME@/ +ament_prepend_unique_value GZ_SIM_RESOURCE_PATH "$AMENT_CURRENT_PREFIX/share/@PROJECT_NAME@/worlds" +ament_prepend_unique_value GZ_SIM_PLUGIN_PATH "$AMENT_CURRENT_PREFIX/lib/@PROJECT_NAME@" diff --git a/simulation/include/astrobee_gazebo/astrobee_gazebo.h b/simulation/include/astrobee_gazebo/astrobee_gazebo.h index 161a206472..dff7b74786 100644 --- a/simulation/include/astrobee_gazebo/astrobee_gazebo.h +++ b/simulation/include/astrobee_gazebo/astrobee_gazebo.h @@ -49,7 +49,9 @@ typedef msg::CameraInfo CameraInfo; // Gazebo includes #include #include +#include #include +#include //#include // Eigen includes @@ -60,11 +62,7 @@ typedef msg::CameraInfo CameraInfo; #include #include -namespace gz { - -namespace sim { - -namespace system { +namespace astrobee_gazebo { // Convenience wrapper around a model plugin class FreeFlyerPlugin : public ff_util::FreeFlyerComponent { @@ -96,17 +94,18 @@ class FreeFlyerPlugin : public ff_util::FreeFlyerComponent { // Manage the extrinsics based on the sensor type void SetupExtrinsics(); - // Custom callback queue to avoid contention between the global callback - // queue and gazebo update work. - // void CallbackThread(); - // Child classes need access std::string robot_name_, plugin_name_, plugin_frame_, parent_frame_; NodeHandle nh_; std::shared_ptr buffer_; std::shared_ptr listener_; - // ros::CallbackQueue callback_queue_; - std::thread thread_; + + // Spin node based on: https://github.com/ros-controls/gz_ros2_control/blob/rolling/gz_ros2_control/src/gz_ros2_control_plugin.cpp + // Thread where the executor will spin + std::thread thread_executor_spin_; + // Executor to spin the controller + rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; + ff_util::FreeFlyerTimer timer_; }; @@ -130,16 +129,20 @@ class FreeFlyerModelPlugin : public FreeFlyerPlugin, gz::sim::EntityComponentManager &_ecm, gz::sim::EventManager &_eventManager) override; - virtual void PreUpdate(const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) = 0; + void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm); + + virtual void PreUpdate_(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) {} virtual void PostUpdate(const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm) = 0; + const gz::sim::EntityComponentManager &_ecm) {}; + protected: - // Get the model link - gz::sim::Entity GetLink(); + // Get the model's canonical link + std::shared_ptr GetLink(); // Get the model world gz::sim::Entity GetWorld(); @@ -148,17 +151,19 @@ class FreeFlyerModelPlugin : public FreeFlyerPlugin, std::shared_ptr GetModel(); // Callback when the model has loaded - virtual void LoadCallback(NodeHandle &nh, - std::shared_ptr model, sdf::ElementPtr sdf) = 0; + virtual void LoadCallback(NodeHandle &nh, gz::sim::EntityComponentManager &_ecm) = 0; // Manage the extrinsics based on the sensor type virtual bool ExtrinsicsCallback(geometry_msgs::TransformStamped const* tf); - private: + + protected: sdf::ElementPtr sdf_; - gz::sim::Entity link_; gz::sim::Entity world_; + std::shared_ptr link_; std::shared_ptr model_; + bool update_extrinsics_; + gz::math::Pose3d extrinsics_pose_; }; @@ -184,11 +189,15 @@ class FreeFlyerSensorPlugin : public FreeFlyerPlugin, gz::sim::EntityComponentManager &_ecm, gz::sim::EventManager &_eventManager) override; - virtual void PreUpdate(const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) = 0; + void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm); + + virtual void PreUpdate_(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) {}; virtual void PostUpdate(const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm) = 0; + const gz::sim::EntityComponentManager &_ecm) {}; + protected: @@ -203,17 +212,19 @@ class FreeFlyerSensorPlugin : public FreeFlyerPlugin, std::string GetRotationType(); // Callback when the sensor has loaded - virtual void LoadCallback(NodeHandle &nh, - std::shared_ptr sensor, sdf::ElementPtr sdf) = 0; + virtual void LoadCallback(NodeHandle &nh, gz::sim::EntityComponentManager &_ecm) = 0; // Manage the extrinsics based on the sensor type virtual bool ExtrinsicsCallback(geometry_msgs::TransformStamped const* tf); private: - std::shared_ptr sensor_; + gz::sim::Entity sensor_entity_; + //std::shared_ptr sensor_; gz::sim::Entity world_; std::shared_ptr model_; sdf::ElementPtr sdf_; + bool update_extrinsics_; + gz::math::Pose3d extrinsics_pose_; }; // Utility functions @@ -225,11 +236,7 @@ Eigen::Affine3d SensorToWorld(gz::math::Pose3d const& world_pose, // Read the camera info //void FillCameraInfo(rendering::CameraPtr camera, sensor_msgs::CameraInfo & info_msg); -} // namespace systems - -} // namespace sim - -} // namespace gz +} // namespace astrobee_gazebo #endif // ASTROBEE_GAZEBO_ASTROBEE_GAZEBO_H_ diff --git a/simulation/launch/spawn_astrobee.launch.py b/simulation/launch/spawn_astrobee.launch.py index 26991aa270..748acd7c53 100644 --- a/simulation/launch/spawn_astrobee.launch.py +++ b/simulation/launch/spawn_astrobee.launch.py @@ -43,16 +43,22 @@ def launch_setup(context, *args, **kwargs): entity = ns spawn_entity = Node( - package='gazebo_ros', - executable='spawn_entity.py', + package='ros_gz_sim', + executable='create', name='spawn_astrobee', output='screen', - arguments=["-topic", topic, "-entity", entity, "-timeout", "30.0", - "-x", LaunchConfiguration("x"), "-y", LaunchConfiguration("y"), "-z", LaunchConfiguration("z"), - "-R", LaunchConfiguration("R"), "-P", LaunchConfiguration("P"), "-Y", LaunchConfiguration("Y"), - "-robot_namespace", LaunchConfiguration("ns")] - ) - + arguments=[ + "-topic", topic, + "-name", entity, + "-x", LaunchConfiguration("x"), + "-y", LaunchConfiguration("y"), + "-z", LaunchConfiguration("z"), + "-R", LaunchConfiguration("R"), + "-P", LaunchConfiguration("P"), + "-Y", LaunchConfiguration("Y")] + ) + #"-robot_namespace", LaunchConfiguration("ns") + return [spawn_entity] def generate_launch_description(): diff --git a/simulation/launch/start_simulation.launch.py b/simulation/launch/start_simulation.launch.py index 55ab755df7..eeffb9fc41 100644 --- a/simulation/launch/start_simulation.launch.py +++ b/simulation/launch/start_simulation.launch.py @@ -66,6 +66,18 @@ def generate_launch_description(): ]) ], condition=IfCondition(LaunchConfiguration('gui')) - ), + ), + + # Publish the Clock -- Bridge ROS topics and Gazebo messages for establishing communication + Node( + name="bridge_clock", + package='ros_gz_bridge', + executable='parameter_bridge', + parameters=[{ + 'config_file': os.path.join(pkg_astrobee_gazebo, 'config', 'ros_gz_astrobee_bridge.yaml'), + 'qos_overrides./tf_static.publisher.durability': 'transient_local', + }], + output='screen' + ) ]) diff --git a/simulation/src/astrobee_gazebo.cc b/simulation/src/astrobee_gazebo.cc index 7b42cc3be1..99a7cc7b3d 100644 --- a/simulation/src/astrobee_gazebo.cc +++ b/simulation/src/astrobee_gazebo.cc @@ -23,11 +23,7 @@ #include #include -namespace gz { - -namespace sim { - -namespace system { +namespace astrobee_gazebo { FF_DEFINE_LOGGER("gazebo"); @@ -41,7 +37,8 @@ FreeFlyerPlugin::FreeFlyerPlugin(std::string const& plugin_name, // Destructor FreeFlyerPlugin::~FreeFlyerPlugin() { // nh_ff_.shutdown(); - thread_.join(); + this->executor_->cancel(); + this->thread_executor_spin_.join(); } // Some plugins might want the world as the parent frame @@ -52,7 +49,7 @@ void FreeFlyerPlugin::SetParentFrame(std::string const& parent) { // Load function void FreeFlyerPlugin::InitializePlugin(std::string const& robot_name, std::string const& plugin_name, sdf::ElementPtr sdf) { - gzwarn << "Starting plugin " << plugin_name_ << plugin_name << std::endl; + gzwarn << "Starting plugin " << plugin_name_ << ": "<Get("node_name"); - nh_ = rclcpp::Node::make_shared(node_name); + nh_ = rclcpp::Node::make_shared(plugin_name, robot_name); + + // Start a thread to spin the node + this->executor_ = std::make_shared(); + this->executor_->add_node(nh_); + auto spin = [this]() { this->executor_->spin(); }; + this->thread_executor_spin_ = std::thread(spin); + // Initialize ROS node for Gazebo FreeFlyerComponent::FreeFlyerComponentGazeboInit(nh_, plugin_name); FF_DEBUG_STREAM("Loading " << plugin_name_ << plugin_name << " on robot " << robot_name_); // Get nodehandle based on the model name. - // nh_.setCallbackQueue(&callback_queue_); - // thread_ = std::thread(&FreeFlyerPlugin::CallbackThread, this); buffer_.reset(new tf2_ros::Buffer(nh_->get_clock())); listener_.reset(new tf2_ros::TransformListener(*buffer_)); @@ -84,12 +85,6 @@ void FreeFlyerPlugin::InitializePlugin(std::string const& robot_name, std::strin std::bind(&FreeFlyerPlugin::SetupExtrinsics, this), nh_); } -// Service the callback thread -// void FreeFlyerPlugin::CallbackThread() { -// while (nh_.ok()) -// callback_queue_.callAvailable(ros::WallDuration(0.01)); -// } - // Poll for extrinsics until found void FreeFlyerPlugin::SetupExtrinsics() { // If we don't need extrinsics, then don't bother looking... @@ -126,23 +121,27 @@ FreeFlyerModelPlugin::FreeFlyerModelPlugin(std::string const& plugin_name, FreeFlyerPlugin::FreeFlyerPlugin( plugin_name, plugin_frame, send_heartbeats) { - link_ = gz::sim::kNullEntity; + link_ = nullptr; world_ = gz::sim::kNullEntity; model_ = nullptr; + update_extrinsics_ = false; } // Destructor FreeFlyerModelPlugin::~FreeFlyerModelPlugin() {} // Auto-called when Gazebo loads the plugin -void FreeFlyerModelPlugin::Configure(const gz::sim::Entity &_entity, +void FreeFlyerModelPlugin::Configure(const gz::sim::Entity &_model_entity, const std::shared_ptr &_sdf, gz::sim::EntityComponentManager &_ecm, gz::sim::EventManager &_eventMgr) { + sdf_ = _sdf->Clone(); - //link_ = model->GetLink(); + model_.reset( new gz::sim::Model(_model_entity) ); + + auto link_entity = model_->CanonicalLink(_ecm); + link_.reset( new gz::sim::Link(link_entity) ); //world_ = model->GetWorld(); - //model_ = gz::sim::Model(_entity); // Read namespace std::string ns = model_->Name(_ecm); @@ -151,23 +150,34 @@ void FreeFlyerModelPlugin::Configure(const gz::sim::Entity &_entity, // Read plugin custom name if specified std::string plugin_name = ""; - if (_sdf->HasElement("plugin_name")) - plugin_name = _sdf->Get("plugin_name"); + if (sdf_->HasElement("plugin_name")) + plugin_name = sdf_->Get("plugin_name"); // Read plugin custom frame if specified - if (_sdf->HasElement("plugin_frame")) - plugin_frame_ = _sdf->Get("plugin_frame"); + if (sdf_->HasElement("plugin_frame")) + plugin_frame_ = sdf_->Get("plugin_frame"); // Initialize the FreeFlyerPlugin InitializePlugin(ns, plugin_name, sdf_); // Now load the rest of the plugin - LoadCallback(nh_, model_, sdf_); + LoadCallback(nh_, _ecm); } +void FreeFlyerModelPlugin::PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + + if(update_extrinsics_) + { + model_->SetWorldPoseCmd(_ecm, extrinsics_pose_); + update_extrinsics_ = false; + } + PreUpdate_(_info, _ecm); +} // Get the model link -gz::sim::Entity FreeFlyerModelPlugin::GetLink() { +std::shared_ptr FreeFlyerModelPlugin::GetLink() { return link_; } @@ -187,7 +197,7 @@ bool FreeFlyerModelPlugin::ExtrinsicsCallback( // A tf nullptr means no transform is required if (tf) { // Handle the transform for all sensor types - gz::math::Pose3d pose( + extrinsics_pose_ = gz::math::Pose3d( tf->transform.translation.x, tf->transform.translation.y, tf->transform.translation.z, @@ -196,7 +206,7 @@ bool FreeFlyerModelPlugin::ExtrinsicsCallback( tf->transform.rotation.y, tf->transform.rotation.z); // Set the model pose - //model_->SetWorldPoseCmd(_ecm, pose); + update_extrinsics_ = true; } // Success return true; @@ -208,7 +218,9 @@ bool FreeFlyerModelPlugin::ExtrinsicsCallback( FreeFlyerSensorPlugin::FreeFlyerSensorPlugin(std::string const& plugin_name, std::string const& plugin_frame, bool send_heartbeats) : FreeFlyerPlugin::FreeFlyerPlugin( - plugin_name, plugin_frame, send_heartbeats) {} + plugin_name, plugin_frame, send_heartbeats) { + update_extrinsics_ = false; +} // Destructor FreeFlyerSensorPlugin::~FreeFlyerSensorPlugin() {} @@ -218,8 +230,18 @@ void FreeFlyerSensorPlugin::Configure(const gz::sim::Entity &_entity, const std::shared_ptr &_sdf, gz::sim::EntityComponentManager &_ecm, gz::sim::EventManager &_eventMgr) { - //sensor_ = sensor; + sdf_ = _sdf->Clone(); + + if(!_ecm.Component(_entity)) + { + printf("Error, this plugin should be attached to a sensor!!!!! \n"); + return; + } + + sensor_entity_ = _entity; + //sensor_ = new gz::sim::Sensor(sensor_entity); + //world_ = gazebo::physics::get_world(sensor->WorldName()); // Store pointer to model @@ -242,7 +264,7 @@ void FreeFlyerSensorPlugin::Configure(const gz::sim::Entity &_entity, InitializePlugin(ns, plugin_name, sdf_); // Now load the rest of the plugin - LoadCallback(nh_, sensor_, sdf_); + LoadCallback(nh_, _ecm); } // Get the sensor world @@ -255,6 +277,20 @@ std::shared_ptr FreeFlyerSensorPlugin::GetModel() { return model_; } + +void FreeFlyerSensorPlugin::PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + if(update_extrinsics_) + { + //model_->SetWorldPoseCmd(_ecm, extrinsics_pose_); + update_extrinsics_ = false; + } + + PreUpdate_(_info, _ecm); +} + + // Manage the extrinsics bool FreeFlyerSensorPlugin::ExtrinsicsCallback( geometry_msgs::TransformStamped const* tf) { @@ -299,7 +335,7 @@ bool FreeFlyerSensorPlugin::ExtrinsicsCallback( gz::math::Pose3d world_pose(tf_bs + tf_wb); // In the case of a camera update the camera world pose - if (sensor_->Type() == "camera") { + if (gz::sim::entityTypeId(sensor_entity_, ecm_) == gz::sim::components::Camera::typeId) { sensors::CameraSensorPtr sensor = std::dynamic_pointer_cast(sensor_); if (sensor && sensor->Camera()) @@ -309,9 +345,9 @@ bool FreeFlyerSensorPlugin::ExtrinsicsCallback( } // In the case of a wide angle camera update the camera world pose - if (sensor_->Type() == "wideanglecamera") { - std::shared_ptr sensor = - std::dynamic_pointer_cast(sensor_); + if (gz::sim::entityTypeId(sensor_entity_, ecm) == gz::sim::components::WideAngleCamera::typeId) { + std::shared_ptr sensor = gz::sim::components::WideAngleCamera(sensor_entity_); + //std::dynamic_pointer_cast(sensor_); if (sensor && sensor->Camera()) sensor->Camera()->SetWorldPose(world_pose); else @@ -319,7 +355,7 @@ bool FreeFlyerSensorPlugin::ExtrinsicsCallback( } // In the case of a depth camera update the depth camera pose - if (sensor_->Type() == "depth") { + if (gz::sim::entityTypeId(sensor_entity_, ecm) == gz::sim::components::DepthCamera::typeId) { sensors::DepthCameraSensorPtr sensor = std::dynamic_pointer_cast(sensor_); if (sensor && sensor->DepthCamera()) @@ -406,8 +442,4 @@ void FillCameraInfo(rendering::CameraPtr camera, sensor_msgs::CameraInfo & msg) } }*/ -} // namespace system - -} // namespace sim - -} // namespace gz +} // namespace astrobee_gazebo diff --git a/simulation/src/gazebo_model_plugin_drag/gazebo_model_plugin_drag.cc b/simulation/src/gazebo_model_plugin_drag/gazebo_model_plugin_drag.cc index 59fe0a05a2..dc1eec0d3a 100644 --- a/simulation/src/gazebo_model_plugin_drag/gazebo_model_plugin_drag.cc +++ b/simulation/src/gazebo_model_plugin_drag/gazebo_model_plugin_drag.cc @@ -18,8 +18,10 @@ // Gazebo includes #include +#include +#include -namespace gazebo { +namespace astrobee_gazebo { // This class is a plugin that calls the GNC autocode to predict // the forced to be applied to the rigid body @@ -29,73 +31,69 @@ class GazeboModelPluginDrag : public FreeFlyerModelPlugin { coefficient_(1.05), area_(0.092903), density_(1.225) {} ~GazeboModelPluginDrag() { - #if GAZEBO_MAJOR_VERSION > 7 - update_.reset(); - #else - event::Events::DisconnectWorldUpdateBegin(update_); - #endif } protected: // Called when the plugin is loaded into the simulator - void LoadCallback(NodeHandle &nh, physics::ModelPtr model, sdf::ElementPtr sdf) { + void LoadCallback(NodeHandle &nh, gz::sim::EntityComponentManager &_ecm) { // Drag coefficient - if (sdf->HasElement("coefficient")) - coefficient_ = sdf->Get("coefficient"); + if (sdf_->HasElement("coefficient")) + coefficient_ = sdf_->Get("coefficient"); // Cross-sectional area - if (sdf->HasElement("area")) - area_ = sdf->Get("area"); + if (sdf_->HasElement("area")) + area_ = sdf_->Get("area"); // Air density - if (sdf->HasElement("density")) - density_ = sdf->Get("density"); - // Called before each iteration of simulated world update - #if GAZEBO_MAJOR_VERSION > 7 - next_tick_ = GetWorld()->SimTime(); - #else - next_tick_ = GetWorld()->GetSimTime(); - #endif - update_ = event::Events::ConnectWorldUpdateBegin( - std::bind(&GazeboModelPluginDrag::WorldUpdateCallback, this)); + if (sdf_->HasElement("density")) + density_ = sdf_->Get("density"); } // Called on simulation reset void Reset() { - #if GAZEBO_MAJOR_VERSION > 7 - next_tick_ = GetWorld()->SimTime(); - #else - next_tick_ = GetWorld()->GetSimTime(); - #endif } - // Called on each sensor update event - void WorldUpdateCallback() { + // Called on each sensor update event + void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) + { // Calculate drag - #if GAZEBO_MAJOR_VERSION > 7 - drag_ = GetLink()->RelativeLinearVel(); + //drag_ = GetLink()->RelativeLinearVel(); // ANA HACK - Check if this works + std::optional linvel = GetLink()->WorldLinearVelocity(_ecm, gz::math::Vector3d(0, 0, 0)); + if(!linvel) + return; + + drag_ = linvel.value(); + vel_ = drag_.Length(); - #else - drag_ = GetLink()->GetRelativeLinearVel(); - vel_ = drag_.GetLength(); - #endif drag_ = -0.5 * coefficient_ * area_ * density_ * vel_ * vel_ * drag_.Normalize(); // Apply the force and torque to the model - GetLink()->AddRelativeForce(drag_); + GetLink()->AddWorldForce(_ecm, drag_, gz::math::Vector3d(0,0,0) ); //AddRelativeForce(drag_); // ANA HACK - Check if this work + } + void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) + { + } + + private: double coefficient_, area_, density_, vel_; // Drag parameters - common::Time next_tick_; -#if GAZEBO_MAJOR_VERSION > 7 - ignition::math::Vector3d drag_; -#else - math::Vector3 drag_; -#endif - event::ConnectionPtr update_; + gz::math::Vector3d drag_; }; +} // namespace gazebo + + // Register this plugin with the simulator -GZ_REGISTER_MODEL_PLUGIN(GazeboModelPluginDrag) +GZ_ADD_PLUGIN( + astrobee_gazebo::GazeboModelPluginDrag, + gz::sim::System, + astrobee_gazebo::GazeboModelPluginDrag::ISystemConfigure, + astrobee_gazebo::GazeboModelPluginDrag::ISystemPreUpdate, + astrobee_gazebo::GazeboModelPluginDrag::ISystemPostUpdate +) + + -} // namespace gazebo diff --git a/simulation/src/gazebo_model_plugin_eps/gazebo_model_plugin_eps.cc b/simulation/src/gazebo_model_plugin_eps/gazebo_model_plugin_eps.cc index f4bb062b93..394510d929 100644 --- a/simulation/src/gazebo_model_plugin_eps/gazebo_model_plugin_eps.cc +++ b/simulation/src/gazebo_model_plugin_eps/gazebo_model_plugin_eps.cc @@ -21,6 +21,11 @@ #include #include +// Gazebo +#include +#include +#include + // Standard messages #include #include @@ -79,12 +84,13 @@ typedef msg::Header Header; #include #include -namespace gazebo { +namespace astrobee_gazebo { FF_DEFINE_LOGGER("gazebo_model_plugin_eps"); using FSM = ff_util::FSM; - +using namespace std::chrono_literals; + /* Provides a simple EPS plugin for helping with the the docking procedure. Like the EPS, it publishes a dock state. To do this is it first determines @@ -121,7 +127,8 @@ class GazeboModelPluginEps : public FreeFlyerModelPlugin { this, std::placeholders::_1, std::placeholders::_2)), rate_(10.0), distance_near_(0.05), distance_far_(0.05), delay_(5.0), lock_(false), battery_capacity_(3.4), battery_charge_(3.0), - battery_discharge_rate_(0.005) { + battery_discharge_rate_(0.005), + update_(true) { // In an unknown state, if we are sensed to be near or far from a berth // then update to either a docked or undocked state. fsm_.Add(UNKNOWN, SENSE_NEAR | SENSE_FAR, @@ -210,31 +217,29 @@ class GazeboModelPluginEps : public FreeFlyerModelPlugin { // Destructor ~GazeboModelPluginEps() { - #if GAZEBO_MAJOR_VERSION > 7 - update_.reset(); - #else - event::Events::DisconnectWorldUpdateEnd(update_); - #endif + update_ = false; } protected: // Called when the plugin is loaded into the simulator - void LoadCallback(NodeHandle &nh, - physics::ModelPtr model, sdf::ElementPtr sdf) { - + void LoadCallback(NodeHandle &nh, gz::sim::EntityComponentManager &_ecm) + { + // Save model name + model_name_ = model_->Name(_ecm); + // Initialize Transform lookup buffer_.reset(new tf2_ros::Buffer(nh->get_clock())); listener_.reset(new tf2_ros::TransformListener(*buffer_)); // Get parameters - if (sdf->HasElement("rate")) - rate_ = sdf->Get("rate"); - if (sdf->HasElement("distance_near")) - distance_near_ = sdf->Get("distance_near"); - if (sdf->HasElement("distance_far")) - distance_far_ = sdf->Get("distance_far"); - if (sdf->HasElement("delay")) - delay_ = sdf->Get("delay"); + if (sdf_->HasElement("rate")) + rate_ = sdf_->Get("rate"); + if (sdf_->HasElement("distance_near")) + distance_near_ = sdf_->Get("distance_near"); + if (sdf_->HasElement("distance_far")) + distance_far_ = sdf_->Get("distance_far"); + if (sdf_->HasElement("delay")) + delay_ = sdf_->Get("delay"); // Setup telemetry publishers pub_dock_state_ = FF_CREATE_PUBLISHER(nh, ff_hw_msgs::EpsDockStateStamped, TOPIC_HARDWARE_EPS_DOCK_STATE, 1); pub_housekeeping_ = FF_CREATE_PUBLISHER(nh, ff_hw_msgs::EpsHousekeeping, TOPIC_HARDWARE_EPS_HOUSEKEEPING, 1); @@ -268,34 +273,31 @@ class GazeboModelPluginEps : public FreeFlyerModelPlugin { // Create timer to publish battery states telem_timer_.createTimer(5.0, std::bind(&GazeboModelPluginEps::TelemetryCallback, this), nh_, false, true); - // Defer the extrinsics setup to allow plugins to load - update_ = event::Events::ConnectWorldUpdateEnd( - std::bind(&GazeboModelPluginEps::BerthCallback, this)); // Initialize battery states // state_tl_.header.stamp = ros::Time::now(); state_tl_.location = ff_hw_msgs::EpsBatteryLocation::TOP_LEFT; - state_tl_.present = sdf->Get("battery_top_left"); + state_tl_.present = sdf_->Get("battery_top_left"); state_tl_.capacity = battery_capacity_; state_tl_.charge = battery_charge_; state_tl_.percentage = state_tl_.charge / state_tl_.capacity; battery_state_pub_tl_->publish(state_tl_); // state_tr_.header.stamp = ros::Time::now(); state_tr_.location = ff_hw_msgs::EpsBatteryLocation::TOP_RIGHT; - state_tr_.present = sdf->Get("battery_top_right"); + state_tr_.present = sdf_->Get("battery_top_right"); state_tr_.capacity = battery_capacity_; state_tr_.charge = battery_charge_; state_tr_.percentage = state_tr_.charge / state_tl_.capacity; battery_state_pub_tr_->publish(state_tr_); // state_bl_.header.stamp = ros::Time::now(); state_bl_.location = ff_hw_msgs::EpsBatteryLocation::BOTTOM_LEFT; - state_bl_.present = sdf->Get("battery_bottom_left"); + state_bl_.present = sdf_->Get("battery_bottom_left"); state_bl_.capacity = battery_capacity_; state_bl_.charge = battery_charge_; state_bl_.percentage = state_bl_.charge / state_bl_.capacity; battery_state_pub_bl_->publish(state_bl_); // state_br_.header.stamp = ros::Time::now(); state_br_.location = ff_hw_msgs::EpsBatteryLocation::BOTTOM_RIGHT; - state_br_.present = sdf->Get("battery_bottom_right"); + state_br_.present = sdf_->Get("battery_bottom_right"); state_br_.capacity = battery_capacity_; state_br_.charge = battery_charge_; state_br_.percentage = state_br_.charge /state_br_.capacity; @@ -391,8 +393,23 @@ class GazeboModelPluginEps : public FreeFlyerModelPlugin { FF_DEBUG_STREAM("State changed to " << str); } - // Manage the extrinsics based on the sensor type - void BerthCallback() { + // Defer the extrinsics setup to allow plugins to load + void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) override + { + if(update_) + BerthCallback(); + + // Get model pose + mutex_data_.lock(); + model_pose_ = GetLink()->WorldPose(_ecm); + mutex_data_.unlock(); + + } + + // Manage the extrinsics based on the sensor type + void BerthCallback() + { // Create a buffer and listener for TF2 transforms static geometry_msgs::TransformStamped tf; // Get extrinsics from framestore @@ -401,7 +418,7 @@ class GazeboModelPluginEps : public FreeFlyerModelPlugin { tf = buffer_->lookupTransform( "world", "dock/berth1/complete", ros::Time(0)); // Handle the transform for all sensor types - berths_["dock/berth1/complete"] = ignition::math::Pose3d( + berths_["dock/berth1/complete"] = gz::math::Pose3d( tf.transform.translation.x, tf.transform.translation.y, tf.transform.translation.z, @@ -413,7 +430,7 @@ class GazeboModelPluginEps : public FreeFlyerModelPlugin { tf = buffer_->lookupTransform( "world", "dock/berth2/complete", ros::Time(0)); // Handle the transform for all sensor types - berths_["dock/berth2/complete"] = ignition::math::Pose3d( + berths_["dock/berth2/complete"] = gz::math::Pose3d( tf.transform.translation.x, tf.transform.translation.y, tf.transform.translation.z, @@ -422,11 +439,7 @@ class GazeboModelPluginEps : public FreeFlyerModelPlugin { tf.transform.rotation.y, tf.transform.rotation.z); // Kill the connection when we have a dock pose - #if GAZEBO_MAJOR_VERSION > 7 - update_.reset(); - #else - event::Events::DisconnectWorldUpdateEnd(update_); - #endif + update_ = false; // Once we have berth locations start timer for checking dock status timer_update_.start(); // If we have an exception we need to quietly wait for transform(s) @@ -436,34 +449,32 @@ class GazeboModelPluginEps : public FreeFlyerModelPlugin { // Create a virtual joint to lock the freeflyer to the berth void Lock(bool enable) { if (enable) { - // We are not guaranteed to have a dock yet, so we need to check to see - // that the model pointer is valid. If it is valid, then we to quietly - // ignore locking for the time being. - #if GAZEBO_MAJOR_VERSION > 7 - physics::ModelPtr dock = GetWorld()->ModelByName("dock"); - #else - physics::ModelPtr dock = GetWorld()->GetModel("dock"); - #endif - if (dock == nullptr) - return; - // By this point we are guaranteed to have a dock - #if GAZEBO_MAJOR_VERSION > 7 - joint_ = GetWorld()->Physics()->CreateJoint("fixed", GetModel()); - #else - joint_ = GetWorld()->GetPhysicsEngine()->CreateJoint("fixed", GetModel()); - #endif - joint_->Attach(GetModel()->GetLink(), dock->GetLink()); + // Send a request to lock/attach + gz::transport::Node node; + std::string attach_topic = "/model/" + model_name_ + "/detachable_joint/attach"; + auto attachPub = node.Advertise(attach_topic); + attachPub.Publish(gz::msgs::Empty()); + std::this_thread::sleep_for(250ms); // If we have an air carriage, stop colliding with anything - physics::LinkPtr link = GetModel()->GetLink("body"); - if (link) - link->SetCollideMode("none"); - } else if (joint_) { - joint_->Detach(); - joint_->Fini(); + // ANA MIGRATION HACK: CANNOT DO THIS YET IN LATEST GAZEBO + //Link link = GetModel()->GetLink("body"); + //if (link) + // link->SetCollideMode("none"); + } else { + + // Send a request to detach + gz::transport::Node node; + + std::string detach_topic = "/model/" + model_name_ + "/detachable_joint/attach"; + auto pub = node.Advertise(detach_topic); + pub.Publish(gz::msgs::Empty()); + std::this_thread::sleep_for(250ms); + // If we have an air carriage, start colliding with everything - physics::LinkPtr link = GetModel()->GetLink("body"); - if (link) - link->SetCollideMode("all"); + // ANA MIGRATION HACK: CANNOT DO THIS YET IN LATEST GAZEBO + //Link link = GetModel()->GetLink("body"); + //if (link) + // link->SetCollideMode("all"); } } @@ -475,13 +486,17 @@ class GazeboModelPluginEps : public FreeFlyerModelPlugin { bool near = false; bool far = true; for (nearest_ = berths_.begin(); nearest_ != berths_.end(); nearest_++) { - #if GAZEBO_MAJOR_VERSION > 7 - double distance = GetModel()->WorldPose().Pos().Distance( - nearest_->second.Pos()); - #else - double distance = GetModel()->GetWorldPose().Ign().Pos().Distance( + + gz::math::Pose3d world_pose; + mutex_data_.lock(); + if(!model_pose_) + return; // Is this correct? ANA MIGRATION CHECK + + world_pose = model_pose_.value(); + mutex_data_.unlock(); + + double distance = world_pose.Pos().Distance( nearest_->second.Pos()); - #endif // There should always only be one dock that we are close to if (distance < distance_near_) { @@ -704,15 +719,17 @@ class GazeboModelPluginEps : public FreeFlyerModelPlugin { private: ff_util::FSM fsm_; + std::string model_name_; double rate_, distance_near_, distance_far_ , delay_; bool lock_; double battery_capacity_, battery_charge_, battery_discharge_rate_; - event::ConnectionPtr update_; - std::map berths_; - std::map::iterator nearest_; - ignition::math::Vector3d force_; - physics::JointPtr joint_; + + std::map berths_; + std::map::iterator nearest_; + gz::math::Vector3d force_; + ff_util::FreeFlyerTimer timer_update_, timer_delay_, telem_timer_; + bool update_; rclcpp::Publisher::SharedPtr pub_dock_state_; rclcpp::Publisher::SharedPtr pub_housekeeping_; rclcpp::Publisher::SharedPtr pub_power_; @@ -728,9 +745,17 @@ class GazeboModelPluginEps : public FreeFlyerModelPlugin { std::shared_ptr buffer_; std::shared_ptr listener_; + std::optional model_pose_; + std::mutex mutex_data_; }; -// Register this plugin with the simulator -GZ_REGISTER_MODEL_PLUGIN(GazeboModelPluginEps) +} // namespace astrobee_gazebo -} // namespace gazebo +// Register this plugin with the simulator +GZ_ADD_PLUGIN( + astrobee_gazebo::GazeboModelPluginEps, + gz::sim::System, + astrobee_gazebo::GazeboModelPluginEps::ISystemConfigure, + astrobee_gazebo::GazeboModelPluginEps::ISystemPreUpdate, + astrobee_gazebo::GazeboModelPluginEps::ISystemPostUpdate +) diff --git a/simulation/src/gazebo_model_plugin_heartbeat/gazebo_model_plugin_heartbeat.cc b/simulation/src/gazebo_model_plugin_heartbeat/gazebo_model_plugin_heartbeat.cc index 02568c08b0..a160078aff 100644 --- a/simulation/src/gazebo_model_plugin_heartbeat/gazebo_model_plugin_heartbeat.cc +++ b/simulation/src/gazebo_model_plugin_heartbeat/gazebo_model_plugin_heartbeat.cc @@ -23,11 +23,12 @@ // Gazebo includes #include +#include // STL includes #include -namespace gazebo { +namespace astrobee_gazebo { FF_DEFINE_LOGGER("gazebo_model_plugin_empty"); @@ -40,17 +41,33 @@ class GazeboModelPluginHeartbeat : public FreeFlyerModelPlugin { protected: // Called when the plugin is loaded into the simulator - void LoadCallback(NodeHandle& nh, - physics::ModelPtr model, sdf::ElementPtr sdf) { - } + void LoadCallback(NodeHandle &nh, gz::sim::EntityComponentManager &_ecm) { + + } // Manage the extrinsics based on the sensor type bool ExtrinsicsCallback(geometry_msgs::TransformStamped const* tf) { return true; } + + void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) { + } + + void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) { + + } + }; -// Register this plugin with the simulator -GZ_REGISTER_MODEL_PLUGIN(GazeboModelPluginHeartbeat) +} // namespace astrobee_gazebo -} // namespace gazebo +// Register this plugin with the simulator +GZ_ADD_PLUGIN( + astrobee_gazebo::GazeboModelPluginHeartbeat, + gz::sim::System, + astrobee_gazebo::GazeboModelPluginHeartbeat::ISystemConfigure, + astrobee_gazebo::GazeboModelPluginHeartbeat::ISystemPreUpdate, + astrobee_gazebo::GazeboModelPluginHeartbeat::ISystemPostUpdate +) diff --git a/simulation/src/gazebo_model_plugin_perching_arm/gazebo_model_plugin_perching_arm.cc b/simulation/src/gazebo_model_plugin_perching_arm/gazebo_model_plugin_perching_arm.cc index c364def2d7..a083d87199 100644 --- a/simulation/src/gazebo_model_plugin_perching_arm/gazebo_model_plugin_perching_arm.cc +++ b/simulation/src/gazebo_model_plugin_perching_arm/gazebo_model_plugin_perching_arm.cc @@ -45,7 +45,7 @@ typedef srv::CalibrateGripper CalibrateGripper; // STL includes #include -namespace gazebo { +namespace astrobee_gazebo { FF_DEFINE_LOGGER("gazebo_model_plugin_perching_arm"); @@ -102,26 +102,30 @@ class GazeboModelPluginPerchingArm : public FreeFlyerModelPlugin { static constexpr double RPM_TO_RADS_PER_S = 0.1047198; // Called when the plugin is loaded into the simulator - void LoadCallback(NodeHandle &nh, physics::ModelPtr model, - sdf::ElementPtr sdf) { + void LoadCallback(NodeHandle &nh, gz::sim::EntityComponentManager &_ecm) { // Get parameters - if (sdf->HasElement("bay")) - bay_ = sdf->Get("bay"); - if (sdf->HasElement("rate")) - rate_ = sdf->Get("rate"); + if (sdf_->HasElement("bay")) + bay_ = sdf_->Get("bay"); + if (sdf_->HasElement("rate")) + rate_ = sdf_->Get("rate"); double prox = PROXIMAL_STOWED; - if (sdf->HasElement("proximal")) - prox = sdf->Get("proximal"); + if (sdf_->HasElement("proximal")) + prox = sdf_->Get("proximal"); double dist = DISTAL_STOWED; - if (sdf->HasElement("distal")) - dist = sdf->Get("distal"); + if (sdf_->HasElement("distal")) + dist = sdf_->Get("distal"); double grip = GRIPPER_CLOSED; - if (sdf->HasElement("gripper")) - grip = sdf->Get("gripper"); + if (sdf_->HasElement("gripper")) + grip = sdf_->Get("gripper"); // Set the initial joint positions - model->GetJoint(bay_+"_arm_proximal_joint")->SetPosition(0, prox); - model->GetJoint(bay_+"_arm_distal_joint")->SetPosition(0, dist); + auto prox_entity = GetModel()->JointByName(_ecm, bay_+"_arm_proximal_joint"); + auto dist_entity = GetModel()->JointByName(_ecm, bay_+"_arm_distal_joint"); + auto prox_joint = gz::sim::Joint(prox_entity); + prox_joint->ResetPosition(_ecm, std::vector{prox}); + + auto dist_joint = gz::sim::Joint(dist_entity); + dist_joint->ResetPosition(_ecm, std::vector{dist}); SetGripperPosition(grip); // Setup the eight PID controllers used in this driver @@ -150,21 +154,21 @@ class GazeboModelPluginPerchingArm : public FreeFlyerModelPlugin { // We're going to publish all joint states plus one composite state. The // gripper states are used by rviz to print how the gripper appears. The // composite state is used by the high level driver to monitor progress. - joints_.push_back(GetModel()->GetJoint( - bay_+"_arm_proximal_joint")); - joints_.push_back(GetModel()->GetJoint( - bay_+"_arm_distal_joint")); - joints_.push_back(GetModel()->GetJoint( - bay_+"_gripper_left_proximal_joint")); - joints_.push_back(GetModel()->GetJoint( - bay_+"_gripper_left_distal_joint")); - joints_.push_back(GetModel()->GetJoint( - bay_+"_gripper_right_proximal_joint")); - joints_.push_back(GetModel()->GetJoint( - bay_+"_gripper_right_distal_joint")); + joints_.push_back(GetModel()->JointByName( + _ecm, bay_+"_arm_proximal_joint")); + joints_.push_back(GetModel()->JointByName( + _ecm, bay_+"_arm_distal_joint")); + joints_.push_back(GetModel()->JointByName( + _ecm, bay_+"_gripper_left_proximal_joint")); + joints_.push_back(GetModel()->JointByName( + _ecm, bay_+"_gripper_left_distal_joint")); + joints_.push_back(GetModel()->JointByName( + _ecm, bay_+"_gripper_right_proximal_joint")); + joints_.push_back(GetModel()->JointByName( + _ecm, bay_+"_gripper_right_distal_joint")); // Avoid resizing in each callback - msg_.header.frame_id = GetModel()->GetName(); + msg_.header.frame_id = GetModel()->Name(_ecm); msg_.name.resize(joints_.size() + 1); msg_.position.resize(joints_.size() + 1); msg_.velocity.resize(joints_.size() + 1); @@ -225,15 +229,9 @@ class GazeboModelPluginPerchingArm : public FreeFlyerModelPlugin { double value = 0.0; switch (type) { case POSITION: { - #if GAZEBO_MAJOR_VERSION > 7 double lower = joint->LowerLimit(); double upper = joint->UpperLimit(); value = (joint->Position() - lower) / (upper - lower); - #else - double lower = joint->GetLowerLimit(0).Radian(); - double upper = joint->GetUpperLimit(0).Radian(); - value = (joint->GetAngle(0).Radian() - lower) / (upper - lower); - #endif break; } @@ -254,13 +252,8 @@ class GazeboModelPluginPerchingArm : public FreeFlyerModelPlugin { void SetGripperJointGoal(std::string const& name, double position) { physics::JointPtr joint = GetModel()->GetJoint(name); // Get the joint limits - #if GAZEBO_MAJOR_VERSION > 7 double lower = joint->LowerLimit(); double upper = joint->UpperLimit(); - #else - double lower = joint->GetLowerLimit(0).Radian(); - double upper = joint->GetUpperLimit(0).Radian(); - #endif // Calculate the correct joint angle based on the position (0 - 100) double value = lower + position * (upper - lower); GetModel()->GetJointController()->SetPositionTarget( @@ -288,13 +281,8 @@ class GazeboModelPluginPerchingArm : public FreeFlyerModelPlugin { // Set the joint angle based on a gripper position from 0 to 100 void SetGripperJointPosition(std::string const& name, double position) { physics::JointPtr joint = GetModel()->GetJoint(name); - #if GAZEBO_MAJOR_VERSION > 7 - double lower = joint->LowerLimit(); - double upper = joint->UpperLimit(); - #else - double lower = joint->GetLowerLimit(0).Radian(); - double upper = joint->GetUpperLimit(0).Radian(); - #endif + double lower = joint->LowerLimit(); + double upper = joint->UpperLimit(); double value = lower + position * (upper - lower); joint->SetPosition(0, value); } @@ -345,11 +333,7 @@ class GazeboModelPluginPerchingArm : public FreeFlyerModelPlugin { size_t i = 0; for (; i < joints_.size(); i++) { msg_.name[i] = joints_[i]->GetName(); - #if GAZEBO_MAJOR_VERSION > 7 - msg_.position[i] = joints_[i]->Position(); - #else - msg_.position[i] = joints_[i]->GetAngle(0).Radian(); - #endif + msg_.position[i] = joints_[i]->Position(); msg_.velocity[i] = joints_[i]->GetVelocity(0); msg_.effort[i] = joints_[i]->GetForce(0); } @@ -432,22 +416,30 @@ class GazeboModelPluginPerchingArm : public FreeFlyerModelPlugin { rclcpp::Subscription::SharedPtr sub_; // Joint goal subscriber rclcpp::Service::SharedPtr srv_p_; // Set max pan velocity rclcpp::Service::SharedPtr srv_t_; // Set max tilt velcoity - physics::Joint_V joints_; // List of joints in system + std::vector joints_; // List of joints in system sensor_msgs::JointState msg_; // Joint state message double grip_; // Joint state message - common::PID pid_prox_p_; // PID : arm proximal position - common::PID pid_dist_p_; // PID : arm distal position - common::PID pid_gl_prox_p_; // PID : gripper left proximal position - common::PID pid_gl_dist_p_; // PID : gripper left distal position - common::PID pid_gr_prox_p_; // PID : gripper right proximal position - common::PID pid_gr_dist_p_; // PID : gripper right distal position + gz::math::PID pid_prox_p_; // PID : arm proximal position + gz::math::PID pid_dist_p_; // PID : arm distal position + gz::math::PID pid_gl_prox_p_; // PID : gripper left proximal position + gz::math::PID pid_gl_dist_p_; // PID : gripper left distal position + gz::math::PID pid_gr_prox_p_; // PID : gripper right proximal position + gz::math::PID pid_gr_dist_p_; // PID : gripper right distal position rclcpp::Service::SharedPtr srv_ps_; // Enable/Disable the proximal joint servo rclcpp::Service::SharedPtr srv_ds_; // Enable/Disable the distal joint servo rclcpp::Service::SharedPtr srv_gs_; // Enable/Disable the gripper joint servo rclcpp::Service::SharedPtr srv_c_; // Calibrate gripper }; +} // namespace astrobee_gazebo + // Register this plugin with the simulator -GZ_REGISTER_MODEL_PLUGIN(GazeboModelPluginPerchingArm) +GZ_ADD_PLUGIN( + astrobee_gazebo::GazeboModelPluginPerchingArm, + gz::sim::System, + astrobee_gazebo::GazeboModelPluginPerchingArm::ISystemConfigure, + astrobee_gazebo::GazeboModelPluginPerchingArm::ISystemPreUpdate, + astrobee_gazebo::GazeboModelPluginPerchingArm::ISystemPostUpdate +) + -} // namespace gazebo diff --git a/simulation/src/gazebo_model_plugin_truth/gazebo_model_plugin_truth.cc b/simulation/src/gazebo_model_plugin_truth/gazebo_model_plugin_truth.cc index 4655131ba0..b4cfaa2d31 100644 --- a/simulation/src/gazebo_model_plugin_truth/gazebo_model_plugin_truth.cc +++ b/simulation/src/gazebo_model_plugin_truth/gazebo_model_plugin_truth.cc @@ -18,6 +18,8 @@ // Gazebo includes #include +#include +#include // Tf2 includes #include @@ -36,7 +38,7 @@ typedef msg::TwistStamped TwistStamped; // STL includes #include -namespace gazebo { +namespace astrobee_gazebo { // This class is a plugin that calls the GNC autocode to predict // the force to be applied to the rigid body @@ -51,23 +53,22 @@ class GazeboModelPluginTruth : public FreeFlyerModelPlugin { protected: // Called when the plugin is loaded into the simulator - void LoadCallback(NodeHandle &nh, - physics::ModelPtr model, sdf::ElementPtr sdf) { + void LoadCallback(NodeHandle &nh, gz::sim::EntityComponentManager &_ecm) { // If we specify a frame name different to our sensor tag name - if (sdf->HasElement("rate")) - rate_ = sdf->Get("rate"); - if (sdf->HasElement("parent")) - parent_ = sdf->Get("parent"); - if (sdf->HasElement("child")) - child_ = sdf->Get("child"); - if (sdf->HasElement("pose")) - pose_ = sdf->Get("pose"); - if (sdf->HasElement("twist")) - twist_ = sdf->Get("twist"); - if (sdf->HasElement("tf")) - tf_ = sdf->Get("tf"); - if (sdf->HasElement("static")) - static_ = sdf->Get("static"); + if (sdf_->HasElement("rate")) + rate_ = sdf_->Get("rate"); + if (sdf_->HasElement("parent")) + parent_ = sdf_->Get("parent"); + if (sdf_->HasElement("child")) + child_ = sdf_->Get("child"); + if (sdf_->HasElement("pose")) + pose_ = sdf_->Get("pose"); + if (sdf_->HasElement("twist")) + twist_ = sdf_->Get("twist"); + if (sdf_->HasElement("tf")) + tf_ = sdf_->Get("tf"); + if (sdf_->HasElement("static")) + static_ = sdf_->Get("static"); // Setup TF2 message msg_.header.frame_id = parent_; @@ -77,26 +78,22 @@ class GazeboModelPluginTruth : public FreeFlyerModelPlugin { tf_broadcaster_ = std::make_unique(*nh); + // Enable velocity checks + // (by default it seems Gazebo does not publish links' velocities, + // so we have to explicitly request them to be published if using twist=true + GetLink()->EnableVelocityChecks(_ecm, true); + // If we are if (static_) { msg_.header.stamp = GetTimeNow(); - #if GAZEBO_MAJOR_VERSION > 7 - msg_.transform.translation.x = GetModel()->WorldPose().Pos().X(); - msg_.transform.translation.y = GetModel()->WorldPose().Pos().Y(); - msg_.transform.translation.z = GetModel()->WorldPose().Pos().Z(); - msg_.transform.rotation.x = GetModel()->WorldPose().Rot().X(); - msg_.transform.rotation.y = GetModel()->WorldPose().Rot().Y(); - msg_.transform.rotation.z = GetModel()->WorldPose().Rot().Z(); - msg_.transform.rotation.w = GetModel()->WorldPose().Rot().W(); - #else - msg_.transform.translation.x = GetModel()->GetWorldPose().pos.x; - msg_.transform.translation.y = GetModel()->GetWorldPose().pos.y; - msg_.transform.translation.z = GetModel()->GetWorldPose().pos.z; - msg_.transform.rotation.x = GetModel()->GetWorldPose().rot.x; - msg_.transform.rotation.y = GetModel()->GetWorldPose().rot.y; - msg_.transform.rotation.z = GetModel()->GetWorldPose().rot.z; - msg_.transform.rotation.w = GetModel()->GetWorldPose().rot.w; - #endif + gz::math::Pose3d pose = gz::sim::worldPose(model_->Entity(), _ecm); + msg_.transform.translation.x = pose.Pos().X(); + msg_.transform.translation.y = pose.Pos().Y(); + msg_.transform.translation.z = pose.Pos().Z(); + msg_.transform.rotation.x = pose.Rot().X(); + msg_.transform.rotation.y = pose.Rot().Y(); + msg_.transform.rotation.z = pose.Rot().Z(); + msg_.transform.rotation.w = pose.Rot().W(); tf_broadcaster_->sendTransform(msg_); return; } @@ -124,92 +121,92 @@ class GazeboModelPluginTruth : public FreeFlyerModelPlugin { last_time_ = msg_.header.stamp; } - #if GAZEBO_MAJOR_VERSION > 7 - if (tf_ && publish_tf) { - msg_.transform.translation.x = GetModel()->WorldPose().Pos().X(); - msg_.transform.translation.y = GetModel()->WorldPose().Pos().Y(); - msg_.transform.translation.z = GetModel()->WorldPose().Pos().Z(); - msg_.transform.rotation.x = GetModel()->WorldPose().Rot().X(); - msg_.transform.rotation.y = GetModel()->WorldPose().Rot().Y(); - msg_.transform.rotation.z = GetModel()->WorldPose().Rot().Z(); - msg_.transform.rotation.w = GetModel()->WorldPose().Rot().W(); - tf_broadcaster_->sendTransform(msg_); - } - // Pose - if (pose_) { - ros_truth_pose_.header = msg_.header; - ros_truth_pose_.pose.position.x = GetModel()->WorldPose().Pos().X(); - ros_truth_pose_.pose.position.y = GetModel()->WorldPose().Pos().Y(); - ros_truth_pose_.pose.position.z = GetModel()->WorldPose().Pos().Z(); - ros_truth_pose_.pose.orientation.x = GetModel()->WorldPose().Rot().X(); - ros_truth_pose_.pose.orientation.y = GetModel()->WorldPose().Rot().Y(); - ros_truth_pose_.pose.orientation.z = GetModel()->WorldPose().Rot().Z(); - ros_truth_pose_.pose.orientation.w = GetModel()->WorldPose().Rot().W(); - pub_truth_pose_->publish(ros_truth_pose_); - } - // Twist - if (twist_) { - ros_truth_twist_.header = msg_.header; - ros_truth_twist_.twist.linear.x = GetModel()->WorldLinearVel().X(); - ros_truth_twist_.twist.linear.y = GetModel()->WorldLinearVel().Y(); - ros_truth_twist_.twist.linear.z = GetModel()->WorldLinearVel().Z(); - ros_truth_twist_.twist.angular.x = GetModel()->RelativeAngularVel().X(); - ros_truth_twist_.twist.angular.y = GetModel()->RelativeAngularVel().Y(); - ros_truth_twist_.twist.angular.z = GetModel()->RelativeAngularVel().Z(); - pub_truth_twist_->publish(ros_truth_twist_); - } - #else + std::lock_guard guard(mutex_data_); + + if(!model_pose_ || !model_linear_vel_ || !model_angular_vel_) + return; + if (tf_ && publish_tf) { - msg_.transform.translation.x = GetModel()->GetWorldPose().pos.x; - msg_.transform.translation.y = GetModel()->GetWorldPose().pos.y; - msg_.transform.translation.z = GetModel()->GetWorldPose().pos.z; - msg_.transform.rotation.x = GetModel()->GetWorldPose().rot.x; - msg_.transform.rotation.y = GetModel()->GetWorldPose().rot.y; - msg_.transform.rotation.z = GetModel()->GetWorldPose().rot.z; - msg_.transform.rotation.w = GetModel()->GetWorldPose().rot.w; + msg_.transform.translation.x = model_pose_.value().Pos().X(); + msg_.transform.translation.y = model_pose_.value().Pos().Y(); + msg_.transform.translation.z = model_pose_.value().Pos().Z(); + msg_.transform.rotation.x = model_pose_.value().Rot().X(); + msg_.transform.rotation.y = model_pose_.value().Rot().Y(); + msg_.transform.rotation.z = model_pose_.value().Rot().Z(); + msg_.transform.rotation.w = model_pose_.value().Rot().W(); tf_broadcaster_->sendTransform(msg_); } // Pose if (pose_) { ros_truth_pose_.header = msg_.header; - ros_truth_pose_.pose.position.x = GetModel()->GetWorldPose().pos.x; - ros_truth_pose_.pose.position.y = GetModel()->GetWorldPose().pos.y; - ros_truth_pose_.pose.position.z = GetModel()->GetWorldPose().pos.z; - ros_truth_pose_.pose.orientation.x = GetModel()->GetWorldPose().rot.x; - ros_truth_pose_.pose.orientation.y = GetModel()->GetWorldPose().rot.y; - ros_truth_pose_.pose.orientation.z = GetModel()->GetWorldPose().rot.z; - ros_truth_pose_.pose.orientation.w = GetModel()->GetWorldPose().rot.w; + ros_truth_pose_.pose.position.x = model_pose_.value().Pos().X(); + ros_truth_pose_.pose.position.y = model_pose_.value().Pos().Y(); + ros_truth_pose_.pose.position.z = model_pose_.value().Pos().Z(); + ros_truth_pose_.pose.orientation.x = model_pose_.value().Rot().X(); + ros_truth_pose_.pose.orientation.y = model_pose_.value().Rot().Y(); + ros_truth_pose_.pose.orientation.z = model_pose_.value().Rot().Z(); + ros_truth_pose_.pose.orientation.w = model_pose_.value().Rot().W(); pub_truth_pose_->publish(ros_truth_pose_); } // Twist if (twist_) { ros_truth_twist_.header = msg_.header; - ros_truth_twist_.twist.linear.x = GetModel()->GetWorldLinearVel().x; - ros_truth_twist_.twist.linear.y = GetModel()->GetWorldLinearVel().y; - ros_truth_twist_.twist.linear.z = GetModel()->GetWorldLinearVel().z; - ros_truth_twist_.twist.angular.x = GetModel()->GetRelativeAngularVel().x; - ros_truth_twist_.twist.angular.y = GetModel()->GetRelativeAngularVel().y; - ros_truth_twist_.twist.angular.z = GetModel()->GetRelativeAngularVel().z; + ros_truth_twist_.twist.linear.x = model_linear_vel_.value().X(); + ros_truth_twist_.twist.linear.y = model_linear_vel_.value().Y(); + ros_truth_twist_.twist.linear.z = model_linear_vel_.value().Z(); + ros_truth_twist_.twist.angular.x = model_angular_vel_.value().X(); + ros_truth_twist_.twist.angular.y = model_angular_vel_.value().Y(); + ros_truth_twist_.twist.angular.z = model_angular_vel_.value().Z(); pub_truth_twist_->publish(ros_truth_twist_); } - #endif + } + void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) + { + } + + void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) + { + std::lock_guard guard(mutex_data_); + model_pose_ = GetLink()->WorldPose(_ecm); + model_linear_vel_ = GetLink()->WorldLinearVelocity(_ecm); + model_angular_vel_ = GetLink()->WorldAngularVelocity(_ecm); + } + + private: std::unique_ptr tf_broadcaster_; double rate_; bool tf_, pose_, twist_, static_; std::string parent_, child_; + + // Storing data + std::optional model_linear_vel_; + std::optional model_angular_vel_; + std::optional model_pose_; + std::mutex mutex_data_; + + // Publishing to ROS2 side geometry_msgs::TransformStamped msg_; geometry_msgs::PoseStamped ros_truth_pose_; geometry_msgs::TwistStamped ros_truth_twist_; + rclcpp::Publisher::SharedPtr pub_truth_pose_; rclcpp::Publisher::SharedPtr pub_truth_twist_; ff_util::FreeFlyerTimer timer_; rclcpp::Time last_time_; }; -// Register this plugin with the simulator -GZ_REGISTER_MODEL_PLUGIN(GazeboModelPluginTruth) +} // namespace astrobee_gazebo -} // namespace gazebo +// Register this plugin with the simulator +GZ_ADD_PLUGIN( + astrobee_gazebo::GazeboModelPluginTruth, + gz::sim::System, + astrobee_gazebo::GazeboModelPluginTruth::ISystemConfigure, + astrobee_gazebo::GazeboModelPluginTruth::ISystemPreUpdate, + astrobee_gazebo::GazeboModelPluginTruth::ISystemPostUpdate +) diff --git a/simulation/src/gazebo_sensor_plugin_dock_cam/gazebo_sensor_plugin_dock_cam.cc b/simulation/src/gazebo_sensor_plugin_dock_cam/gazebo_sensor_plugin_dock_cam.cc index 542f48b4dd..33a39fbfdc 100644 --- a/simulation/src/gazebo_sensor_plugin_dock_cam/gazebo_sensor_plugin_dock_cam.cc +++ b/simulation/src/gazebo_sensor_plugin_dock_cam/gazebo_sensor_plugin_dock_cam.cc @@ -40,13 +40,6 @@ class GazeboSensorPluginDockCam : public FreeFlyerSensorPlugin { GazeboSensorPluginDockCam() : FreeFlyerSensorPlugin("dock_cam", "dock_cam", true), rate_(0.0) {} ~GazeboSensorPluginDockCam() { - if (update_) { -#if GAZEBO_MAJOR_VERSION > 7 - update_.reset(); -#else - sensor_->DisconnectUpdated(update_); -#endif - } } protected: @@ -126,10 +119,17 @@ class GazeboSensorPluginDockCam : public FreeFlyerSensorPlugin { sensor_msgs::Image msg_; rclcpp::Publisher::SharedPtr pub_img_; std::shared_ptr sensor_; - event::ConnectionPtr update_; double rate_; }; -GZ_REGISTER_SENSOR_PLUGIN(GazeboSensorPluginDockCam) -} // namespace gazebo +} // namespace astrobee_gazebo + +// Register this plugin with the simulator +GZ_ADD_PLUGIN( + astrobee_gazebo::GazeboSensorPluginDockCam, + gz::sim::System, + astrobee_gazebo::GazeboSensorPluginDockCam::ISystemConfigure, + astrobee_gazebo::GazeboSensorPluginDockCam::ISystemPreUpdate, + astrobee_gazebo::GazeboSensorPluginDockCam::ISystemPostUpdate +)