diff --git a/.ipynb_checkpoints/building_robot-checkpoint.sdf b/.ipynb_checkpoints/building_robot-checkpoint.sdf
new file mode 100644
index 0000000..1d6c887
--- /dev/null
+++ b/.ipynb_checkpoints/building_robot-checkpoint.sdf
@@ -0,0 +1,62 @@
+
+
+
+
+ 0.001
+ 1.0
+
+
+
+
+
+
+
+
+
+ true
+ 0 0 10 0 0 0
+ 0.8 0.8 0.8 1
+ 0.2 0.2 0.2 1
+
+ 1000
+ 0.9
+ 0.01
+ 0.001
+
+ -0.5 0.1 -0.9
+
+
+
+ true
+
+
+
+
+ 0 0 1
+
+
+
+
+
+
+ 0 0 1
+ 100 100
+
+
+
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/README.md b/README.md
index 013b80c..c50dcdc 100644
--- a/README.md
+++ b/README.md
@@ -2,6 +2,10 @@
A ROS2 workspace for controlling differential drive robots in Gazebo simulation.
+## Demo
+
+See video demo of robot navigating maze [here](https://youtu.be/Ss9XE4Z-uH0)
+
## Overview
This workspace contains a ROS2 package (`gazebo_controller`) that provides:
@@ -17,13 +21,17 @@ gazebo_controller_ws/
├── src/
│ └── gazebo_controller/
│ ├── gazebo_controller/ # Python package
+│ │ └── diffdrive_pid.py
│ ├── launch/ # Launch files
│ │ ├── ros_gz_bridge.launch.py
-│ │ └── gazebo_with_bridge.launch.py
+│ │ └── full_simulation.launch.py
│ ├── config/ # Configuration files
│ │ └── gazebo_bridge.yaml
+│ ├── rviz/ # RVIZ settings
+│ │ └── rviz_view.rviz
│ ├── sdf/ # Robot model files
-│ │ └── building_robot.sdf
+│ │ └── maze_world.sdf
+│ │ └── vehicle_blue_model.sdf
│ ├── package.xml # Package dependencies
│ └── setup.py # Package setup
└── README.md
@@ -43,7 +51,7 @@ source install/setup.bash
**Option A: Complete System (Gazebo + Bridge)**
```bash
-ros2 launch gazebo_controller gazebo_with_bridge.launch.py
+ros2 launch gazebo_controller full_simulation.launch.py
```
**Option B: Bridge Only (if Gazebo already running)**
@@ -75,11 +83,7 @@ ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "linear: {x: 0.0, y: 0.0, z: 0.0
## Robot Model
-The `building_robot.sdf` contains a unique **Pizza Delivery Bot** design featuring:
-- **Red chassis** with yellow delivery compartment
-- **Differential drive** with realistic tire/rim wheels
-- **Dual front support wheels** for stability
-- **Communication antenna** for autonomous operation
+The `vehicle_blue_model.sdf` contains a differential drive robot designed for navigation and control testing within the Gazebo simulation environment. The robot features a blue rectangular chassis with two primary drive wheels and a rear caster for stability.
## Available Topics
@@ -88,6 +92,7 @@ The `building_robot.sdf` contains a unique **Pizza Delivery Bot** design featuri
| `/cmd_vel` | `geometry_msgs/msg/Twist` | ROS → Gazebo | Velocity commands |
| `/odom` | `nav_msgs/msg/Odometry` | Gazebo → ROS | Robot odometry |
| `/tf` | `tf2_msgs/msg/TFMessage` | Gazebo → ROS | Transform data |
+| `/tf_static` | `tf2_msgs/msg/TFMessage` | Gazebo → ROS | Transform data (static version to help RVIZ find all links) |
| `/clock` | `rosgraph_msgs/msg/Clock` | Gazebo → ROS | Simulation time |
## Dependencies
@@ -118,6 +123,6 @@ The `building_robot.sdf` contains a unique **Pizza Delivery Bot** design featuri
---
-**Course**: EN613
-**Author**: [Your Name]
+**Course**: EN613
+**Authors**: Xiaolei Hu, Alyssa Roman, James Davis, Beth Dittberner
**Date**: October 2025
diff --git a/Team_members_and_challenges.pdf b/Team_members_and_challenges.pdf
new file mode 100644
index 0000000..d0e5e11
Binary files /dev/null and b/Team_members_and_challenges.pdf differ
diff --git a/src/gazebo_controller/building_robot copy.sdf b/src/gazebo_controller/building_robot copy.sdf
deleted file mode 100644
index 89cb6bb..0000000
--- a/src/gazebo_controller/building_robot copy.sdf
+++ /dev/null
@@ -1,354 +0,0 @@
-
-
-
-
- 0.001
- 1.0
-
-
-
-
-
-
-
-
-
- true
- 0 0 10 0 0 0
- 0.8 0.8 0.8 1
- 0.2 0.2 0.2 1
-
- 1000
- 0.9
- 0.01
- 0.001
-
- -0.5 0.1 -0.9
-
-
-
- true
-
-
-
-
- 0 0 1
-
-
-
-
-
-
- 0 0 1
- 100 100
-
-
-
- 0.8 0.8 0.8 1
- 0.8 0.8 0.8 1
- 0.8 0.8 0.8 1
-
-
-
-
-
- 0 0 0 0 0 0
-
- 0.5 0 0.3 0 0 0
-
- 2.5
-
- 0.2
- 0
- 0
- 0.8
- 0
- 1.0
-
-
-
-
-
- 1.5 0.8 0.4
-
-
-
- 0.9 0.1 0.1 1
- 0.9 0.1 0.1 1
- 0.5 0.5 0.5 1
-
-
-
- 0 0 0.35 0 0 0
-
-
- 1.0 0.6 0.3
-
-
-
- 1.0 1.0 0.0 1
- 1.0 1.0 0.0 1
- 0.8 0.8 0.8 1
-
-
-
- -0.3 0 0.6 0 0 0
-
-
- 0.02
- 0.4
-
-
-
- 0.0 0.0 0.0 1
- 0.0 0.0 0.0 1
- 0.2 0.2 0.2 1
-
-
-
- -0.3 0 0.8 0 0 0
-
-
- 0.05
-
-
-
- 1.0 0.0 1.0 1
- 1.0 0.0 1.0 1
- 1.0 0.5 1.0 1
-
-
-
-
-
- 1.5 0.8 0.7
-
-
-
-
-
- -0.4 0.5 0 -1.5707 0 0
-
- 1.5
-
- 0.065
- 0
- 0
- 0.065
- 0
- 0.12
-
-
-
-
-
- 0.35
- 0.25
-
-
-
- 0.2 0.2 0.2 1
- 0.2 0.2 0.2 1
- 0.1 0.1 0.1 1
-
-
-
- 0 0 0.05 0 0 0
-
-
- 0.25
- 0.15
-
-
-
- 0.7 0.7 0.7 1
- 0.7 0.7 0.7 1
- 0.9 0.9 0.9 1
-
-
-
-
-
- 0.35
- 0.25
-
-
-
-
-
- -0.4 -0.5 0 -1.5707 0 0
-
- 1.5
-
- 0.065
- 0
- 0
- 0.065
- 0
- 0.12
-
-
-
-
-
- 0.35
- 0.25
-
-
-
- 0.2 0.2 0.2 1
- 0.2 0.2 0.2 1
- 0.1 0.1 0.1 1
-
-
-
- 0 0 0.05 0 0 0
-
-
- 0.25
- 0.15
-
-
-
- 0.7 0.7 0.7 1
- 0.7 0.7 0.7 1
- 0.9 0.9 0.9 1
-
-
-
-
-
- 0.35
- 0.25
-
-
-
-
-
- 0.6 0 -0.15 0 0 0
-
-
- 0 0.2 0 1.5707 0 0
-
- 0.5
-
- 0.008
- 0
- 0
- 0.008
- 0
- 0.008
-
-
-
-
-
- 0.15
- 0.1
-
-
-
- 0.1 0.1 0.8 1
- 0.1 0.1 0.8 1
- 0.5 0.5 1.0 1
-
-
-
-
-
- 0.15
- 0.1
-
-
-
-
-
- 0 -0.2 0 1.5707 0 0
-
- 0.5
-
- 0.008
- 0
- 0
- 0.008
- 0
- 0.008
-
-
-
-
-
- 0.15
- 0.1
-
-
-
- 0.1 0.1 0.8 1
- 0.1 0.1 0.8 1
- 0.5 0.5 1.0 1
-
-
-
-
-
- 0.15
- 0.1
-
-
-
-
-
-
- chassis
- left_wheel
-
- 0 1 0
-
- -1.79769e+308
- 1.79769e+308
-
-
-
-
-
- chassis
- right_wheel
-
- 0 1 0
-
- -1.79769e+308
- 1.79769e+308
-
-
-
-
- chassis
- front_left_support
-
- 0 1 0
-
- -1.79769e+308
- 1.79769e+308
-
-
-
-
- chassis
- front_right_support
-
- 0 1 0
-
- -1.79769e+308
- 1.79769e+308
-
-
-
-
-
-
\ No newline at end of file
diff --git a/src/gazebo_controller/config/gazebo_bridge.yaml b/src/gazebo_controller/config/gazebo_bridge.yaml
index afcfecd..c7a359b 100644
--- a/src/gazebo_controller/config/gazebo_bridge.yaml
+++ b/src/gazebo_controller/config/gazebo_bridge.yaml
@@ -1,31 +1,43 @@
# Bridge configuration for ROS2 and Gazebo
topics:
- # Velocity command topic from ROS to Gazebo
- - ros_topic_name: "cmd_vel"
- gz_topic_name: "/cmd_vel"
- ros_type_name: "geometry_msgs/msg/Twist"
- gz_type_name: "gz.msgs.Twist"
- direction: "ROS_TO_GZ"
+ # Velocity command topic from ROS to Gazebo
+ - ros_topic_name: "cmd_vel"
+ gz_topic_name: "/cmd_vel"
+ ros_type_name: "geometry_msgs/msg/Twist"
+ gz_type_name: "gz.msgs.Twist"
+ direction: "ROS_TO_GZ"
- # Odometry topic from Gazebo to ROS
- - ros_topic_name: "odom"
- gz_topic_name: "/odom"
- ros_type_name: "nav_msgs/msg/Odometry"
- gz_type_name: "gz.msgs.Odometry"
- direction: "GZ_TO_ROS"
+ # Odometry topic from Gazebo to ROS
+ - ros_topic_name: "odom"
+ gz_topic_name: "/odom"
+ ros_type_name: "nav_msgs/msg/Odometry"
+ gz_type_name: "gz.msgs.Odometry"
+ direction: "GZ_TO_ROS"
- # TF topic from Gazebo to ROS
- - ros_topic_name: "tf"
- gz_topic_name: "/tf"
- ros_type_name: "tf2_msgs/msg/TFMessage"
- gz_type_name: "gz.msgs.Pose_V"
- direction: "GZ_TO_ROS"
+ # TF topic from Gazebo to ROS
+ - ros_topic_name: "tf"
+ gz_topic_name: "/tf"
+ ros_type_name: "tf2_msgs/msg/TFMessage"
+ gz_type_name: "gz.msgs.Pose_V"
+ direction: "GZ_TO_ROS"
+ qos:
+ reliability: "RELIABLE"
+ history: "KEEP_LAST"
+ depth: 10
- # Clock topic from Gazebo to ROS
- - 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"
-
\ No newline at end of file
+ - ros_topic_name: "tf_static"
+ gz_topic_name: "/tf_static"
+ ros_type_name: "tf2_msgs/msg/TFMessage"
+ gz_type_name: "gz.msgs.Pose_V"
+ direction: "GZ_TO_ROS"
+ qos:
+ durability: "TRANSIENT_LOCAL"
+
+ # Clock topic from Gazebo to ROS
+ - 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"
+
\ No newline at end of file
diff --git a/src/gazebo_controller/gazebo_controller/diffdrive_pid.py b/src/gazebo_controller/gazebo_controller/diffdrive_pid.py
new file mode 100644
index 0000000..721ba4d
--- /dev/null
+++ b/src/gazebo_controller/gazebo_controller/diffdrive_pid.py
@@ -0,0 +1,161 @@
+#!/usr/bin/env python3
+import math
+from typing import Optional, Tuple
+
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import PoseStamped, Twist
+from tf2_ros import Buffer, TransformListener
+
+# ROS 2 Jazzy compatible quaternion to euler conversion
+def euler_from_quaternion(quaternion):
+ """
+ Convert quaternion (x, y, z, w) to euler angles (roll, pitch, yaw)
+ """
+ x, y, z, w = quaternion
+
+ # Roll (x-axis rotation)
+ sinr_cosp = 2 * (w * x + y * z)
+ cosr_cosp = 1 - 2 * (x * x + y * y)
+ roll = math.atan2(sinr_cosp, cosr_cosp)
+
+ # Pitch (y-axis rotation)
+ sinp = 2 * (w * y - z * x)
+ if abs(sinp) >= 1:
+ pitch = math.copysign(math.pi / 2, sinp)
+ else:
+ pitch = math.asin(sinp)
+
+ # Yaw (z-axis rotation)
+ siny_cosp = 2 * (w * z + x * y)
+ cosy_cosp = 1 - 2 * (y * y + z * z)
+ yaw = math.atan2(siny_cosp, cosy_cosp)
+
+ return (roll, pitch, yaw)
+
+class DiffDrivePID(Node):
+ def __init__(self):
+ super().__init__('diffdrive_pid')
+
+ # PD gains (no I)
+ self.declare_parameter('kp', 1.5)
+ self.declare_parameter('kd', 0.8)
+ self.declare_parameter('lookahead', 0.3) # trailer-hitch offset (m)
+ self.declare_parameter('publish_rate', 30.0)
+
+ self.kp = float(self.get_parameter('kp').get_parameter_value().double_value)
+ self.kd = float(self.get_parameter('kd').get_parameter_value().double_value)
+ self.lh = float(self.get_parameter('lookahead').get_parameter_value().double_value)
+ self.publish_rate = float(self.get_parameter('publish_rate').get_parameter_value().double_value)
+ self.dt = 1.0 / self.publish_rate
+
+ # TF
+ self.tf_buffer = Buffer()
+ self.tf_listener = TransformListener(self.tf_buffer, self)
+
+ # I/O
+ self.goal_sub = self.create_subscription(PoseStamped, '/goal_pose', self.goal_cb, 10)
+ self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)
+
+ # Internal state
+ self.goal: Optional[Tuple[float, float]] = None # (xg, yg) in odom
+ self.prev_pose = None # (x, y, yaw)
+ self.prev_pose_time = None
+
+ # Diagnostic variables for tuning
+ self.error_history = []
+ self.max_error_history = 100
+ self.diagnostic_counter = 0
+
+ # Timer loop
+ self.timer = self.create_timer(self.dt, self.on_timer)
+
+ self.get_logger().info(f'DiffDrivePID started: kp={self.kp}, kd={self.kd}, lookahead={self.lh}, rate={self.publish_rate} Hz')
+
+ def goal_cb(self, msg: PoseStamped):
+ # Take XY from PoseStamped (odom frame recommended in RViz config)
+ self.goal = (float(msg.pose.position.x), float(msg.pose.position.y))
+ self.get_logger().info(f'Goal set to: {self.goal}')
+
+ def on_timer(self):
+ # Need current robot pose (odom -> base_link)
+ try:
+ tf = self.tf_buffer.lookup_transform('odom', 'base_link', rclpy.time.Time())
+ except Exception:
+ return
+
+ x = tf.transform.translation.x
+ y = tf.transform.translation.y
+ q = tf.transform.rotation
+ yaw = euler_from_quaternion([q.x, q.y, q.z, q.w])[2]
+
+ # Velocity estimate from finite difference (optional damping term)
+ vx = 0.0
+ vy = 0.0
+ now = self.get_clock().now()
+ if self.prev_pose is not None and self.prev_pose_time is not None:
+ dt = (now - self.prev_pose_time).nanoseconds * 1e-9
+ if dt > 1e-6:
+ vx = (x - self.prev_pose[0]) / dt
+ vy = (y - self.prev_pose[1]) / dt
+
+ self.prev_pose = (x, y, yaw)
+ self.prev_pose_time = now
+
+ if self.goal is None:
+ # No goal -> command zero
+ self.cmd_pub.publish(Twist())
+ return
+
+ xg, yg = self.goal
+
+ # Virtual "trailer hitch" point ahead of robot by lookahead distance (in odom frame)
+ xp = x + self.lh * math.cos(yaw)
+ yp = y + self.lh * math.sin(yaw)
+
+ # PD in world frame for the point
+ ex = xg - xp
+ ey = yg - yp
+
+ # Desired world-frame point velocity
+ vpx = self.kp * ex - self.kd * vx
+ vpy = self.kp * ey - self.kd * vy
+
+ # Diagnostic logging for tuning (every 30 cycles = 1 second at 30Hz)
+ error_magnitude = math.sqrt(ex*ex + ey*ey)
+ self.error_history.append(error_magnitude)
+ if len(self.error_history) > self.max_error_history:
+ self.error_history.pop(0)
+
+ self.diagnostic_counter += 1
+ if self.diagnostic_counter >= 30: # Log every second
+ avg_error = sum(self.error_history) / len(self.error_history)
+ self.get_logger().info(f'Avg error: {avg_error:.3f}m, Current: {error_magnitude:.3f}m, P: {self.kp*error_magnitude:.3f}, D: {self.kd*math.sqrt(vx*vx+vy*vy):.3f}')
+ self.diagnostic_counter = 0
+
+ # Convert (vpx, vpy) to robot (v, w) using inverse of:
+ # [xp_dot] = [ cos -sin] [v]
+ # [yp_dot] [ sin cos] [l*w]
+ # => [v, l*w] = R(-yaw) @ [vpx, vpy]
+ c = math.cos(yaw)
+ s = math.sin(yaw)
+ v = c * vpx + s * vpy
+ lw = -s * vpx + c * vpy
+ w = lw / self.lh if abs(self.lh) > 1e-9 else 0.0
+
+ # Publish
+ cmd = Twist()
+ cmd.linear.x = float(v)
+ cmd.angular.z = float(w)
+ self.cmd_pub.publish(cmd)
+
+def main():
+ rclpy.init()
+ node = DiffDrivePID()
+ try:
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ pass
+ node.destroy_node()
+ rclpy.shutdown()
diff --git a/src/gazebo_controller/launch/full_simulation.launch.py b/src/gazebo_controller/launch/full_simulation.launch.py
new file mode 100644
index 0000000..ab00fdc
--- /dev/null
+++ b/src/gazebo_controller/launch/full_simulation.launch.py
@@ -0,0 +1,85 @@
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+def generate_launch_description():
+ pkg_share = get_package_share_directory('gazebo_controller')
+ world_sdf_file = os.path.join(pkg_share, 'sdf', 'maze_world.sdf')
+ robot_sdf_file = os.path.join(pkg_share, 'sdf', 'vehicle_blue_model.sdf')
+ bridge_launch = os.path.join(pkg_share, 'launch', 'ros_gz_bridge.launch.py')
+ rviz_config = os.path.join(pkg_share, 'rviz', 'rviz_view.rviz')
+
+ # Start Gazebo with the robot world
+ gazebo_cmd = ExecuteProcess(
+ cmd=['gz', 'sim', '-r', world_sdf_file],
+ output='screen',
+ name='gazebo_sim'
+ )
+
+ # Include the bridge launch file
+ bridge_launch_include = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(bridge_launch)
+ )
+
+ # Start the PID controller node
+ pid_controller = Node(
+ package='gazebo_controller',
+ executable='diffdrive_pid',
+ name='diffdrive_pid',
+ output='screen',
+ parameters=[
+ {'kp': 1.5},
+ {'kd': 0.8},
+ {'lookahead': 0.3},
+ {'publish_rate': 30.0}
+ ]
+ )
+
+ # Start RViz with our custom config
+ rviz = Node(
+ package='rviz2',
+ executable='rviz2',
+ name='rviz2',
+ output='screen',
+ arguments=['-d', rviz_config]
+ )
+
+ # Read the robot model SDF file for robot_state_publisher
+ # This is a separate SDF containing only the robot model (no world, no plugins)
+ with open(robot_sdf_file, 'r', encoding='utf-8') as f:
+ robot_desc = f.read()
+
+ # Robot State Publisher - publishes robot_description and TF transforms
+ # It will use joint states from Gazebo to compute link transforms
+ robot_state_publisher = Node(
+ package='robot_state_publisher',
+ executable='robot_state_publisher',
+ name='robot_state_publisher',
+ output='screen',
+ parameters=[{
+ 'robot_description': robot_desc,
+ 'use_sim_time': True
+ }]
+ )
+
+ # Static transform from map to odom (for navigation stack)
+ # This creates the TF hierarchy: map -> odom -> base_link
+ # Gazebo DiffDrive plugin publishes odom->base_link, this completes the chain with map->odom
+ map_to_odom_tf = Node(
+ package='tf2_ros',
+ executable='static_transform_publisher',
+ name='map_to_odom_broadcaster',
+ arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']
+ )
+
+ return LaunchDescription([
+ gazebo_cmd,
+ bridge_launch_include,
+ robot_state_publisher,
+ pid_controller,
+ map_to_odom_tf,
+ rviz
+ ])
diff --git a/src/gazebo_controller/launch/gazebo_with_bridge.launch.py b/src/gazebo_controller/launch/gazebo_with_bridge.launch.py
deleted file mode 100644
index 43958f5..0000000
--- a/src/gazebo_controller/launch/gazebo_with_bridge.launch.py
+++ /dev/null
@@ -1,28 +0,0 @@
-from launch import LaunchDescription
-from launch.actions import ExecuteProcess, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch_ros.actions import Node
-from ament_index_python.packages import get_package_share_directory
-import os
-
-def generate_launch_description():
- pkg_share = get_package_share_directory('gazebo_controller')
- sdf_file = os.path.join(pkg_share, 'sdf', 'building_robot.sdf')
- bridge_launch = os.path.join(pkg_share, 'launch', 'ros_gz_bridge.launch.py')
-
- # Start Gazebo with the robot world
- gazebo_cmd = ExecuteProcess(
- cmd=['gz', 'sim', sdf_file],
- output='screen',
- name='gazebo_sim'
- )
-
- # Include the bridge launch file
- bridge_launch_include = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(bridge_launch)
- )
-
- return LaunchDescription([
- gazebo_cmd,
- bridge_launch_include
- ])
diff --git a/src/gazebo_controller/launch/ros_gz_bridge.launch.py b/src/gazebo_controller/launch/ros_gz_bridge.launch.py
index fbe8b80..596bec2 100644
--- a/src/gazebo_controller/launch/ros_gz_bridge.launch.py
+++ b/src/gazebo_controller/launch/ros_gz_bridge.launch.py
@@ -4,15 +4,23 @@
import os
def generate_launch_description():
- pkg_share = get_package_share_directory('gazebo_controller')
- bridge_yaml = os.path.join(pkg_share, 'config', 'gazebo_bridge.yaml')
+ pkg_share = get_package_share_directory('gazebo_controller')
+ bridge_yaml = os.path.join(pkg_share, 'config', 'gazebo_bridge.yaml')
- bridge = Node(
- package='ros_gz_bridge',
- executable='parameter_bridge',
- name='ros_gz_bridge',
- output='screen',
- parameters=[bridge_yaml]
- )
+ bridge = Node(
+ package='ros_gz_bridge',
+ executable='parameter_bridge',
+ name='ros_gz_bridge',
+ output='screen',
+ arguments=[
+ '/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist',
+ '/odom@nav_msgs/msg/Odometry@gz.msgs.Odometry',
+ '/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V',
+ '/tf_static@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V',
+ '/joint_states@sensor_msgs/msg/JointState@gz.msgs.Model',
+ '/clock@rosgraph_msgs/msg/Clock@gz.msgs.Clock'],
+ remappings=[
+ ('/tf_static', '/tf_static'),
+ ])
- return LaunchDescription([bridge])
+ return LaunchDescription([bridge])
diff --git a/src/gazebo_controller/package.xml b/src/gazebo_controller/package.xml
index 9b06d77..9771f95 100644
--- a/src/gazebo_controller/package.xml
+++ b/src/gazebo_controller/package.xml
@@ -1,28 +1,34 @@
- gazebo_controller
- 0.0.0
- PID + Gazebo bridge controller for differential drive maze robot
- ubuntu
- MIT
+ gazebo_controller
+ 0.0.0
+ PID + Gazebo bridge controller for differential drive maze robot
+ ubuntu
+ MIT
- rclpy
- geometry_msgs
- nav_msgs
- std_msgs
- tf2_ros
- ros_gz_bridge
- gazebo_msgs
- launch
- launch_ros
+ rclpy
+ geometry_msgs
+ nav_msgs
+ std_msgs
+ tf2_ros
+ tf2_geometry_msgs
+ tf2_py
+ ros_gz_bridge
+ gazebo_msgs
+ launch
+ launch_ros
+ rviz2
+ robot_state_publisher
+ joint_state_publisher
+ sdformat_urdf
- ament_copyright
- ament_flake8
- ament_pep257
- python3-pytest
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
-
- ament_python
-
+
+ ament_python
+
diff --git a/src/gazebo_controller/rviz/rviz_view.rviz b/src/gazebo_controller/rviz/rviz_view.rviz
new file mode 100644
index 0000000..62181c9
--- /dev/null
+++ b/src/gazebo_controller/rviz/rviz_view.rviz
@@ -0,0 +1,217 @@
+Panels:
+ - Class: rviz_common/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /TF1
+ - /RobotModel1
+ - /RobotModel1/Description Topic1
+ Splitter Ratio: 0.5
+ Tree Height: 359
+ - Class: rviz_common/Selection
+ Name: Selection
+ - Class: rviz_common/Tool Properties
+ Expanded:
+ - /2D Goal Pose1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz_common/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz_common/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz_default_plugins/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Class: rviz_default_plugins/TF
+ Enabled: true
+ Filter (blacklist): ""
+ Filter (whitelist): ""
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: false
+ Tree:
+ {}
+ Update Interval: 0
+ Value: true
+ - Alpha: 1
+ Class: rviz_default_plugins/RobotModel
+ Collision Enabled: false
+ Description File: ""
+ Description Source: Topic
+ Description Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: robot_description
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: ""
+ Mass Properties:
+ Inertia: false
+ Mass: false
+ Name: RobotModel
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Angle Tolerance: 0.10000000149011612
+ Class: rviz_default_plugins/Odometry
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: false
+ Enabled: true
+ Keep: 100
+ Name: Odometry
+ Position Tolerance: 0.10000000149011612
+ Shape:
+ Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Color: 255; 25; 0
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Value: Arrow
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /odom
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: odom
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz_default_plugins/Interact
+ Hide Inactive Objects: true
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/Select
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Line color: 128; 128; 0
+ - Class: rviz_default_plugins/SetInitialPose
+ Covariance x: 0.25
+ Covariance y: 0.25
+ Covariance yaw: 0.06853891909122467
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /initialpose
+ - Class: rviz_default_plugins/SetGoal
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /goal_pose
+ - Class: rviz_default_plugins/PublishPoint
+ Single click: true
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /clicked_point
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
+ Value: true
+ Views:
+ Current:
+ Class: rviz_default_plugins/Orbit
+ Distance: 10
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.785398006439209
+ Target Frame:
+ Value: Orbit (rviz)
+ Yaw: 0.785398006439209
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 656
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd0000000400000000000001c4000001f2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001f2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001f2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000001f2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000026f00fffffffb0000000800540069006d00650100000000000004500000000000000000000001d1000001f200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1200
+ X: 60
+ Y: 28
diff --git a/src/gazebo_controller/sdf/building_robot.sdf b/src/gazebo_controller/sdf/building_robot.sdf
deleted file mode 100644
index ce31bcb..0000000
--- a/src/gazebo_controller/sdf/building_robot.sdf
+++ /dev/null
@@ -1,281 +0,0 @@
-
-
-
-
- 0.001
- 1.0
-
-
-
-
-
-
-
-
- true
- 0 0 10 0 0 0
- 0.8 0.8 0.8 1
- 0.2 0.2 0.2 1
-
- 1000
- 0.9
- 0.01
- 0.001
-
- -0.5 0.1 -0.9
-
-
-
- true
-
-
-
-
- 0 0 1
-
-
-
-
-
-
- 0 0 1
- 100 100
-
-
-
- 0.8 0.8 0.8 1
- 0.8 0.8 0.8 1
- 0.8 0.8 0.8 1
-
-
-
-
-
-
- left_wheel_joint
- right_wheel_joint
- 1.2
- 0.4
- 1
- /cmd_vel
-
- 0 0 0 0 0 0
-
- 0.5 0 0.4 0 0 0
-
- 1.14395
-
- 0.095329
- 0
- 0
- 0.381317
- 0
- 0.476646
-
-
-
-
-
- 2.0 1.0 0.5
-
-
-
- 0.0 0.0 1.0 1
- 0.0 0.0 1.0 1
- 0.0 0.0 1.0 1
-
-
-
-
-
- 2.0 1.0 0.5
-
-
-
-
-
- -0.5 0.6 0 -1.5707 0 0
-
- 1
-
- 0.043333
- 0
- 0
- 0.043333
- 0
- 0.08
-
-
-
-
-
- 0.4
- 0.2
-
-
-
- 1.0 0.0 0.0 1
- 1.0 0.0 0.0 1
- 1.0 0.0 0.0 1
-
-
-
-
-
- 0.4
- 0.2
-
-
-
-
-
- -0.5 -0.6 0 -1.5707 0 0
-
- 1
-
- 0.043333
- 0
- 0
- 0.043333
- 0
- 0.08
-
-
-
-
-
- 0.4
- 0.2
-
-
-
- 1.0 0.0 0.0 1
- 1.0 0.0 0.0 1
- 1.0 0.0 0.0 1
-
-
-
-
-
- 0.4
- 0.2
-
-
-
-
-
- 0.8 0 -0.2 0 0 0
-
-
-
-
- 1
-
- 0.016
- 0
- 0
- 0.016
- 0
- 0.016
-
-
-
-
-
- 0.2
-
-
-
- 0.0 1 0.0 1
- 0.0 1 0.0 1
- 0.0 1 0.0 1
-
-
-
-
-
- 0.2
-
-
-
-
-
-
- chassis
- left_wheel
-
- 0 1 0
-
- -1.79769e+308
- 1.79769e+308
-
-
-
-
-
- chassis
- right_wheel
-
- 0 1 0
-
- -1.79769e+308
- 1.79769e+308
-
-
-
-
- chassis
- caster
-
-
-
-
-
-
- 16777234
-
-
-
-
-
-
- 16777235
-
-
-
-
-
-
- 16777236
-
-
-
-
-
-
- 16777237
-
-
-
-
-
\ No newline at end of file
diff --git a/src/gazebo_controller/sdf/maze_world.sdf b/src/gazebo_controller/sdf/maze_world.sdf
new file mode 100644
index 0000000..ccd70d0
--- /dev/null
+++ b/src/gazebo_controller/sdf/maze_world.sdf
@@ -0,0 +1,242 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0 0 10 0 0 0
+ 0.8 0.8 0.8 1
+ 0.2 0.2 0.2 1
+ -0.5 0.1 -0.9
+
+
+
+
+ true
+
+
+
+ 0 0 1
+
+
+
+
+
+ 0 0 1
+ 60 60
+
+
+
+ 0.7 0.7 0.7 1
+
+
+
+
+
+
+
+
+ true
+ -4.5 7.5 0.5 0 0 0
+
+
+
+ 6 0.2 1
+
+
+
+
+ 6 0.2 1
+
+ 0.6 0.6 0.6 1
+
+
+
+
+
+
+ true
+ 4.5 7.5 0.5 0 0 0
+
+
+
+ 6 0.2 1
+
+
+
+
+ 6 0.2 1
+
+ 0.6 0.6 0.6 1
+
+
+
+
+
+
+ true
+ -4.5 -7.5 0.5 0 0 0
+
+
+
+ 6 0.2 1
+
+
+
+
+ 6 0.2 1
+
+ 0.6 0.6 0.6 1
+
+
+
+
+
+
+ true
+ 4.5 -7.5 0.5 0 0 0
+
+
+
+ 6 0.2 1
+
+
+
+
+ 6 0.2 1
+
+ 0.6 0.6 0.6 1
+
+
+
+
+
+
+ true
+ -7.5 0 0.5 0 0 0
+
+
+
+ 0.2 15 1
+
+
+
+
+ 0.2 15 1
+
+ 0.6 0.6 0.6 1
+
+
+
+
+
+
+ true
+ 7.5 0 0.5 0 0 0
+
+
+
+ 0.2 15 1
+
+
+
+
+ 0.2 15 1
+
+ 0.6 0.6 0.6 1
+
+
+
+
+
+
+ true
+ 0 0 0.5 0 0 0
+
+
+
+ 9 0.2 1
+
+
+
+
+ 9 0.2 1
+
+ 0.6 0.6 0.6 1
+
+
+
+
+
+
+ true
+ 0 3.75 0.5 0 0 0
+
+
+
+ 4.5 0.2 1
+
+
+
+
+ 4.5 0.2 1
+
+ 0.6 0.6 0.6 1
+
+
+
+
+
+
+
+ file:///home/ubuntu/EN613/gazebo_controller_ws/install/gazebo_controller/share/gazebo_controller/sdf/vehicle_blue_model.sdf
+ vehicle_blue
+ 0 -9 0.4 0 0 1.57
+
+
+ left_wheel_joint
+ right_wheel_joint
+ 1.2
+ 0.4
+ 30
+ /cmd_vel
+ /odom
+ odom
+ base_link
+ /tf
+ true
+
+
+
+ /joint_states
+
+
+
+ true
+ false
+ false
+ false
+ false
+ true
+ true
+ 1
+
+
+
+
+
\ No newline at end of file
diff --git a/src/gazebo_controller/sdf/vehicle_blue_model.sdf b/src/gazebo_controller/sdf/vehicle_blue_model.sdf
new file mode 100644
index 0000000..57d3e70
--- /dev/null
+++ b/src/gazebo_controller/sdf/vehicle_blue_model.sdf
@@ -0,0 +1,172 @@
+
+
+
+
+ 0.5 0 0.4 0 0 0
+
+ 1.14395
+
+ 0.095329
+ 0
+ 0
+ 0.381317
+ 0
+ 0.476646
+
+
+
+
+
+ 2.0 1.0 0.5
+
+
+
+ 0.0 0.0 1.0 1
+ 0.0 0.0 1.0 1
+ 0.0 0.0 1.0 1
+
+
+
+
+
+ 2.0 1.0 0.5
+
+
+
+
+
+ -0.5 0.6 0 -1.5707 0 0
+
+ 1
+
+ 0.043333
+ 0
+ 0
+ 0.043333
+ 0
+ 0.08
+
+
+
+
+
+ 0.4
+ 0.2
+
+
+
+ 1.0 0.0 0.0 1
+ 1.0 0.0 0.0 1
+ 1.0 0.0 0.0 1
+
+
+
+
+
+ 0.4
+ 0.2
+
+
+
+
+
+ -0.5 -0.6 0 -1.5707 0 0
+
+ 1
+
+ 0.043333
+ 0
+ 0
+ 0.043333
+ 0
+ 0.08
+
+
+
+
+
+ 0.4
+ 0.2
+
+
+
+ 1.0 0.0 0.0 1
+ 1.0 0.0 0.0 1
+ 1.0 0.0 0.0 1
+
+
+
+
+
+ 0.4
+ 0.2
+
+
+
+
+
+ 0.8 0 -0.2 0 0 0
+
+
+
+
+ 1
+
+ 0.016
+ 0
+ 0
+ 0.016
+ 0
+ 0.016
+
+
+
+
+
+ 0.2
+
+
+
+ 0.0 1 0.0 1
+ 0.0 1 0.0 1
+ 0.0 1 0.0 1
+
+
+
+
+
+ 0.2
+
+
+
+
+
+
+ base_link
+ left_wheel
+
+ 0 1 0
+
+ -1.79769e+308
+ 1.79769e+308
+
+
+
+
+
+ base_link
+ right_wheel
+
+ 0 1 0
+
+ -1.79769e+308
+ 1.79769e+308
+
+
+
+
+ base_link
+ caster
+
+
+
diff --git a/src/gazebo_controller/setup.py b/src/gazebo_controller/setup.py
index dc1ca53..6f57f2b 100644
--- a/src/gazebo_controller/setup.py
+++ b/src/gazebo_controller/setup.py
@@ -4,26 +4,28 @@
package_name = 'gazebo_controller'
setup(
- name=package_name,
- version='0.0.0',
- packages=find_packages(exclude=['test']),
- data_files=[
- ('share/ament_index/resource_index/packages',
- ['resource/' + package_name]),
- ('share/' + package_name, ['package.xml']),
- ('share/' + package_name + '/launch', glob('launch/*.py')),
- ('share/' + package_name + '/config', glob('config/*.yaml')),
- ('share/' + package_name + '/sdf', glob('*.sdf')),
- ],
- install_requires=['setuptools'],
- zip_safe=True,
- maintainer='ubuntu',
- maintainer_email='ubuntu@todo.todo',
- description='PID + Gazebo bridge controller for differential drive maze robot',
- license='MIT',
- tests_require=['pytest'],
- entry_points={
- 'console_scripts': [
- ],
- },
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(exclude=['test']),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ('share/' + package_name + '/launch', glob('launch/*.py')),
+ ('share/' + package_name + '/config', glob('config/*.yaml')),
+ ('share/' + package_name + '/sdf', glob('sdf/*.sdf')),
+ ('share/' + package_name + '/rviz', glob('rviz/*.rviz')),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='ubuntu',
+ maintainer_email='ubuntu@todo.todo',
+ description='PID + Gazebo bridge controller for differential drive maze robot',
+ license='MIT',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'diffdrive_pid = gazebo_controller.diffdrive_pid:main',
+ ],
+ },
)
diff --git a/src/gazebo_controller/test/test_copyright.py b/src/gazebo_controller/test/test_copyright.py
index 97a3919..8b84971 100644
--- a/src/gazebo_controller/test/test_copyright.py
+++ b/src/gazebo_controller/test/test_copyright.py
@@ -21,5 +21,5 @@
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found errors'
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/src/gazebo_controller/test/test_flake8.py b/src/gazebo_controller/test/test_flake8.py
index 27ee107..a0618f6 100644
--- a/src/gazebo_controller/test/test_flake8.py
+++ b/src/gazebo_controller/test/test_flake8.py
@@ -19,7 +19,7 @@
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
- rc, errors = main_with_errors(argv=[])
- assert rc == 0, \
- 'Found %d code style errors / warnings:\n' % len(errors) + \
- '\n'.join(errors)
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/src/gazebo_controller/test/test_pep257.py b/src/gazebo_controller/test/test_pep257.py
index b234a38..81134e9 100644
--- a/src/gazebo_controller/test/test_pep257.py
+++ b/src/gazebo_controller/test/test_pep257.py
@@ -19,5 +19,5 @@
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found code style errors / warnings'
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'