From d1ac41ad1af12b61da48885f7638afa67b0ebf5c Mon Sep 17 00:00:00 2001
From: "bruno.galdos@rwth-aachen.de" <bruno.galdos@rwth-aachen.de>
Date: Thu, 25 Jul 2024 11:22:45 +0000
Subject: [PATCH] followaypoints
---
colcon_ws/src/my_bot/CMakeLists.txt | 33 +++
colcon_ws/src/my_bot/config/empty.yaml | 1 +
colcon_ws/src/my_bot/config/view_bot.rviz | 204 ++++++++++++++++++
.../description/gazebo_control_kairos.xacro | 77 +++++++
.../src/my_bot/launch/controller_launch.py | 65 ++++++
.../src/my_bot/launch/evaluation_launch.py | 76 +++++++
colcon_ws/src/my_bot/package.xml | 22 ++
.../evaluation_node_odom_final.py | 14 +-
.../my_controller/trial_controller_goal.py | 4 +-
.../my_controller/trial_controller_opti.py | 40 ++--
.../trial_controller_opti_improved.py | 4 +-
.../trial_controller_waypoints.py | 193 +++++++++++++++++
colcon_ws/src/my_controller/setup.py | 1 +
entrypoint_scripts/entrypoint_controller.sh | 5 +-
14 files changed, 706 insertions(+), 33 deletions(-)
create mode 100644 colcon_ws/src/my_bot/CMakeLists.txt
create mode 100644 colcon_ws/src/my_bot/config/empty.yaml
create mode 100644 colcon_ws/src/my_bot/config/view_bot.rviz
create mode 100644 colcon_ws/src/my_bot/description/gazebo_control_kairos.xacro
create mode 100644 colcon_ws/src/my_bot/launch/controller_launch.py
create mode 100644 colcon_ws/src/my_bot/launch/evaluation_launch.py
create mode 100644 colcon_ws/src/my_bot/package.xml
create mode 100755 colcon_ws/src/my_controller/my_controller/trial_controller_waypoints.py
diff --git a/colcon_ws/src/my_bot/CMakeLists.txt b/colcon_ws/src/my_bot/CMakeLists.txt
new file mode 100644
index 0000000..03c12dd
--- /dev/null
+++ b/colcon_ws/src/my_bot/CMakeLists.txt
@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.8)
+project(my_bot)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+install(DIRECTORY
+ launch
+ DESTINATION share/${PROJECT_NAME}
+)
+
+
+
+
+install(DIRECTORY
+ config
+ DESTINATION share/${PROJECT_NAME}
+)
+
+install(DIRECTORY
+ description
+ DESTINATION share/${PROJECT_NAME}
+)
+
+
+ament_package()
diff --git a/colcon_ws/src/my_bot/config/empty.yaml b/colcon_ws/src/my_bot/config/empty.yaml
new file mode 100644
index 0000000..04700ba
--- /dev/null
+++ b/colcon_ws/src/my_bot/config/empty.yaml
@@ -0,0 +1 @@
+# This is an empty file, so that git commits the folder correctly
\ No newline at end of file
diff --git a/colcon_ws/src/my_bot/config/view_bot.rviz b/colcon_ws/src/my_bot/config/view_bot.rviz
new file mode 100644
index 0000000..9b6f19b
--- /dev/null
+++ b/colcon_ws/src/my_bot/config/view_bot.rviz
@@ -0,0 +1,204 @@
+Panels:
+ - Class: rviz_common/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /TF1
+ - /RobotModel1
+ - /RobotModel1/Status1
+ Splitter Ratio: 0.5
+ Tree Height: 759
+ - 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
+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: <Fixed Frame>
+ Value: true
+ - Class: rviz_default_plugins/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ base_link:
+ Value: true
+ caster_wheel:
+ Value: true
+ chassis:
+ Value: true
+ left_wheel:
+ Value: true
+ right_wheel:
+ Value: true
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ base_link:
+ chassis:
+ caster_wheel:
+ {}
+ left_wheel:
+ {}
+ right_wheel:
+ {}
+ 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: Links in Alphabetic Order
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ caster_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ chassis:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: RobotModel
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: base_link
+ 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
+ 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: 1.0897012948989868
+ 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.4002038538455963
+ Target Frame: <Fixed Frame>
+ Value: Orbit (rviz)
+ Yaw: 5.888577461242676
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 997
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000016a00000386fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000386000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000386fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004100000386000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004fb0000038600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1920
+ X: 0
+ Y: 28
diff --git a/colcon_ws/src/my_bot/description/gazebo_control_kairos.xacro b/colcon_ws/src/my_bot/description/gazebo_control_kairos.xacro
new file mode 100644
index 0000000..8e58a46
--- /dev/null
+++ b/colcon_ws/src/my_bot/description/gazebo_control_kairos.xacro
@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+
+ <!-- ros control plugin-->
+ <xacro:macro name="ros_control">
+ <gazebo>
+ <plugin name="ros_control" filename="libgazebo_ros_control.so">
+ <!--<robotNamespace>/summit_xl</robotNamespace>-->
+ <robotParam>robot_description</robotParam>
+ <controlPeriod>0.001</controlPeriod>
+ <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+ </plugin>
+ </gazebo>
+ </xacro:macro>
+
+ <!-- skid steering plugin
+ libgazebo_ros_skid_steer_drive plugin was
+ replaced with the libgazebo_ros_diff_drive -->
+ <xacro:macro name="skid_steering" params="broadcastOdomTF prefix">
+ <gazebo>
+ <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_diff_drive.so">
+ <!--<robotNamespace>/summit_xl</robotNamespace>-->
+ <updateRate>100.0</updateRate>
+ <leftFrontJoint>${prefix}joint_front_left_wheel</leftFrontJoint>
+ <rightFrontJoint>${prefix}joint_front_right_wheel</rightFrontJoint>
+ <leftRearJoint>${prefix}joint_back_left_wheel</leftRearJoint>
+ <rightRearJoint>${prefix}joint_back_right_wheel</rightRearJoint>
+ <!-- wheelSeparation>0.566</wheelSeparation --> <!-- real parameter value -->
+ <wheelSeparation>1.5</wheelSeparation> <!-- works a bit better in Gazebo -->
+ <wheelDiameter>0.234</wheelDiameter>
+ <robotBaseFrame>${prefix}base_footprint</robotBaseFrame>
+ <torque>50</torque>
+ <commandTopic>robotnik_base_control/cmd_vel</commandTopic>
+ <odometryTopic>robotnik_base_control/odom</odometryTopic>
+ <!--odometryFrame>/odom</odometryFrame-->
+ <odometryFrame>${prefix}odom</odometryFrame>
+ <broadcastTF>${broadcastOdomTF}</broadcastTF>
+ </plugin>
+ </gazebo>
+ </xacro:macro>
+
+ <xacro:macro name="omni_steering" params="prefix publish_tf">
+ <gazebo>
+ <plugin name="omni_steering" filename="libgazebo_ros_planar_move.so">
+ <commandTopic>robotnik_base_control/cmd_vel</commandTopic>
+ <odometryTopic>robotnik_base_control/odom</odometryTopic>
+ <odometryFrame>${prefix}odom</odometryFrame>
+ <odometryRate>50.0</odometryRate>
+ <robotBaseFrame>${prefix}base_footprint</robotBaseFrame>
+ <publishTF>${publish_tf}</publishTF>
+ </plugin>
+ </gazebo>
+ </xacro:macro>
+<!--<robotNamespace>/summit_xl</robotNamespace>
+ <xacro:macro name="ros_force_based_move" params="publish_tf prefix">
+ <gazebo>
+ <plugin name="ros_force_based_move" filename="libgazebo_ros_force_based_move.so">
+ <commandTopic>robotnik_base_control/cmd_vel</commandTopic>
+ <odometryTopic>robotnik_base_control/odom</odometryTopic>
+ <odometryFrame>${prefix}odom</odometryFrame>
+ <yaw_velocity_p_gain>10000.0</yaw_velocity_p_gain>
+ <x_velocity_p_gain>10000.0</x_velocity_p_gain>
+ <y_velocity_p_gain>10000.0</y_velocity_p_gain>
+ <robotBaseFrame>${prefix}base_footprint</robotBaseFrame>
+ <odometryRate>50.0</odometryRate>
+ <publishOdometryTf>${publish_tf}</publishOdometryTf>
+ </plugin>
+ </gazebo>
+ </xacro:macro>
+-->
+ <gazebo reference="base_footprint">
+ <material>Gazebo/Green</material>
+ </gazebo>
+
+
+</robot>
\ No newline at end of file
diff --git a/colcon_ws/src/my_bot/launch/controller_launch.py b/colcon_ws/src/my_bot/launch/controller_launch.py
new file mode 100644
index 0000000..a56a3eb
--- /dev/null
+++ b/colcon_ws/src/my_bot/launch/controller_launch.py
@@ -0,0 +1,65 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+import logging
+
+def generate_launch_description():
+ try:
+ return LaunchDescription([
+
+ DeclareLaunchArgument(
+ 'x', default_value='1.0', description='X coordinate of the goal pose'
+ ),
+ DeclareLaunchArgument(
+ 'y', default_value='1.0', description='Y coordinate of the goal pose'
+ ),
+ DeclareLaunchArgument(
+ 'theta', default_value='0.0', description='Theta of the goal pose'
+ ),
+ DeclareLaunchArgument(
+ 'velocity', default_value='0.2', description='Velocity of the goal pose'
+ ),
+ DeclareLaunchArgument(
+ 'waypoints',
+ default_value='['
+ '2.9, -0.08, 0.0, '
+ '4.9, -0.08, 0.0, '
+ '4.9, 1.92, 0.0, '
+ '2.9, 1.92, 0.0'
+ ']',
+
+ # default_value='['
+ # '2.9, -0.08, 0.0, '
+ # '4.9, 1.92, 0.0, '
+ # '4.9, -0.08, 0.0, '
+ # '2.9, 1.92, 0.0'
+ # ']',
+ description='Waypoints to follow (flat list)'
+ ),
+ DeclareLaunchArgument(
+ 'waypoint_velocity', default_value='0.2', description='Velocity for following waypoints'
+ ),
+
+ Node(
+ package='my_controller',
+ executable='trial_controller_waypoints',
+ name='trial_controller_node',
+ output='screen',
+ parameters=[
+ {'x': LaunchConfiguration('x')},
+ {'y': LaunchConfiguration('y')},
+ {'theta': LaunchConfiguration('theta')},
+ {'velocity': LaunchConfiguration('velocity')},
+ {'waypoints': LaunchConfiguration('waypoints')},
+ {'waypoint_velocity': LaunchConfiguration('waypoint_velocity')}
+ ]
+ )
+
+ ])
+ except Exception as e:
+ logging.error("An error occurred: " + str(e))
+ raise
diff --git a/colcon_ws/src/my_bot/launch/evaluation_launch.py b/colcon_ws/src/my_bot/launch/evaluation_launch.py
new file mode 100644
index 0000000..2ee1278
--- /dev/null
+++ b/colcon_ws/src/my_bot/launch/evaluation_launch.py
@@ -0,0 +1,76 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ # Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled
+ # !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!!
+ package_name = 'my_controller' # <--- CHANGE ME
+
+ # Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
+ evaluation_slam = Node(
+ package=package_name,
+ executable='evaluation_node_slam',
+ output='screen'
+ )
+
+ evaluation_odom = Node(
+ package=package_name,
+ executable='evaluation_node_odom',
+ output='screen'
+ )
+ """
+ trial_controller_1 = Node(
+ package=package_name,
+ executable='trial_controller',
+ name='trial_controller_1', # Fixed the name
+ parameters=[
+
+ {'goal_pose': {'x': -2.0, 'y': -1.5, 'theta': 0.0}}],
+ output='screen'
+ )
+
+ trial_controller_2 = Node(
+ package=package_name,
+ executable='trial_controller',
+ name='trial_controller_2', # Fixed the name
+ parameters=[{'goal_pose': {'x': 2.5, 'y': -2.0, 'theta': 0.0}}],
+ output='screen'
+ )
+
+
+ trial_controller_3 = Node(
+ package=package_name,
+ executable='trial_controller',
+ name='trial_controller_3', # Fixed the name
+
+ #parameters=[{'goal_pose': {'x': 2.0, 'y': 3.0, 'theta': 0.0}}],
+ output='screen'
+ )
+ """
+ # Code for delaying a node (I haven't tested how effective it is)
+ # First add the below lines to imports
+ # from launch.actions import RegisterEventHandler
+ # from launch.event_handlers import OnProcessExit
+ #
+ # Then add the following below the current diff_drive_spawner
+ # delayed_diff_drive_spawner = RegisterEventHandler(
+ # event_handler=OnProcessExit(
+ # target_action=spawn_entity,
+ # on_exit=[diff_drive_spawner],
+ # )
+ # )
+ #
+ # Replace the diff_drive_spawner in the final return with delayed_diff_drive_spawner
+
+ # Launch them all!
+ return LaunchDescription([
+
+
+ evaluation_odom,
+ evaluation_slam
+ ])
diff --git a/colcon_ws/src/my_bot/package.xml b/colcon_ws/src/my_bot/package.xml
new file mode 100644
index 0000000..b9d41c4
--- /dev/null
+++ b/colcon_ws/src/my_bot/package.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>my_bot</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="athena@todo.todo">athena</maintainer>
+ <license>TODO: License declaration</license>
+
+ <buildtool_depend>ament_cmake</buildtool_depend>
+
+ <exec_depend>robot_state_publisher</exec_depend>
+
+ <exec_depend>my_controller</exec_depend>
+
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_lint_common</test_depend>
+
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>
diff --git a/colcon_ws/src/my_controller/my_controller/evaluation_node_odom_final.py b/colcon_ws/src/my_controller/my_controller/evaluation_node_odom_final.py
index 5482557..1367e4b 100755
--- a/colcon_ws/src/my_controller/my_controller/evaluation_node_odom_final.py
+++ b/colcon_ws/src/my_controller/my_controller/evaluation_node_odom_final.py
@@ -17,18 +17,18 @@ class EvaluateNode(Node):
def __init__(self):
super().__init__('optitrack_subscriber')
- self.subscription = self.create_subscription(
- RigidBodyStamped,
- '/optitrack_multiplexer_node/rigid_body/kairosAB',
- self.pose_callback,
- 10)
-
#self.subscription = self.create_subscription(
# RigidBodyStamped,
- # '/optitrack_multiplexer_node/rigid_body/BenchmarkArtefact',
+ # '/optitrack_multiplexer_node/rigid_body/kairosAB',
# self.pose_callback,
# 10)
+ self.subscription = self.create_subscription(
+ RigidBodyStamped,
+ '/optitrack_multiplexer_node/rigid_body/BenchmarkArtefact',
+ self.pose_callback,
+ 10)
+
self.get_logger().info(f"taking metrics from optitrack")
self.csv_file=None
diff --git a/colcon_ws/src/my_controller/my_controller/trial_controller_goal.py b/colcon_ws/src/my_controller/my_controller/trial_controller_goal.py
index 8ae49d7..0278ce1 100755
--- a/colcon_ws/src/my_controller/my_controller/trial_controller_goal.py
+++ b/colcon_ws/src/my_controller/my_controller/trial_controller_goal.py
@@ -23,8 +23,8 @@ class MoveRobotNode(Node):
#self.subscription = self.create_subscription(Odometry, '/diff_cont/odom', self.odom_callback, 10) #differential
#self.subscription = self.create_subscription(Odometry, '/robot/robotnik_base_control/odom', self.odom_callback, 10) #for kairos simulation
#self.subscription = self.create_subscription(Odometry, '/kairosAB/robotnik_base_control/odom', self.odom_callback, 10) #for kairos AB real
- self.subscription = self.create_subscription(RigidBodyStamped,'/optitrack_multiplexer_node/rigid_body/BenchmarkArtefact',self.odom_callback,10)
- #self.subscription = self.create_subscription(RigidBodyStamped,'/optitrack_multiplexer_node/rigid_body/kairosAB',self.odom_callback,10)
+ #self.subscription = self.create_subscription(RigidBodyStamped,'/optitrack_multiplexer_node/rigid_body/BenchmarkArtefact',self.odom_callback,10)
+ self.subscription = self.create_subscription(RigidBodyStamped,'/optitrack_multiplexer_node/rigid_body/kairosAB',self.odom_callback,10)
self.current_pose = [0.02, 0.02, 0.02]
#self.current_pose =self.get_start_pose()
diff --git a/colcon_ws/src/my_controller/my_controller/trial_controller_opti.py b/colcon_ws/src/my_controller/my_controller/trial_controller_opti.py
index 47f6bc3..faa9911 100755
--- a/colcon_ws/src/my_controller/my_controller/trial_controller_opti.py
+++ b/colcon_ws/src/my_controller/my_controller/trial_controller_opti.py
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
-
+import time
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
@@ -29,12 +29,12 @@ class MoveRobotNode(Node):
#self.publisher_ = self.create_publisher(Twist, '/diff_cont/cmd_vel_unstamped', 10) #for differential robot
#self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10) #normal msg topic
#self.publisher_ = self.create_publisher(Twist, 'robot/cmd_vel', 10) #for kairos simulation
- self.publisher_ = self.create_publisher(Twist, 'kairosAB/cmd_vel', 10) #for kairosAA real
+ self.publisher_ = self.create_publisher(Twist, 'kairosAA/cmd_vel', 10) #for kairosAA real
#self.publisher_ = self.create_publisher(Twist, 'kairosAB/cmd_vel', 10) #for kairosAB real
#self.subscription = self.create_subscription(Odometry, '/odom', self.odom_callback, 10) #normal
#self.subscription = self.create_subscription(Odometry, '/diff_cont/odom', self.odom_callback, 10) #differential
- self.subscription = self.create_subscription(RigidBodyStamped,'/optitrack_multiplexer_node/rigid_body/kairosAB',self.odom_callback,10)
+ self.subscription = self.create_subscription(RigidBodyStamped,'/optitrack_multiplexer_node/rigid_body/kairosAA',self.odom_callback,10)
#self.subscription = self.create_subscription(RigidBodyStamped,'/optitrack_multiplexer_node/rigid_body/kairosAB',self.odom_callback,10)
#self.subscription = self.create_subscription(Odometry, '/robot/robotnik_base_control/odom', self.odom_callback, 10) #for kairos simulation
@@ -46,8 +46,11 @@ class MoveRobotNode(Node):
#self.current_pose = [0.02, 0.02, 0.02]
#self.goal_pose = [0.05, 0.05, 0.05]
- self.current_pose = [0.0002, 0.0002, 0.002]
- self.goal_pose = [1.5003, 1.5003, 0.005]
+ self.current_pose = [-1.92, -2.9, 0.02]
+ #self.current_pose = [0.0002, 0.0002, 0.02] original
+ self.goal_pose = [-2.14, -6.5, 0.05]
+ #self.goal_pose = [1.9603, 1.96003, 0.05] original
+
def pose_callback(self,msg):
@@ -75,15 +78,10 @@ class MoveRobotNode(Node):
-
+ #move it gheers
#function for moving the robot
- """
- def move_robot(self, velocity, obstacle):
-
-
- obstacle_border = obstacle['border']
- """
- def move_robot(self, goal_pose_2,velocity):
+
+ def move_robot(self,velocity):
@@ -94,7 +92,7 @@ class MoveRobotNode(Node):
# Stage 1: Move the robot forward towards the goal
distance = (error_x**2 + error_y**2)**0.5
- while distance >= 1.40: #previous 0.01
+ while distance >= 1.7: #previous 0.6 original
@@ -124,7 +122,7 @@ class MoveRobotNode(Node):
msg.linear.x = linear_velocity_x
msg.linear.y= linear_velocity_y
- if distance < 1.40: # threshold original 0.01
+ if distance < 1.7: # threshold original 0.6
print("first stage reached END") # stop rotating
@@ -142,8 +140,9 @@ class MoveRobotNode(Node):
-
+ """
# Stage 2: Rotate the robot to the goal orientation
+ time.sleep(1)
error_theta_final = goal_pose_2 - self.current_pose[2]
error_theta_final = (error_theta_final + math.pi) % (2 * math.pi) - math.pi # normalize to [-pi, pi]
while abs(error_theta_final) > 0.01: # adjust threshold
@@ -170,7 +169,7 @@ class MoveRobotNode(Node):
self.publisher_.publish(msg)
rclpy.spin_once(self)
-
+ """
@@ -181,14 +180,15 @@ def main(args=None):
- goal_pose_2=float(input("Enter goal theta from -π to π : "))
+ #goal_pose_2=float(input("Enter goal theta from -π to π : "))
velocity = float(input("Enter velocity: "))
- obstacle = {'border': [(1, 1), (1, 2), (2, 2), (2, 1)]}
+ #obstacle = {'border': [(1, 1), (1, 2), (2, 2), (2, 1)]}
- move_robot_node.move_robot(goal_pose_2,velocity)
+ #move_robot_node.move_robot(goal_pose_2,velocity)
+ move_robot_node.move_robot(velocity)
#move_robot_node.move_robot(velocity,obstacle)
rclpy.spin(move_robot_node)
diff --git a/colcon_ws/src/my_controller/my_controller/trial_controller_opti_improved.py b/colcon_ws/src/my_controller/my_controller/trial_controller_opti_improved.py
index 46b5dc8..9556d2f 100755
--- a/colcon_ws/src/my_controller/my_controller/trial_controller_opti_improved.py
+++ b/colcon_ws/src/my_controller/my_controller/trial_controller_opti_improved.py
@@ -29,12 +29,12 @@ class MoveRobotNode(Node):
#self.publisher_ = self.create_publisher(Twist, '/diff_cont/cmd_vel_unstamped', 10) #for differential robot
#self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10) #normal msg topic
#self.publisher_ = self.create_publisher(Twist, 'robot/cmd_vel', 10) #for kairos simulation
- self.publisher_ = self.create_publisher(Twist, 'kairosAB/cmd_vel', 10) #for kairosAA real
+ self.publisher_ = self.create_publisher(Twist, 'kairosAA/cmd_vel', 10) #for kairosAA real
#self.publisher_ = self.create_publisher(Twist, 'kairosAB/cmd_vel', 10) #for kairosAB real
#self.subscription = self.create_subscription(Odometry, '/odom', self.odom_callback, 10) #normal
#self.subscription = self.create_subscription(Odometry, '/diff_cont/odom', self.odom_callback, 10) #differential
- self.subscription = self.create_subscription(RigidBodyStamped,'/optitrack_multiplexer_node/rigid_body/kairosAB',self.odom_callback,10)
+ self.subscription = self.create_subscription(RigidBodyStamped,'/optitrack_multiplexer_node/rigid_body/kairosAA',self.odom_callback,10)
#self.subscription = self.create_subscription(RigidBodyStamped,'/optitrack_multiplexer_node/rigid_body/kairosAB',self.odom_callback,10)
#self.subscription = self.create_subscription(Odometry, '/robot/robotnik_base_control/odom', self.odom_callback, 10) #for kairos simulation
diff --git a/colcon_ws/src/my_controller/my_controller/trial_controller_waypoints.py b/colcon_ws/src/my_controller/my_controller/trial_controller_waypoints.py
new file mode 100755
index 0000000..46af3d8
--- /dev/null
+++ b/colcon_ws/src/my_controller/my_controller/trial_controller_waypoints.py
@@ -0,0 +1,193 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+
+from scipy.spatial.transform import Rotation as R
+import math
+from optitrack_multiplexer_ros2_msgs.msg import RigidBodyStamped
+import sys
+from functools import partial
+class MoveRobotNode(Node):
+
+
+ def __init__(self):
+ super().__init__('move_robot_node')
+ self.declare_parameter('x', 0.0)
+ self.declare_parameter('y', 0.0)
+ self.declare_parameter('theta', 0.0)
+ self.declare_parameter('velocity', 0.0)
+
+ self.declare_parameter('waypoints', [
+
+ 2.9, 0.32, 0.0, \
+ 4.5, 0.32, 0.0, \
+ 4.5, 1.92, 0.0, \
+ 2.9, 1.92, 0.0
+ ])
+ self.declare_parameter('waypoint_velocity', 0.2)
+
+ #self.publisher_ = self.create_publisher(Twist, '/diff_cont/cmd_vel_unstamped', 10) #for differential robot
+ #self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10) #normal msg topic
+ #self.publisher_ = self.create_publisher(Twist, 'robot/cmd_vel', 10) #for kairos simulation
+ self.publisher_ = self.create_publisher(Twist, 'kairosAA/cmd_vel', 10) #for kairosAB real
+ #self.subscription = self.create_subscription(Odometry, '/odom', self.odom_callback, 10) #normal
+ #self.subscription = self.create_subscription(Odometry, '/diff_cont/odom', self.odom_callback, 10) #differential
+ #self.subscription = self.create_subscription(Odometry, '/robot/robotnik_base_control/odom', self.odom_callback, 10) #for kairos simulation
+ #self.subscription = self.create_subscription(Odometry, '/kairosAB/robotnik_base_control/odom', self.odom_callback, 10) #for kairos AB real
+ #self.subscription = self.create_subscription(RigidBodyStamped,'/optitrack_multiplexer_node/rigid_body/BenchmarkArtefact',self.odom_callback,10)
+ self.subscription = self.create_subscription(RigidBodyStamped,'/optitrack_multiplexer_node/rigid_body/kairosAA',self.odom_callback,10)
+
+ self.current_pose = [0.02, 0.02, 0.02]
+ #self.current_pose =self.get_start_pose()
+
+ """
+ def get_start_pose(self):
+
+ start_x = -float(input("Enter start y in optitrack coords: "))
+ start_y = -float(input("Enter start x in optitrack coords: "))
+ self.start_theta = float( input("Enter start theta from -π to π :"))
+ return [start_x, start_y, self.start_theta]
+ """
+
+ def follow_waypoints(self):
+ waypoints_param = self.get_parameter('waypoints').get_parameter_value().double_array_value
+ velocity = self.get_parameter('waypoint_velocity').get_parameter_value().double_value
+
+ # Reshape flat list into list of [x, y, theta] lists
+ waypoints = [waypoints_param[i:i + 3] for i in range(0, len(waypoints_param), 3)]
+
+ for waypoint in waypoints:
+ self.get_logger().info(f'Moving to waypoint: {waypoint}')
+
+ self.move_robot(waypoint, velocity)
+ # Simulate waiting for robot to reach the waypoint (you might want to implement proper feedback)
+
+ rclpy.spin_once(self, timeout_sec=6)
+
+ self.get_logger().info('Finished following waypoints')
+
+
+ def odom_callback(self, msg):
+
+ self.current_pose[0] = msg.rigid_body.pose.position.y
+ self.current_pose[1] = -msg.rigid_body.pose.position.x
+ orientation_q = msg.rigid_body.pose.orientation
+ r = R.from_quat([orientation_q.q_x, orientation_q.q_y, orientation_q.q_z, orientation_q.q_w])
+ _, _, roll = r.as_euler('XYZ')
+
+ self.current_pose[2] = roll
+
+ #function for moving the robot
+ def move_robot(self, goal_pose, velocity):
+
+ error_x = goal_pose[0] - self.current_pose[0]
+ error_y = goal_pose[1] - self.current_pose[1]
+
+
+ # Stage 1: Move the robot forward towards the goal
+ distance = (error_x**2 + error_y**2)**0.5
+
+ while distance >= 0.03:
+ error_x = goal_pose[0] - self.current_pose[0]
+ error_y = goal_pose[1] - self.current_pose[1]
+ distance = (error_x**2 + error_y**2)**0.5
+
+ print("Current pose x,y,theta: ", self.current_pose)
+ print("distance left", distance)
+ # direction towards the goal
+ direction_to_goal = [goal_pose[0] - self.current_pose[0], goal_pose[1] - self.current_pose[1]]
+
+ # current direction of the robot based on start_theta pose
+ #current_direction=[math.cos(self.start_theta), math.sin(self.start_theta)]
+ current_direction = [math.cos(self.current_pose[2]), math.sin(self.current_pose[2])]
+
+ # angle between the current direction and the direction to the goal
+ angle_to_goal = math.atan2(direction_to_goal[1], direction_to_goal[0]) - math.atan2(current_direction[1], current_direction[0])
+
+ # Normalize the angle to the range [-pi, pi] since gazebo is in that way
+ angle_to_goal = (angle_to_goal + math.pi) % (2 * math.pi) - math.pi
+
+ linear_velocity_x = velocity * math.cos(angle_to_goal)
+ linear_velocity_y = velocity * math.sin(angle_to_goal)
+
+ msg = Twist()
+ msg.linear.x = linear_velocity_x
+ msg.linear.y= linear_velocity_y
+ if distance < 0.03: # threshold if the robot is close enough to the target orientation
+
+ print("first stage reached END") # stop rotating
+
+ stop_msg = Twist()
+ stop_msg.linear.x = 0.0
+ stop_msg.linear.y =0.0
+ stop_msg.angular.z =0.0
+ self.publisher_.publish(stop_msg)
+ break
+
+
+
+ self.publisher_.publish(msg)
+
+ rclpy.spin_once(self)
+
+ # Stage 2: Rotate the robot to the goal orientation
+ error_theta_final = goal_pose[2] - self.current_pose[2]
+ error_theta_final = (error_theta_final + math.pi) % (2 * math.pi) - math.pi # normalize to [-pi, pi]
+ while abs(error_theta_final) > 0.01: # adjust threshold
+
+ angular_velocity = velocity * error_theta_final
+ print("error_theta_is",error_theta_final)
+ error_theta_final = goal_pose[2] - self.current_pose[2]
+ error_theta_final = (error_theta_final + math.pi) % (2 * math.pi) - math.pi
+ if abs(error_theta_final) < 0.01: # if the robot is close enough to the target orientation
+
+ print("goal orientation reached END") # stop rotating
+ # create and publish Twist message to stop the robot
+ stop_msg = Twist()
+ stop_msg.linear.x = 0.0
+ stop_msg.linear.y = 0.0
+ stop_msg.angular.z =0.0
+ self.publisher_.publish(stop_msg)
+
+
+ break
+
+ msg = Twist()
+ msg.angular.z = angular_velocity
+ self.publisher_.publish(msg)
+
+ rclpy.spin_once(self)
+
+
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ move_robot_node = MoveRobotNode()
+
+ #goal_pose = [-float(input("Enter goal y in optitrack coords: ")), -float(input("Enter goal x in optitrack coords: ")), float(input("Enter goal theta from -π to π : "))]
+ #velocity = float(input("Enter velocity: "))
+
+ goal_pose = [
+ move_robot_node.get_parameter('x').get_parameter_value().double_value,
+ move_robot_node.get_parameter('y').get_parameter_value().double_value,
+ move_robot_node.get_parameter('theta').get_parameter_value().double_value
+ ]
+ velocity = move_robot_node.get_parameter('velocity').get_parameter_value().double_value
+
+
+ #move_robot_node.move_robot(goal_pose, velocity)
+ move_robot_node.follow_waypoints()
+
+ rclpy.spin(move_robot_node)
+
+ move_robot_node.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
+
diff --git a/colcon_ws/src/my_controller/setup.py b/colcon_ws/src/my_controller/setup.py
index 664ce66..95f09f7 100644
--- a/colcon_ws/src/my_controller/setup.py
+++ b/colcon_ws/src/my_controller/setup.py
@@ -27,6 +27,7 @@ setup(
"trial_controller_opti_improved= my_controller.trial_controller_opti_improved:main",
"trial_controller_backup_original= my_controller.trial_controller_backup_original:main",
"trial_controller_goal= my_controller.trial_controller_goal:main",
+ "trial_controller_waypoints= my_controller.trial_controller_waypoints:main",
"evaluation_node=my_controller.evaluation_node:main",
"simplenode=my_controller.simplenode:main",
"node_markers=my_controller.node_markers:main",
diff --git a/entrypoint_scripts/entrypoint_controller.sh b/entrypoint_scripts/entrypoint_controller.sh
index 8523635..a1dfa2d 100644
--- a/entrypoint_scripts/entrypoint_controller.sh
+++ b/entrypoint_scripts/entrypoint_controller.sh
@@ -19,8 +19,9 @@ source /colcon_ws/install/setup.bash
#ros2 run my_controller trial_controller_diff_opti
#ros2 run my_controller trial_controller_diff
#ros2 run my_controller trial_controller_backup
-#ros2 run my_controller trial_controller_opti
+ros2 run my_controller trial_controller_opti
#ros2 run my_controller trial_controller_opti_improved
#ros2 run my_controller trial_controller
-ros2 run my_controller trial_controller_goal
+#ros2 run my_controller trial_controller_goal
+#ros2 launch my_bot controller_launch.py
#ros2 run my_controller node_markers
--
GitLab