Skip to content

Commit

Permalink
support jazzy
Browse files Browse the repository at this point in the history
  • Loading branch information
fred-labs committed Jul 29, 2024
1 parent e3d2b33 commit 6845cbb
Show file tree
Hide file tree
Showing 19 changed files with 99 additions and 88 deletions.
1 change: 1 addition & 0 deletions .github/workflows/Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@ RUN --mount=type=bind,source=.,target=/scenario_execution \
python3-pip \
xvfb \
tk \
ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \
libgl1 && \
xargs -a /scenario_execution/deb_requirements.txt apt-get install -y --no-install-recommends && \
rosdep update --rosdistro=${ROS_DISTRO} && \
Expand Down
17 changes: 9 additions & 8 deletions .github/workflows/test_build.yml
Original file line number Diff line number Diff line change
Expand Up @@ -148,7 +148,7 @@ jobs:
source /opt/ros/${{ github.event.pull_request.base.ref == 'main' && 'humble' || github.event.pull_request.base.ref }}/setup.bash
source install/setup.bash
#shellcheck disable=SC1083
scenario_batch_execution -i examples/example_scenario/ -o test_example_scenario -- ros2 launch scenario_execution_ros scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR}
scenario_batch_execution -i examples/example_scenario/ -o test_example_scenario --ignore-process-return-value -- ros2 launch scenario_execution_ros scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR}
- name: Upload result
uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4
if: always()
Expand All @@ -175,7 +175,7 @@ jobs:
source /opt/ros/${{ github.event.pull_request.base.ref == 'main' && 'humble' || github.event.pull_request.base.ref }}/setup.bash
source install/setup.bash
# shellcheck disable=SC1083
scenario_batch_execution -i examples/example_library/scenarios -o test_example_library -- ros2 launch scenario_execution_ros scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR}
scenario_batch_execution -i examples/example_library/scenarios -o test_example_library --ignore-process-return-value -- ros2 launch scenario_execution_ros scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR}
- name: Upload result
uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4
if: always()
Expand Down Expand Up @@ -203,7 +203,7 @@ jobs:
source install/setup.bash
scenario_variation -o scenario_variation_out examples/example_scenario_variation/example_scenario_variation.osc
# shellcheck disable=SC1083
scenario_batch_execution -i scenario_variation_out -o test_example_variation -- ros2 launch scenario_execution_ros scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR}
scenario_batch_execution -i scenario_variation_out -o test_example_variation --ignore-process-return-value -- ros2 launch scenario_execution_ros scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR}
- name: Upload result
uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4
if: always()
Expand Down Expand Up @@ -236,7 +236,7 @@ jobs:
export IGN_PARTITION=${HOSTNAME}:${GITHUB_RUN_ID}
sed -i 's/60s/600s/g' examples/example_nav2/example_nav2.osc
# shellcheck disable=SC1083
scenario_batch_execution -i examples/example_nav2/ -o test_example_nav2 -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=True
scenario_batch_execution -i examples/example_nav2/ -o test_example_nav2 --ignore-process-return-value -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=True rviz:=False
- name: Upload result
uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4
if: always()
Expand Down Expand Up @@ -269,7 +269,7 @@ jobs:
export IGN_PARTITION=${HOSTNAME}:${GITHUB_RUN_ID}
sed -i 's/120s/600s/g' examples/example_simulation/scenarios/example_simulation.osc
# shellcheck disable=SC1083
scenario_batch_execution -i examples/example_simulation/scenarios/ -o test_example_simulation -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=True
scenario_batch_execution -i examples/example_simulation/scenarios/ -o test_example_simulation --ignore-process-return-value -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=True rviz:=False
- name: Upload result
uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4
if: always()
Expand Down Expand Up @@ -298,11 +298,12 @@ jobs:
Xvfb :1 -screen 0 800x600x16 &
export DISPLAY=:1.0
export -n CYCLONEDDS_URI
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_DOMAIN_ID=2
export IGN_PARTITION=${HOSTNAME}:${GITHUB_RUN_ID}
sed -i 's/240s/900s/g' examples/example_multi_robot/scenarios/example_multi_robot.osc
# shellcheck disable=SC1083
scenario_batch_execution -i examples/example_multi_robot/scenarios/ -o test_example_multirobot -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} yaw:=3.14 output_dir:={OUTPUT_DIR} headless:=True
scenario_batch_execution -i examples/example_multi_robot/scenarios/ -o test_example_multirobot --ignore-process-return-value -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=True rviz:=False
- name: Upload result
uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4
if: always()
Expand Down Expand Up @@ -350,7 +351,7 @@ jobs:
export -n CYCLONEDDS_URI
export ROS_DOMAIN_ID=2
# shellcheck disable=SC1083
scenario_batch_execution -i examples/example_external_method/scenarios -o test_example_external_method -- ros2 launch scenario_execution_ros scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR}
scenario_batch_execution -i examples/example_external_method/scenarios -o test_example_external_method --ignore-process-return-value -- ros2 launch scenario_execution_ros scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR}
- name: Upload result
uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4
if: always()
Expand Down Expand Up @@ -383,7 +384,7 @@ jobs:
export ROS_DOMAIN_ID=2
export IGN_PARTITION=${HOSTNAME}:${GITHUB_RUN_ID}
# shellcheck disable=SC1083
scenario_batch_execution -i test/scenario_execution_gazebo_test/scenarios/ -o test_scenario_execution_gazebo -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR}
scenario_batch_execution -i test/scenario_execution_gazebo_test/scenarios/ -o test_scenario_execution_gazebo --ignore-process-return-value -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=True rviz:=False
- name: Upload result
uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4
if: always()
Expand Down
3 changes: 3 additions & 0 deletions .gitmodules
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
[submodule "dependencies/py_trees_ros"]
path = dependencies/py_trees_ros
url = https://github.com/splintered-reality/py_trees_ros.git
[submodule "dependencies/nav2_minimal_turtlebot_simulation"]
path = dependencies/nav2_minimal_turtlebot_simulation
url = https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git
11 changes: 8 additions & 3 deletions deb_requirements.txt
Original file line number Diff line number Diff line change
@@ -1,6 +1,11 @@
ros-humble-turtlebot4-simulator
ros-humble-py-trees-ros-interfaces
ros-humble-py-trees
ros-jazzy-nav2-minimal-tb4-sim
ros-jazzy-py-trees
ros-jazzy-py-trees-ros-interfaces
python3-autopep8
clang-format
pylint
python3-antlr4
python3-pexpect
python3-transforms3d
python3-defusedxml
cppzmq-dev
1 change: 1 addition & 0 deletions dependencies/nav2_minimal_turtlebot_simulation
4 changes: 2 additions & 2 deletions examples/example_multi_robot/launch/robot2_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -40,10 +40,10 @@ def generate_launch_description():
}],
arguments=[
[robot_name,
'/cmd_vel' + '@geometry_msgs/msg/Twist' + '[ignition.msgs.Twist'],
'/cmd_vel' + '@geometry_msgs/msg/Twist' + '[gz.msgs.Twist'],
['/model/', robot_name, '/cmd_vel' +
'@geometry_msgs/msg/Twist' +
']ignition.msgs.Twist']
']gz.msgs.Twist']
],
remappings=[
(['/model/', robot_name, '/cmd_vel'], [robot_name, '/cmd_vel'])
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,10 @@ scenario multi_robot:
robot2.spawn(pose_3d(position_3d(x: -3.0, y: 1.5, z: 0.3), orientation_3d(yaw: -1.57)), model: 'example_multi_robot://models/robot.sdf')
ros_launch("example_multi_robot", "robot2_launch.py", wait_for_shutdown: false)
ros_launch("gazebo_static_camera", "spawn_static_camera_launch.py", [ key_value('x', '-3'), key_value('z', '10'), key_value('pitch', '1.57')], wait_for_shutdown: false)
record_bag(['/tf', '/tf_static', '/scenario_execution/snapshots', '/map', '/static_camera/image_raw', '/local_costmap/costmap', '/initialpose'], use_sim_time: true)
record_bag(['/tf', '/tf_static', '/scenario_execution/snapshots', '/map', '/static_camera/image_raw', '/local_costmap/costmap', '/initialpose', '/bond'], use_sim_time: true)
parallel:
test_drive: serial:
robot.init_nav2(initial_pose: pose_3d(orientation: orientation_3d(yaw: 3.14rad)))
robot.init_nav2(initial_pose: pose_3d())
robot.nav_to_pose(goal_pose: pose_3d(position: position_3d(x: -4.0m), orientation: orientation_3d(yaw: 3.14rad)))
emit end
serial:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ def __init__(self, entity_name: str, world_name: str):

def execute(self, entity_name: str, world_name: str): # pylint: disable=arguments-differ
self.entity_name = entity_name
self.set_command(["ign", "topic", "-t", "/world/" +
self.set_command(["gz", "topic", "-t", "/world/" +
world_name + "/pose/info", "-e", "--json-output"])

def on_executed(self):
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -42,9 +42,9 @@ def __init__(self, associated_actor, entity_name: str, world_name: str):
self.current_state = DeleteActionState.IDLE

def execute(self, associated_actor, entity_name: str, world_name: str): # pylint: disable=arguments-differ
self.set_command(["ign", "service", "-s", "/world/" + world_name + "/remove",
"--reqtype", "ignition.msgs.Entity",
"--reptype", "ignition.msgs.Boolean",
self.set_command(["gz", "service", "-s", "/world/" + world_name + "/remove",
"--reqtype", "gz.msgs.Entity",
"--reptype", "gz.msgs.Boolean",
"--timeout", "1000", "--req", "name: \"" + entity_name + "\" type: MODEL"])

def on_executed(self):
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -129,9 +129,9 @@ def shutdown(self):
return

self.logger.info(f"Deleting entity '{self.entity_name}' from simulation.")
subprocess.run(["ign", "service", "-s", "/world/" + self.world_name + "/remove", # pylint: disable=subprocess-run-check
"--reqtype", "ignition.msgs.Entity",
"--reptype", "ignition.msgs.Boolean",
subprocess.run(["gz", "service", "-s", "/world/" + self.world_name + "/remove", # pylint: disable=subprocess-run-check
"--reqtype", "gz.msgs.Entity",
"--reptype", "gz.msgs.Boolean",
"--timeout", "1000", "--req", "name: \"" + self.entity_name + "\" type: MODEL"])

def on_process_finished(self, ret):
Expand Down Expand Up @@ -184,9 +184,9 @@ def set_command(self, command):
"""
pose = self.get_spawn_pose()

super().set_command(["ign", "service", "-s", "/world/" + self.world_name + "/create",
"--reqtype", "ignition.msgs.EntityFactory",
"--reptype", "ignition.msgs.Boolean",
super().set_command(["gz", "service", "-s", "/world/" + self.world_name + "/create",
"--reqtype", "gz.msgs.EntityFactory",
"--reptype", "gz.msgs.Boolean",
"--timeout", "30000", "--req", "pose: " + pose + " name: \"" + self.entity_name + "\" allow_renaming: false sdf: \"" + command + "\""])

def topic_callback(self, msg):
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ def __init__(self, world_name: str, timeout: int):
self.current_state = WaitForSimulationActionState.IDLE

def execute(self, world_name: str, timeout: int): # pylint: disable=arguments-differ
self.set_command(["ign", "topic", "-t", "/world/" +
self.set_command(["gz", "topic", "-t", "/world/" +
world_name + "/clock", "-e", "--json-output", "-n", "1"])
self.world_name = world_name
self.timeout_sec = timeout
Expand Down
2 changes: 1 addition & 1 deletion libs/scenario_execution_gazebo/setup.py
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@
],
install_requires=[
'setuptools',
'transforms3d==0.3.1',
'transforms3d==0.4.1',
],
zip_safe=True,
maintainer='Intel Labs',
Expand Down
4 changes: 2 additions & 2 deletions scenario_execution/setup.py
Original file line number Diff line number Diff line change
Expand Up @@ -40,10 +40,10 @@
],
install_requires=[
'setuptools',
'antlr4-python3-runtime==4.7.2',
'antlr4-python3-runtime==4.9.2',
'defusedxml==0.7.1',
'pyyaml==6.0.1',
'py-trees==2.2.3'
'py-trees==2.2.1'
],
zip_safe=True,
include_package_data=True,
Expand Down
2 changes: 1 addition & 1 deletion scenario_execution_ros/setup.py
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@
],
install_requires=[
'setuptools',
'transforms3d==0.3.1',
'transforms3d==0.4.1',
],
zip_safe=True,
maintainer='Intel Labs',
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -55,12 +55,12 @@ def generate_launch_description():
'/model/', camera_name,
'/link/link/sensor/camera/image' +
'@sensor_msgs/msg/Image' +
'[ignition.msgs.Image'],
'[gz.msgs.Image'],
['/world/', world_name,
'/model/', camera_name,
'/link/link/sensor/camera/camera_info' +
'@sensor_msgs/msg/CameraInfo' +
'[ignition.msgs.CameraInfo'],
'[gz.msgs.CameraInfo'],
],
remappings=[
(['/world/', world_name, '/model/', camera_name, '/link/link/sensor/camera/image'],
Expand All @@ -71,7 +71,7 @@ def generate_launch_description():
)

spawn_camera = Node(
package='ros_ign_gazebo',
package='ros_gz_sim',
executable='create',
arguments=['-name', camera_name,
'-x', x,
Expand Down
8 changes: 6 additions & 2 deletions simulation/gazebo/gazebo_tf_publisher/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -22,15 +22,19 @@ find_package(geometry_msgs REQUIRED)
find_package(tf2_msgs REQUIRED)

# gazebo
find_package(ignition-transport11 REQUIRED)
find_package(gz_transport_vendor REQUIRED)
find_package(gz-transport REQUIRED)

add_executable(gazebo_tf_publisher_node src/gazebo_tf_publisher_node.cpp)

ament_target_dependencies(gazebo_tf_publisher_node
rclcpp
geometry_msgs
tf2_msgs
ignition-transport11
)

target_link_libraries(gazebo_tf_publisher_node
gz-transport::core
)

install(DIRECTORY
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,42 +15,42 @@
// SPDX-License-Identifier: Apache-2.0

#include <geometry_msgs/msg/transform_stamped.hpp>
#include <ignition/msgs/pose_v.pb.h>
#include <ignition/transport/Node.hh>
#include <gz/msgs/pose_v.pb.h>
#include <gz/transport/Node.hh>
#include <rclcpp/rclcpp.hpp>
#include <tf2_msgs/msg/tf_message.hpp>

class GazeboTFPublisher : public rclcpp::Node {
std::shared_ptr<ignition::transport::Node> mIgnNode;
std::shared_ptr<gz::transport::Node> mIgnNode;
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr mPublisher;
std::string gz_pose_topic;
std::string base_frame_id;
std::vector<unsigned int> robot_frame_ids;
std::string previous_frame_id;

void IgnCB(const ignition::msgs::Pose_V &poses);
void IgnCB(const gz::msgs::Pose_V &poses);

public:
GazeboTFPublisher();
};

GazeboTFPublisher::GazeboTFPublisher() : Node("gazebo_tf_publisher") {
mPublisher = this->create_publisher<tf2_msgs::msg::TFMessage>("tf", 10);
mIgnNode = std::make_shared<ignition::transport::Node>();
mIgnNode = std::make_shared<gz::transport::Node>();
declare_parameter("gz_pose_topic", "/world/name/dynamic_pose/info");
gz_pose_topic = get_parameter("gz_pose_topic").as_string();
declare_parameter("base_frame_id", "base_link");
base_frame_id = get_parameter("base_frame_id").as_string();
mIgnNode->Subscribe(gz_pose_topic, &GazeboTFPublisher::IgnCB, this);
}

void GazeboTFPublisher::IgnCB(const ignition::msgs::Pose_V &poses) {
void GazeboTFPublisher::IgnCB(const gz::msgs::Pose_V &poses) {
auto tf_msg = std::make_shared<tf2_msgs::msg::TFMessage>();
auto ros2_clock = get_clock()->now();
for (int i = 0; i < poses.pose_size(); i++) {
if (poses.pose(i).name() == base_frame_id) {
// each robot_frame_id is expected to have corresponding
// base_link_id -1 in ignition pose topic.
// base_link_id -1 in gz pose topic.
unsigned int robot_frame_id = poses.pose(i).id() - 1;
if (std::find(robot_frame_ids.begin(), robot_frame_ids.end(),
robot_frame_id) == robot_frame_ids.end()) {
Expand All @@ -68,9 +68,9 @@ void GazeboTFPublisher::IgnCB(const ignition::msgs::Pose_V &poses) {
if (poses.pose(i).id() == robot_frame_id) {
geometry_msgs::msg::TransformStamped tf_frame;
tf_frame.header.stamp = ros2_clock;
const ignition::msgs::Pose *pp = &poses.pose(i);
const ignition::msgs::Vector3d *previous_pv = &pp->position();
const ignition::msgs::Quaternion *previous_pq = &pp->orientation();
const gz::msgs::Pose *pp = &poses.pose(i);
const gz::msgs::Vector3d *previous_pv = &pp->position();
const gz::msgs::Quaternion *previous_pq = &pp->orientation();
tf_frame.header.frame_id = "map";
tf_frame.child_frame_id =
pp->name() + "_" + base_frame_id +
Expand Down
Loading

0 comments on commit 6845cbb

Please sign in to comment.