Skip to content
Snippets Groups Projects
Commit af359afb authored by Bruno Galdos's avatar Bruno Galdos
Browse files

modified: colcon_ws/src/my_controller/my_controller/evaluation_node_odom.py 

	new file:   colcon_ws/src/my_controller/my_controller/evaluation_node_odom_final.py
	modified:   colcon_ws/src/my_controller/my_controller/simplenode.py
	modified:   colcon_ws/src/my_controller/my_controller/trial_controller.py
	renamed:    colcon_ws/src/my_controller/my_controller/trial_controller_backup.py -> colcon_ws/src/my_controller/my_controller/trial_controller_goal.py
	modified:   colcon_ws/src/my_controller/my_controller/trial_controller_opti.py
	new file:   colcon_ws/src/my_controller/my_controller/trial_controller_opti_improved.py
	modified:   colcon_ws/src/my_controller/setup.py
	modified:   entrypoint_scripts/entrypoint_controller.sh
parent 92ed51a2
No related branches found
No related tags found
No related merge requests found
Pipeline #414662 failed
Stage: build
Stage: registry_push
Show whitespace changes
Inline Side-by-side
Showing
with 407 additions and 197 deletions
colcon_ws/src/my_controller/my_controller/evaluation_node_odom.py
+ 19
− 58
View file @ af359afb
...@@ -3,7 +3,6 @@ ...@@ -3,7 +3,6 @@
import threading import threading
import time import time
import csv import csv
from geometry_msgs.msg import Pose,Point,Quaternion
from rclpy.node import Node from rclpy.node import Node
from datetime import datetime from datetime import datetime
import rclpy import rclpy
...@@ -11,7 +10,6 @@ from rclpy.node import Node ...@@ -11,7 +10,6 @@ from rclpy.node import Node
from nav_msgs.msg import Odometry from nav_msgs.msg import Odometry
from scipy.spatial.transform import Rotation as R from scipy.spatial.transform import Rotation as R
from geometry_msgs.msg import PoseWithCovarianceStamped from geometry_msgs.msg import PoseWithCovarianceStamped
from optitrack_multiplexer_ros2_msgs.msg import RigidBodyStamped
class EvaluateNode(Node): class EvaluateNode(Node):
def __init__(self): def __init__(self):
...@@ -24,9 +22,8 @@ class EvaluateNode(Node): ...@@ -24,9 +22,8 @@ class EvaluateNode(Node):
prefix_mapping ={ prefix_mapping ={
#"odom":'/robot/robotnik_base_control/odom', #"odom":'/robot/robotnik_base_control/odom',
"odom":'/optitrack_multiplexer_node/rigid_body/kairosAB',
#"odom" : '/diff_cont/odom', #For differential #"odom" : '/diff_cont/odom', #For differential
#"odom" : '/kairosAB/robotnik_base_control/odom' "odom" : '/kairosAB/robotnik_base_control/odom'
#"nav" : '/robot/amcl_pose' #"nav" : '/robot/amcl_pose'
} }
...@@ -35,10 +32,7 @@ class EvaluateNode(Node): ...@@ -35,10 +32,7 @@ class EvaluateNode(Node):
topic =prefix_mapping[prefix] topic =prefix_mapping[prefix]
self.get_logger().info(f"taking metrics from{ topic}") self.get_logger().info(f"taking metrics from{ topic}")
if prefix=="odom":
self.subscription = self.create_subscription(Odometry, topic, self.odom_callback, 10) self.subscription = self.create_subscription(Odometry, topic, self.odom_callback, 10)
else:
self.pose_subscriber=self.create_subscription(PoseWithCovarianceStamped, '/robot/amcl_pose', self.pose_callback, 10)
now=datetime.now() now=datetime.now()
...@@ -46,11 +40,11 @@ class EvaluateNode(Node): ...@@ -46,11 +40,11 @@ class EvaluateNode(Node):
self.csv_file = open("data_"+prefix+"_"+now_str+".csv", 'w', newline='') self.csv_file = open("data_"+prefix+"_"+now_str+".csv", 'w', newline='')
self.writer = csv.writer(self.csv_file) self.writer = csv.writer(self.csv_file)
self.writer.writerow(['timestamp', 'x', 'y', 'yaw']) self.writer.writerow(['timestamp', 'x', 'y', 'z'])
self.received_message=False self.received_message=False
self.check_thread = threading.Thread(target=self.check_messages) self.check_thread = threading.Thread(target=self.check_messages)
self.check_thread.start() self.check_thread.start()
self.prev_x = self.prev_y = self.prev_theta = None self.prev_x = self.prev_y = None
def check_messages(self): def check_messages(self):
while not self.received_message: while not self.received_message:
...@@ -60,75 +54,39 @@ class EvaluateNode(Node): ...@@ -60,75 +54,39 @@ class EvaluateNode(Node):
def odom_callback(self, msg): def odom_callback(self, msg):
# Extract position # Extract position
self.received_message=True self.received_message=True
x = msg.rigid_body.pose.position.x x = msg.pose.pose.position.x
#y = msg.pose.pose.position.y y = msg.pose.pose.position.y
y = msg.rigid_body.pose.position.z z = msg.pose.pose.position.z
# Extract orientation quaternion
orientation_q = msg.rigid_body.pose.orientation
orientation_list = [orientation_q.x, orientation_q.z, orientation_q.y, orientation_q.w]
# Convert quaternion to yaw # Convert quaternion to yaw
rotation = R.from_quat(orientation_list)
yaw = rotation.as_euler('zyx')[0]
is_moving=False is_moving=False
if self.prev_x is not None: if self.prev_x is not None:
dx = abs(x - self.prev_x) dx = abs(x - self.prev_x)
dy = abs(y - self.prev_y) dy = abs(y - self.prev_y)
dtheta = abs(yaw - self.prev_theta)
if dx > 0.00001 or dy > 0.00001 or dtheta > 0.00001: if dx > 0.00001 or dy > 0.00001:
is_moving=True is_moving=True
self.prev_x = x self.prev_x = x
self.prev_y = y self.prev_y = y
self.prev_theta = yaw
# Extract timestamp # Extract timestamp
if is_moving: if is_moving:
now=datetime.now()
timestamp= now.strftime("%Y-%m-%dT%H:%M:%S.%f")
self.writer.writerow([timestamp, x, y, z])
timestamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 print("Current x:",x,"Current y:",y,"z:",z)
self.writer.writerow([timestamp, x, y, yaw])
print("Current x:",x,"Current y:",y,"yaw:",yaw)
#print("writing in csv") #print("writing in csv")
else: else:
print("Waiting for moving...") print("Waiting for moving...")
def pose_callback(self,msg):
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
self.amcl_pose = [x,y]
orientation_q = msg.pose.pose.orientation
r = R.from_quat([orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w])
_, _, yaw = r.as_euler('xyz')
self.amcl_pose.append(yaw)
is_moving=False
if self.prev_x is not None:
dx = abs(x - self.prev_x)
dy = abs(y - self.prev_y)
dtheta = abs(yaw - self.prev_theta)
if dx > 0.000001 or dy > 0.000001 or dtheta > 0.000001:
is_moving=True
self.prev_x = x
self.prev_y = y
self.prev_theta = yaw
# Extract timestamp
if is_moving:
now = datetime.now() #added on 15th feb
"""
timestamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
"""
timestamp = now.timestamp()
self.writer.writerow([timestamp, x, y, yaw])
print("writing in csv")
else:
print("Waiting for moving...")
def __del__(self): def __del__(self):
if self.csv_file is not None: if self.csv_file is not None:
self.csv_file.close() self.csv_file.close()
PoseWithCovarianceStamped
def main(args=None): def main(args=None):
rclpy.init(args=args) rclpy.init(args=args)
...@@ -143,3 +101,6 @@ def main(args=None): ...@@ -143,3 +101,6 @@ def main(args=None):
if __name__ == "__main__": if __name__ == "__main__":
main() main()
colcon_ws/src/my_controller/my_controller/evaluation_node_odom_final.py 0 → 100755
+ 97
− 0
View file @ af359afb
#!/usr/bin/env python3
import os
import pytz
import threading
import time
import csv
from rclpy.node import Node
from datetime import datetime
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from scipy.spatial.transform import Rotation as R
from optitrack_multiplexer_ros2_msgs.msg import RigidBodyStamped
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',
# self.pose_callback,
# 10)
self.get_logger().info(f"taking metrics from optitrack")
self.csv_file=None
b_tz = pytz.timezone('UTC')
now = datetime.now(b_tz)
#now=datetime.now()
now_str= now.strftime("%Y-%m-%d_%H-%M-%S")
self.csv_file = open("data_"+"optitrack"+"_"+now_str+".csv", 'w', newline='')
self.writer = csv.writer(self.csv_file)
self.writer.writerow(['timestamp', 'x', 'y', 'z'])
self.received_message=False
self.check_thread = threading.Thread(target=self.check_messages)
self.check_thread.start()
def check_messages(self):
while not self.received_message:
self.get_logger().info("Waiting for messages...")
time.sleep(2)
def pose_callback(self, msg):
# Extract position
self.received_message=True
x = msg.rigid_body.pose.position.x
y = -msg.rigid_body.pose.position.y
z = msg.rigid_body.pose.position.z
b_tz = pytz.timezone('UTC')
now=datetime.now(b_tz)
timestamp= now.strftime("%Y-%m-%dT%H:%M:%S.%f")
self.writer.writerow([timestamp, x, y, z])
#print(os.getcwd())
print(timestamp,x,y,z)
#print("writing in csv")
def __del__(self):
if self.csv_file is not None:
self.csv_file.close()
def main(args=None):
rclpy.init(args=args)
node =EvaluateNode()
rclpy.spin(node)
rclpy.shutdown()
if __name__ == "__main__":
main()
colcon_ws/src/my_controller/my_controller/simplenode.py
+ 20
− 3
View file @ af359afb
#!/usr/bin/env python3 #!/usr/bin/env python3
from datetime import datetime
import pytz
import rclpy import rclpy
from rclpy.node import Node from rclpy.node import Node
from scipy.spatial.transform import Rotation as R from scipy.spatial.transform import Rotation as R
from optitrack_multiplexer_ros2_msgs.msg import RigidBodyStamped from optitrack_multiplexer_ros2_msgs.msg import RigidBodyStamped
import numpy as np import numpy as np
from datetime import datetime, timezone
class SimpleSubscriberNode(Node): class SimpleSubscriberNode(Node):
def __init__(self): def __init__(self):
super().__init__('simple_subscriber') super().__init__('simple_subscriber')
...@@ -12,11 +14,22 @@ class SimpleSubscriberNode(Node): ...@@ -12,11 +14,22 @@ class SimpleSubscriberNode(Node):
self.get_logger().info('Initial Pose: %s' % self.current_pose) self.get_logger().info('Initial Pose: %s' % self.current_pose)
self.subscription = self.create_subscription( self.subscription = self.create_subscription(
RigidBodyStamped, RigidBodyStamped,
'/optitrack_multiplexer_node/rigid_body/kairosAB', '/optitrack_multiplexer_node/rigid_body/BenchmarkArtefact',
self.pose_callback, self.pose_callback,
10) 10)
def pose_callback(self, msg): def pose_callback(self, msg):
#now=datetime.now()
#timestamp= now.strftime("%Y-%m-%dT%H:%M:%S.%f")
gb_tz = pytz.timezone('Europe/London')
# Get the current time in the Great Britain timezone
now = datetime.now(gb_tz)
timestamp = now.strftime("%Y-%m-%dT%H:%M:%S.%f")
self.current_pose[0] = msg.rigid_body.pose.position.x self.current_pose[0] = msg.rigid_body.pose.position.x
self.current_pose[1] = -msg.rigid_body.pose.position.y self.current_pose[1] = -msg.rigid_body.pose.position.y
orientation_q = msg.rigid_body.pose.orientation orientation_q = msg.rigid_body.pose.orientation
...@@ -25,7 +38,8 @@ class SimpleSubscriberNode(Node): ...@@ -25,7 +38,8 @@ class SimpleSubscriberNode(Node):
#self.current_pose[2] = np.degrees(pitch) #self.current_pose[2] = np.degrees(pitch)
#self.current_pose[3] = np.degrees(yaw) #self.current_pose[3] = np.degrees(yaw)
self.current_pose[2] = np.degrees(roll) self.current_pose[2] = np.degrees(roll)
self.get_logger().info('Current Pose: %s' % self.current_pose) print(timestamp,self.current_pose[0] ,self.current_pose[1])
#self.get_logger().info('Current Pose: %s' % self.current_pose)
def main(args=None): def main(args=None):
rclpy.init(args=args) rclpy.init(args=args)
...@@ -38,3 +52,6 @@ if __name__ == '__main__': ...@@ -38,3 +52,6 @@ if __name__ == '__main__':
main() main()
#builtin_interfaces/Time stamp
# int32 sec
# uint32 nanosec
colcon_ws/src/my_controller/my_controller/trial_controller.py
+ 45
− 121
View file @ af359afb
...@@ -7,99 +7,70 @@ from nav_msgs.msg import Odometry ...@@ -7,99 +7,70 @@ from nav_msgs.msg import Odometry
from scipy.spatial.transform import Rotation as R from scipy.spatial.transform import Rotation as R
import math import math
import matplotlib.path as mplPath from optitrack_multiplexer_ros2_msgs.msg import RigidBodyStamped
import matplotlib.pyplot as plt
import sys import sys
from functools import partial from functools import partial
class MoveRobotNode(Node): class MoveRobotNode(Node):
def __init__(self): def __init__(self):
super().__init__('move_robot_node')
"""
self.declare_parameter('x', 1.0)
self.declare_parameter('y', 1.0)
self.declare_parameter('theta', 0.0)
self.declare_parameter('velocity', 1.0)
"""
super().__init__('move_robot_node') super().__init__('move_robot_node')
#self.publisher_ = self.create_publisher(Twist, '/diff_cont/cmd_vel_unstamped', 10) #for differential robot #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, '/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, 'robot/cmd_vel', 10) #for kairos simulation
#self.publisher_ = self.create_publisher(Twist, 'kairosAB/cmd_vel', 10) #for kairosAB 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, '/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, '/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, '/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(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.current_pose = [0.02, 0.02, 0.02] self.current_pose = [0.0002, 0.0002, 0.0002]
self.goal_pose= [0.0005, 0.0005, 0.0005]
#self.current_pose =self.get_start_pose()
""" """
def get_start_pose(self):
def get_start_pose(self,msg): start_x = -float(input("Enter start y in optitrack coords: "))
start_y = -float(input("Enter start x in optitrack coords: "))
start_x =0.0 self.start_theta = float( input("Enter start theta from -π to π :"))
start_y =0.0
start_theta = 0.0
return [start_x, start_y, start_theta]
start_pose_str = self.get_parameter('start_pose').get_parameter_value().string_value
start_x, start_y, start_theta = map(float, start_pose_str.split())
self.start_theta = start_theta
return [start_x, start_y, self.start_theta] return [start_x, start_y, self.start_theta]
""" """
#function to update the current pose based on the odometry from gazebo #function to update the current pose based on the odometry from gazebo
def odom_callback(self, msg): 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[0] = msg.pose.pose.position.x self.current_pose[2] = roll
self.current_pose[1] = msg.pose.pose.position.y
orientation_q = msg.pose.pose.orientation
r = R.from_quat([orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w])
_, _, yaw = r.as_euler('xyz')
self.current_pose[2] = yaw
#function for moving the robot #function for moving the robot
def move_robot(self, goal_pose, velocity,obstacle): def move_robot(self, goal_pose, velocity):
obstacle_border = obstacle['border']
error_x = self.goal_pose[0] - self.current_pose[0]
error_y = self.goal_pose[1] - self.current_pose[1]
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 # Stage 1: Move the robot forward towards the goal
distance = (error_x**2 + error_y**2)**0.5 distance = (error_x**2 + error_y**2)**0.5
while distance >= 0.01: while distance >= 0.01:
error_x = goal_pose[0] - self.current_pose[0] error_x = self.goal_pose[0] - self.current_pose[0]
error_y = goal_pose[1] - self.current_pose[1] error_y = self.goal_pose[1] - self.current_pose[1]
distance = (error_x**2 + error_y**2)**0.5 distance = (error_x**2 + error_y**2)**0.5
print("Current pose x,y: ", self.current_pose[0], self.current_pose[1]) print("Current pose x,y,theta: ", self.current_pose)
print("distance left", distance) print("distance left", distance)
# direction towards the goal # direction towards the goal
direction_to_goal = [goal_pose[0] - self.current_pose[0], goal_pose[1] - self.current_pose[1]] direction_to_goal = [self.goal_pose[0] - self.current_pose[0], self.goal_pose[1] - self.current_pose[1]]
# instea of start_theta is now current_pose[2] # 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])] 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 between the current direction and the direction to the goal
...@@ -108,50 +79,20 @@ class MoveRobotNode(Node): ...@@ -108,50 +79,20 @@ class MoveRobotNode(Node):
# Normalize the angle to the range [-pi, pi] since gazebo is in that way # 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 angle_to_goal = (angle_to_goal + math.pi) % (2 * math.pi) - math.pi
path = mplPath.Path(obstacle_border)
if path.contains_point((self.current_pose[0], self.current_pose[1])):
# If the current pose is inside the obstacle, calculate the tangent vector along the obstacle border
print("Robot inside obstacle! Moving along the border.")
# Calculate the tangent vector along the obstacle border
tangent_vector = [obstacle_border[1][0] - obstacle_border[0][0], obstacle_border[1][1] - obstacle_border[0][1]]
# Rotate the tangent vector by 90 degrees to get a vector pointing inward
tangent_vector = [-tangent_vector[1], tangent_vector[0]]
# Normalize the tangent vector
tangent_magnitude = (tangent_vector[0]**2 + tangent_vector[1]**2)**0.5
tangent_vector = [tangent_vector[0] / tangent_magnitude, tangent_vector[1] / tangent_magnitude]
# Move the robot along the tangent vector
linear_velocity_x = velocity * tangent_vector[0]
linear_velocity_y = velocity * tangent_vector[1]
else:
# If the robot is outside the obstacle, move towards the goal
linear_velocity_x = velocity * math.cos(angle_to_goal) linear_velocity_x = velocity * math.cos(angle_to_goal)
linear_velocity_y = velocity * math.sin(angle_to_goal) linear_velocity_y = velocity * math.sin(angle_to_goal)
msg = Twist() msg = Twist()
msg.linear.x = linear_velocity_x msg.linear.x = linear_velocity_x
msg.linear.y= linear_velocity_y msg.linear.y= linear_velocity_y
if distance < 0.01: # threshold if the robot is close enough to the target orientation
if distance < 0.02: # threshold if the robot is close enough to the target orientation
print("first stage reached END") # stop rotating print("first stage reached END") # stop rotating
stop_msg = Twist() stop_msg = Twist()
stop_msg.linear.x = 0.0 stop_msg.linear.x = 0.0
stop_msg.angular.y =0.0 stop_msg.linear.y =0.0
stop_msg.angular.z =0.0
self.publisher_.publish(stop_msg) self.publisher_.publish(stop_msg)
break break
...@@ -162,13 +103,13 @@ class MoveRobotNode(Node): ...@@ -162,13 +103,13 @@ class MoveRobotNode(Node):
rclpy.spin_once(self) rclpy.spin_once(self)
# Stage 2: Rotate the robot to the goal orientation # Stage 2: Rotate the robot to the goal orientation
error_theta_final = goal_pose[2] - self.current_pose[2] error_theta_final = self.goal_pose[2] - self.current_pose[2]
error_theta_final = (error_theta_final + math.pi) % (2 * math.pi) - math.pi # normalize to [-pi, pi] 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 while abs(error_theta_final) > 0.01: # adjust threshold
angular_velocity = velocity * error_theta_final*1.5 angular_velocity = velocity * error_theta_final
print("error_theta_is",error_theta_final) print("error_theta_is",error_theta_final)
error_theta_final = goal_pose[2] - self.current_pose[2] error_theta_final = self.goal_pose[2] - self.current_pose[2]
error_theta_final = (error_theta_final + math.pi) % (2 * math.pi) - math.pi 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 if abs(error_theta_final) < 0.01: # if the robot is close enough to the target orientation
...@@ -197,29 +138,11 @@ def main(args=None): ...@@ -197,29 +138,11 @@ def main(args=None):
move_robot_node = MoveRobotNode() move_robot_node = MoveRobotNode()
self.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 π : "))]
goal_pose = [float(input("Enter goal x: ")), float(input("Enter goal y: ")), float(input("Enter goal theta from -π to π : "))]
velocity = float(input("Enter velocity: ")) velocity = float(input("Enter velocity: "))
""" move_robot_node.move_robot(goal_pose, velocity)
x = move_robot_node.get_parameter('x').get_parameter_value().double_value
y = move_robot_node.get_parameter('y').get_parameter_value().double_value
theta = move_robot_node.get_parameter('theta').get_parameter_value().double_value
velocity = move_robot_node.get_parameter('velocity').get_parameter_value().double_value
goal_pose=[float(x),float(y),float(theta)]
"""
obstacle = {'border': [(1, 1), (1, 2), (2, 2), (2, 1)]}
move_robot_node.move_robot(goal_pose,velocity,obstacle)
#move_robot_node.move_robot(goal_pose, velocity)
rclpy.spin(move_robot_node) rclpy.spin(move_robot_node)
move_robot_node.destroy_node() move_robot_node.destroy_node()
...@@ -227,3 +150,4 @@ def main(args=None): ...@@ -227,3 +150,4 @@ def main(args=None):
if __name__ == '__main__': if __name__ == '__main__':
main() main()
colcon_ws/src/my_controller/my_controller/trial_controller_backup.py → colcon_ws/src/my_controller/my_controller/trial_controller_goal.py
+ 7
− 6
View file @ af359afb
...@@ -23,10 +23,10 @@ class MoveRobotNode(Node): ...@@ -23,10 +23,10 @@ class MoveRobotNode(Node):
#self.subscription = self.create_subscription(Odometry, '/diff_cont/odom', self.odom_callback, 10) #differential #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, '/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(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/kairosAB',self.odom_callback,10) self.current_pose = [0.02, 0.02, 0.02]
self.current_pose = [0.0002, 0.0002, 0.0002]
#self.current_pose =self.get_start_pose() #self.current_pose =self.get_start_pose()
""" """
...@@ -58,7 +58,7 @@ class MoveRobotNode(Node): ...@@ -58,7 +58,7 @@ class MoveRobotNode(Node):
# Stage 1: Move the robot forward towards the goal # Stage 1: Move the robot forward towards the goal
distance = (error_x**2 + error_y**2)**0.5 distance = (error_x**2 + error_y**2)**0.5
while distance >= 0.01: while distance >= 0.02:
error_x = goal_pose[0] - self.current_pose[0] error_x = goal_pose[0] - self.current_pose[0]
error_y = goal_pose[1] - self.current_pose[1] error_y = goal_pose[1] - self.current_pose[1]
distance = (error_x**2 + error_y**2)**0.5 distance = (error_x**2 + error_y**2)**0.5
...@@ -84,13 +84,14 @@ class MoveRobotNode(Node): ...@@ -84,13 +84,14 @@ class MoveRobotNode(Node):
msg = Twist() msg = Twist()
msg.linear.x = linear_velocity_x msg.linear.x = linear_velocity_x
msg.linear.y= linear_velocity_y msg.linear.y= linear_velocity_y
if distance < 0.01: # threshold if the robot is close enough to the target orientation if distance < 0.02: # threshold if the robot is close enough to the target orientation
print("first stage reached END") # stop rotating print("first stage reached END") # stop rotating
stop_msg = Twist() stop_msg = Twist()
stop_msg.linear.x = 0.0 stop_msg.linear.x = 0.0
stop_msg.angular.y =0.0 stop_msg.linear.y =0.0
stop_msg.angular.z =0.0
self.publisher_.publish(stop_msg) self.publisher_.publish(stop_msg)
break break
......
colcon_ws/src/my_controller/my_controller/trial_controller_opti.py
+ 9
− 7
View file @ af359afb
...@@ -25,16 +25,16 @@ class MoveRobotNode(Node): ...@@ -25,16 +25,16 @@ class MoveRobotNode(Node):
super().__init__('move_robot_node')
#self.publisher_ = self.create_publisher(Twist, '/diff_cont/cmd_vel_unstamped', 10) #for differential robot #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, '/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, 'robot/cmd_vel', 10) #for kairos simulation
self.publisher_ = self.create_publisher(Twist, 'kairosAA/cmd_vel', 10) #for kairosAA real self.publisher_ = self.create_publisher(Twist, 'kairosAB/cmd_vel', 10) #for kairosAA real
#self.publisher_ = self.create_publisher(Twist, 'kairosAB/cmd_vel', 10) #for kairosAB 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, '/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, '/diff_cont/odom', self.odom_callback, 10) #differential
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(RigidBodyStamped,'/optitrack_multiplexer_node/rigid_body/kairosAB',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 #self.subscription = self.create_subscription(Odometry, '/robot/robotnik_base_control/odom', self.odom_callback, 10) #for kairos simulation
...@@ -44,8 +44,10 @@ class MoveRobotNode(Node): ...@@ -44,8 +44,10 @@ class MoveRobotNode(Node):
#self.subscription = self.create_subscription(Odometry, '/kairosAB/robotnik_base_control/odom', self.odom_callback, 10) #for kairos AB real #self.subscription = self.create_subscription(Odometry, '/kairosAB/robotnik_base_control/odom', self.odom_callback, 10) #for kairos AB real
self.current_pose = [0.02, 0.02, 0.02] #self.current_pose = [0.02, 0.02, 0.02]
self.goal_pose = [0.05, 0.05, 0.05] #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]
def pose_callback(self,msg): def pose_callback(self,msg):
...@@ -92,7 +94,7 @@ class MoveRobotNode(Node): ...@@ -92,7 +94,7 @@ class MoveRobotNode(Node):
# Stage 1: Move the robot forward towards the goal # Stage 1: Move the robot forward towards the goal
distance = (error_x**2 + error_y**2)**0.5 distance = (error_x**2 + error_y**2)**0.5
while distance >= 0.01: while distance >= 1.40: #previous 0.01
...@@ -122,7 +124,7 @@ class MoveRobotNode(Node): ...@@ -122,7 +124,7 @@ class MoveRobotNode(Node):
msg.linear.x = linear_velocity_x msg.linear.x = linear_velocity_x
msg.linear.y= linear_velocity_y msg.linear.y= linear_velocity_y
if distance < 0.3: # threshold so the robot doesnt collide if distance < 1.40: # threshold original 0.01
print("first stage reached END") # stop rotating print("first stage reached END") # stop rotating
......
colcon_ws/src/my_controller/my_controller/trial_controller_opti_improved.py 0 → 100755
+ 201
− 0
View file @ af359afb
#!/usr/bin/env python3
import time
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Pose,Point,Quaternion
from scipy.spatial.transform import Rotation as R
import math
import matplotlib.path as mplPath
import matplotlib.pyplot as plt
import sys
from functools import partial
from optitrack_multiplexer_ros2_msgs.msg import RigidBodyStamped
#from optitrack_multiplexer_ros2_msgs.msg import RigidBodyStamped
class MoveRobotNode(Node):
def __init__(self):
super().__init__('move_robot_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, '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/kairosAB',self.odom_callback,10)
#self.subscription = self.create_subscription(Odometry, '/robot/robotnik_base_control/odom', self.odom_callback, 10) #for kairos simulation
self.subscription = self.create_subscription(RigidBodyStamped,'/optitrack_multiplexer_node/rigid_body/testbody',self.pose_callback,10)
#self.subscription=self.create_subscription(Pose, 'Rigid_1/pose', self.pose_callback,10)
#self.subscription = self.create_subscription(Odometry, '/kairosAB/robotnik_base_control/odom', self.odom_callback, 10) #for kairos AB real
#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.02]
self.goal_pose = [0.6003, 0.06003, 0.05]
def pose_callback(self,msg):
self.goal_pose[0] = msg.rigid_body.pose.position.y
self.goal_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])
_, _, yaw = r.as_euler('XYZ')
self.goal_pose[2] = yaw
#function to update the current pose based on the odometry from gazebo
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])
_, _, yaw = r.as_euler('XYZ')
self.current_pose[2] = yaw
#function for moving the robot
"""
def move_robot(self, velocity, obstacle):
obstacle_border = obstacle['border']
"""
def move_robot(self,velocity):
error_x = self.goal_pose[0] - self.current_pose[0]
error_y = self.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.6: #previous 0.01
error_x = self.goal_pose[0] - self.current_pose[0]
error_y = self.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 = [self.goal_pose[0] - self.current_pose[0], self.goal_pose[1] - self.current_pose[1]]
# instea of start_theta is now current_pose[2]
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.6: # threshold original 0.01
print("first stage reached END") # stop rotating
stop_msg = Twist()
stop_msg.linear.x = 0.0
stop_msg.linear.y = 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
time.sleep(2)
error_theta_final = self.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*0.5
print("error_theta_is",error_theta_final)
error_theta_final = self.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()
velocity = float(input("Enter velocity: "))
obstacle = {'border': [(1, 1), (1, 2), (2, 2), (2, 1)]}
move_robot_node.move_robot(velocity)
#move_robot_node.move_robot(velocity,obstacle)
rclpy.spin(move_robot_node)
move_robot_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
colcon_ws/src/my_controller/setup.py
+ 3
− 1
View file @ af359afb
...@@ -24,12 +24,14 @@ setup( ...@@ -24,12 +24,14 @@ setup(
"trial_controller_diff_opti= my_controller.trial_controller_diff_opti:main", "trial_controller_diff_opti= my_controller.trial_controller_diff_opti:main",
"trial_controller_diff=my_controller.trial_controller_diff:main", "trial_controller_diff=my_controller.trial_controller_diff:main",
"trial_controller_opti= my_controller.trial_controller_opti:main", "trial_controller_opti= my_controller.trial_controller_opti:main",
"trial_controller_opti_improved= my_controller.trial_controller_opti_improved:main",
"trial_controller_backup_original= my_controller.trial_controller_backup_original:main", "trial_controller_backup_original= my_controller.trial_controller_backup_original:main",
"trial_controller_backup= my_controller.trial_controller_backup:main", "trial_controller_goal= my_controller.trial_controller_goal:main",
"evaluation_node=my_controller.evaluation_node:main", "evaluation_node=my_controller.evaluation_node:main",
"simplenode=my_controller.simplenode:main", "simplenode=my_controller.simplenode:main",
"node_markers=my_controller.node_markers:main", "node_markers=my_controller.node_markers:main",
"evaluation_node_odom=my_controller.evaluation_node_odom:main", "evaluation_node_odom=my_controller.evaluation_node_odom:main",
"evaluation_node_odom_final=my_controller.evaluation_node_odom_final:main",
"evaluation_node_slam=my_controller.evaluation_node_slam:main", "evaluation_node_slam=my_controller.evaluation_node_slam:main",
"optitrack_node=my_controller.optitrack_node:main" "optitrack_node=my_controller.optitrack_node:main"
], ],
......
entrypoint_scripts/entrypoint_controller.sh
+ 6
− 1
View file @ af359afb
...@@ -12,10 +12,15 @@ source /colcon_ws/install/setup.bash ...@@ -12,10 +12,15 @@ source /colcon_ws/install/setup.bash
#ros2 run my_controller evaluation_node_odom #ros2 run my_controller evaluation_node_odom
#ros2 run my_controller evaluation_node_odom_final
#ros2 run my_controller evaluation_node_slam
#ros2 run my_controller simplenode #ros2 run my_controller simplenode
#ros2 run my_controller trial_controller_backup_original #ros2 run my_controller trial_controller_backup_original
#ros2 run my_controller trial_controller_diff_opti #ros2 run my_controller trial_controller_diff_opti
#ros2 run my_controller trial_controller_diff #ros2 run my_controller trial_controller_diff
#ros2 run my_controller trial_controller_backup #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 node_markers #ros2 run my_controller node_markers
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment