diff --git a/CMakeLists.txt b/CMakeLists.txt index 81badea..a5ac908 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,12 +38,13 @@ ament_python_install_package(${PROJECT_NAME}) install(PROGRAMS scripts/landmark_detector.py scripts/visualizer.py + scripts/core_filter.py DESTINATION lib/${PROJECT_NAME} ) -# Install launch files. +# Install launch, rviz and config directories install(DIRECTORY - launch + launch config rviz DESTINATION share/${PROJECT_NAME}/ ) diff --git a/config/landmark_ground_truth.yaml b/config/landmark_ground_truth.yaml new file mode 100644 index 0000000..192c356 --- /dev/null +++ b/config/landmark_ground_truth.yaml @@ -0,0 +1,3 @@ +core_filter: + landmark_ground_truth: + my_global_param: "Test" \ No newline at end of file diff --git a/launch/offboard.launch.py b/launch/offboard.launch.py new file mode 100644 index 0000000..a2b42e0 --- /dev/null +++ b/launch/offboard.launch.py @@ -0,0 +1,53 @@ +import os +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + +from launch_ros.substitutions import FindPackageShare +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution, TextSubstitution + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('particle_filter'), + 'config', + 'landmark_ground_truth.yaml' + ) + + pf_landmark_detector = Node( + package='particle_filter', + executable='landmark_detector.py', + name='landmark_detector' + ) + visualizer = Node( + package='particle_filter', + executable='visualizer.py', + name='visualizer' + ) + tb3_teleop = Node( + package='turtlebot3_teleop', + executable='teleop_keyboard', + name = 'tb3_teleop_keyboard', + output='screen' + # remappings=[ + # ('/cmd_vel', '/wheel_commands'), + ) + rviz_config = os.path.join( + get_package_share_directory('particle_filter'), + 'rviz', + 'particle_filter.rviz' + ) + rviz = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', str(rviz_config)] + ) + + return LaunchDescription([ + rviz, + pf_landmark_detector, + visualizer + ]) \ No newline at end of file diff --git a/launch/tb3_agent_bringup.launch.py b/launch/tb3_agent_bringup.launch.py index 63b1b64..7efbb22 100644 --- a/launch/tb3_agent_bringup.launch.py +++ b/launch/tb3_agent_bringup.launch.py @@ -21,14 +21,14 @@ def generate_launch_description(): name = 'tb3_teleop_keyboard', output='screen' ) - pf_landmark_detector = Node( + landmark_detector = Node( package='particle_filter', executable='landmark_detector.py', name='landmark_detector' ) - + return LaunchDescription([ tb3_bringup, tb3_teleop, - pf_landmark_detector + landmark_detector ]) \ No newline at end of file diff --git a/rviz/particle_filter_viz.rviz b/rviz/particle_filter.rviz old mode 100644 new mode 100755 similarity index 84% rename from rviz/particle_filter_viz.rviz rename to rviz/particle_filter.rviz index 15f7b2b..8ee089c --- a/rviz/particle_filter_viz.rviz +++ b/rviz/particle_filter.rviz @@ -53,7 +53,7 @@ Visualization Manager: Enabled: true Name: MarkerArray Namespaces: - "": true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -65,7 +65,7 @@ Visualization Manager: Enabled: true Name: Marker Namespaces: - "": true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -78,7 +78,7 @@ Visualization Manager: Enabled: true Name: MarkerArray Namespaces: - "": true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -164,39 +164,6 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_scan: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - caster_back_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - wheel_left_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wheel_right_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Name: RobotModel TF Prefix: "" Update Interval: 0 @@ -248,33 +215,33 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 2.5553953647613525 + Distance: 1.8099536895751953 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.29303058981895447 - Y: -0.20801466703414917 - Z: 0.2340378761291504 + 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: 1.5062859058380127 + Pitch: 0.39979732036590576 Target Frame: Value: Orbit (rviz) - Yaw: 6.1554999351501465 + Yaw: 4.2285685539245605 Saved: ~ Window Geometry: Displays: - collapsed: true + collapsed: false Height: 1123 - Hide Left Dock: true + Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016d000003a3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000004c000003a3000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000003a3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004c000003a3000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003b80000004cfc0100000002fb0000000800540069006d00650100000000000003b80000029101000003fb0000000800540069006d00650100000000000004500000000000000000000002a7000003a300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016d000003a3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004c000003a3000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000003a3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004c000003a3000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007780000004cfc0100000002fb0000000800540069006d00650100000000000007780000029101000003fb0000000800540069006d00650100000000000004500000000000000000000004f9000003a300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -283,6 +250,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 952 + Width: 1912 X: 0 Y: 0 diff --git a/scripts/core_filter.py b/scripts/core_filter.py new file mode 100755 index 0000000..78ee898 --- /dev/null +++ b/scripts/core_filter.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from std_msgs.msg import String + +from geometry_msgs.msg import Point, Pose, Point, Quaternion, Vector3 +from rclpy.qos import qos_profile_sensor_data, QoSProfile +from std_msgs.msg import Float64 +import numpy as np +from particle_filter.helper import polar2cartesian, scan2cartesian, circlefit, cartesian2polar, clustering, cartesian_clustering +from particle_filter.msg import LdPose + +class MinimalPublisher(Node): + + def __init__(self): + super().__init__('minimal_publisher') + self.publisher_ = self.create_publisher(String, 'topic', 10) + timer_period = 0.5 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + self.i = 0 + + def timer_callback(self): + msg = String() + msg.data = 'Hello World: %d' % self.i + self.publisher_.publish(msg) + self.get_logger().info('Publishing: "%s"' % msg.data) + self.i += 1 + + +def main(args=None): + rclpy.init(args=args) + + minimal_publisher = MinimalPublisher() + + rclpy.spin(minimal_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file