From 93b59e4d129e81cb38fdf0443218932e7abb717b Mon Sep 17 00:00:00 2001 From: saurabh borse Date: Fri, 21 Jun 2024 16:44:20 +0200 Subject: [PATCH] pre-commit: fixed buil dissues --- .LICENSE | 1 + .../launch/machine_base_description.launch.py | 1 + .../launch/machine_cap_description.launch.py | 1 + .../machine_delivery_description.launch.py | 1 + .../launch/machine_ring_description.launch.py | 1 + .../machine_storage_description.launch.py | 1 + .../config/omnidirectional_controllers.yaml | 2 +- .../robotino_simulation/robotino_driver.hpp | 2 + .../launch/robotino_cluster.launch.py | 1 + .../launch/robotino_controller.launch.py | 1 + .../launch/robotino_simulation.launch.py | 1 + .../robotinocluster_simulation.launch.py | 1 + .../launch/webots_cluster.launch.py | 1 + .../robotino_simulation/__init__.py | 1 + .../extract_rosbag_data.py | 110 +++++++++--------- .../robotino_simulation/mps_publisher.py | 1 + .../robotino_driver_plugin.py | 2 + .../robotinobase1_driver_plugin.py | 2 + .../robotinobase2_driver_plugin.py | 2 + .../robotinobase3_driver_plugin.py | 2 + robotino_simulation/src/robotino_driver.cpp | 36 +++--- robotino_simulation/test/test_copyright.py | 1 + robotino_simulation/test/test_flake8.py | 1 + robotino_simulation/test/test_pep257.py | 1 + 24 files changed, 107 insertions(+), 67 deletions(-) create mode 100644 .LICENSE diff --git a/.LICENSE b/.LICENSE new file mode 100644 index 0000000..904e473 --- /dev/null +++ b/.LICENSE @@ -0,0 +1 @@ +Licensed under GPLv2. See LICENSE file. Copyright Carologistics. diff --git a/mps_description/launch/machine_base_description.launch.py b/mps_description/launch/machine_base_description.launch.py index 056cff3..e876617 100755 --- a/mps_description/launch/machine_base_description.launch.py +++ b/mps_description/launch/machine_base_description.launch.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Author: Saurabh Borse(saurabh.borse@alumni.fh-aachen.de) # MIT License # Copyright (c) 2023 Saurabh Borse diff --git a/mps_description/launch/machine_cap_description.launch.py b/mps_description/launch/machine_cap_description.launch.py index d508746..7d70d3d 100755 --- a/mps_description/launch/machine_cap_description.launch.py +++ b/mps_description/launch/machine_cap_description.launch.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Author: Saurabh Borse(saurabh.borse@alumni.fh-aachen.de) # MIT License # Copyright (c) 2023 Saurabh Borse diff --git a/mps_description/launch/machine_delivery_description.launch.py b/mps_description/launch/machine_delivery_description.launch.py index 0b3c94f..ed6ead6 100755 --- a/mps_description/launch/machine_delivery_description.launch.py +++ b/mps_description/launch/machine_delivery_description.launch.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Author: Saurabh Borse(saurabh.borse@alumni.fh-aachen.de) # MIT License # Copyright (c) 2023 Saurabh Borse diff --git a/mps_description/launch/machine_ring_description.launch.py b/mps_description/launch/machine_ring_description.launch.py index c16d809..b38b999 100755 --- a/mps_description/launch/machine_ring_description.launch.py +++ b/mps_description/launch/machine_ring_description.launch.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Author: Saurabh Borse(saurabh.borse@alumni.fh-aachen.de) # MIT License # Copyright (c) 2023 Saurabh Borse diff --git a/mps_description/launch/machine_storage_description.launch.py b/mps_description/launch/machine_storage_description.launch.py index 886815f..69ec471 100755 --- a/mps_description/launch/machine_storage_description.launch.py +++ b/mps_description/launch/machine_storage_description.launch.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Author: Saurabh Borse(saurabh.borse@alumni.fh-aachen.de) # MIT License # Copyright (c) 2023 Saurabh Borse diff --git a/robotino_simulation/config/omnidirectional_controllers.yaml b/robotino_simulation/config/omnidirectional_controllers.yaml index c937aa5..2c4696c 100644 --- a/robotino_simulation/config/omnidirectional_controllers.yaml +++ b/robotino_simulation/config/omnidirectional_controllers.yaml @@ -24,7 +24,7 @@ omnidirectional_controller: publish_rate: 50.0 odom_frame_id: odom base_frame_id: base_link - pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] odom_numeric_integration_method: runge_kutta2 diff --git a/robotino_simulation/include/robotino_simulation/robotino_driver.hpp b/robotino_simulation/include/robotino_simulation/robotino_driver.hpp index 1cf8e88..8db423b 100644 --- a/robotino_simulation/include/robotino_simulation/robotino_driver.hpp +++ b/robotino_simulation/include/robotino_simulation/robotino_driver.hpp @@ -1,3 +1,5 @@ +// Licensed under GPLv2. See LICENSE file. Copyright Carologistics. + #ifndef ROBOTINO_DRIVER_WEBOTS_PLUGIN_HPP #define ROBOTINO_DRIVER_WEBOTS_PLUGIN_HPP diff --git a/robotino_simulation/launch/robotino_cluster.launch.py b/robotino_simulation/launch/robotino_cluster.launch.py index 55785e3..4186c48 100755 --- a/robotino_simulation/launch/robotino_cluster.launch.py +++ b/robotino_simulation/launch/robotino_cluster.launch.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Author: Saurabh Borse(saurabh.borse@alumni.fh-aachen.de) # MIT License # Copyright (c) 2023 Saurabh Borse diff --git a/robotino_simulation/launch/robotino_controller.launch.py b/robotino_simulation/launch/robotino_controller.launch.py index 9e3cdbe..942306e 100755 --- a/robotino_simulation/launch/robotino_controller.launch.py +++ b/robotino_simulation/launch/robotino_controller.launch.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Author: Saurabh Borse(saurabh.borse@alumni.fh-aachen.de) # MIT License # Copyright (c) 2023 Saurabh Borse diff --git a/robotino_simulation/launch/robotino_simulation.launch.py b/robotino_simulation/launch/robotino_simulation.launch.py index b6b0948..64b5c4e 100755 --- a/robotino_simulation/launch/robotino_simulation.launch.py +++ b/robotino_simulation/launch/robotino_simulation.launch.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Author: Saurabh Borse(saurabh.borse@alumni.fh-aachen.de) # MIT License # Copyright (c) 2023 Saurabh Borse diff --git a/robotino_simulation/launch/robotinocluster_simulation.launch.py b/robotino_simulation/launch/robotinocluster_simulation.launch.py index 5dfedaf..2ae34ca 100755 --- a/robotino_simulation/launch/robotinocluster_simulation.launch.py +++ b/robotino_simulation/launch/robotinocluster_simulation.launch.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Author: Saurabh Borse(saurabh.borse@alumni.fh-aachen.de) # MIT License # Copyright (c) 2023 Saurabh Borse diff --git a/robotino_simulation/launch/webots_cluster.launch.py b/robotino_simulation/launch/webots_cluster.launch.py index 7ef2770..015e6d7 100755 --- a/robotino_simulation/launch/webots_cluster.launch.py +++ b/robotino_simulation/launch/webots_cluster.launch.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Author: Saurabh Borse(saurabh.borse@alumni.fh-aachen.de) # MIT License # Copyright (c) 2023 Saurabh Borse diff --git a/robotino_simulation/robotino_simulation/__init__.py b/robotino_simulation/robotino_simulation/__init__.py index e69de29..c5c80e9 100644 --- a/robotino_simulation/robotino_simulation/__init__.py +++ b/robotino_simulation/robotino_simulation/__init__.py @@ -0,0 +1 @@ +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. diff --git a/robotino_simulation/robotino_simulation/extract_rosbag_data.py b/robotino_simulation/robotino_simulation/extract_rosbag_data.py index 53858d2..bc9f789 100755 --- a/robotino_simulation/robotino_simulation/extract_rosbag_data.py +++ b/robotino_simulation/robotino_simulation/extract_rosbag_data.py @@ -1,38 +1,36 @@ #!/usr/bin/python3 +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. +from decimal import Decimal +import matplotlib.pyplot as plt +import pandas as pd import rclpy -from rclpy.node import Node -from nav_msgs.msg import Path -from tf2_msgs.msg import TFMessage from rcl_interfaces.msg import Log -from decimal import Decimal -from builtin_interfaces.msg import Time -import pandas as pd -import matplotlib.pyplot as plt +from rclpy.node import Node from tf2_ros import TransformException from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener -from PIL import Image + class DataExtractorNode(Node): def __init__(self): - super().__init__('data_extractor') + super().__init__("data_extractor") self.declare_parameter("namespace", "robotinobase1") self.namespace = self.get_parameter("namespace").get_parameter_value().string_value self.goal_start_time = None self.goal_end_time = None # Subscribe to rosout topic - self.rosout_sub = self.create_subscription(Log, '/rosbag_rosout', self.rosout_callback, 10) + self.rosout_sub = self.create_subscription(Log, "/rosbag_rosout", self.rosout_callback, 10) # Subscribe to namespace plan topic - #self.plan_sub = self.create_subscription(Path, self.namespace+'/plan', self.plan_callback, 10) + # self.plan_sub = self.create_subscription(Path, self.namespace+'/plan', self.plan_callback, 10) # Call on_timer function every second self.timer = self.create_timer(0.1, self.on_timer) # Declare and acquire `target_frame` parameter - self.target_frame = self.namespace +'/base_link' + self.target_frame = self.namespace + "/base_link" self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) @@ -40,12 +38,12 @@ def __init__(self): self.machine_pose = { "C-BS-I": (5.50, 2.50, 90), "C-BS-O": (5.50, 4.50, 270), - "C-RS2-I": (3.50, 5.50,180), - "C-RS2-O": (5.50, 5.50,0), - "C-CS1-I": (0.50, 5.50,180), - "C-CS1-O": (2.50, 5.50,0), - "C-CS2-I": (0.50, 1.50,180), - "C-CS2-O": (2.50, 1.50,0), + "C-RS2-I": (3.50, 5.50, 180), + "C-RS2-O": (5.50, 5.50, 0), + "C-CS1-I": (0.50, 5.50, 180), + "C-CS1-O": (2.50, 5.50, 0), + "C-CS2-I": (0.50, 1.50, 180), + "C-CS2-O": (2.50, 1.50, 0), "C-DS-I": (0.50, 0.50, 180), "C-DS-O": (2.50, 0.50, 0), "C-RS1-I": (1.50, 4.50, 270), @@ -54,12 +52,12 @@ def __init__(self): "C-SS-O": (4.50, 3.50, 270), "M-BS-I": (-5.50, 2.50, 90), "M-BS-O": (-5.50, 4.50, 270), - "M-RS2-I": (-3.50, 5.50,0), - "M-RS2-O": (-5.50, 5.50,180), - "M-CS1-I": (-0.50, 5.50,0), - "M-CS1-O": (-2.50, 5.50,180), - "M-CS2-I": (-0.50, 1.50,0), - "M-CS2-O": (-2.50, 1.50,180), + "M-RS2-I": (-3.50, 5.50, 0), + "M-RS2-O": (-5.50, 5.50, 180), + "M-CS1-I": (-0.50, 5.50, 0), + "M-CS1-O": (-2.50, 5.50, 180), + "M-CS2-I": (-0.50, 1.50, 0), + "M-CS2-O": (-2.50, 1.50, 180), "M-DS-I": (-0.50, 0.50, 0), "M-DS-O": (-2.50, 0.50, 180), "M-RS1-I": (-1.50, 4.50, 270), @@ -68,7 +66,17 @@ def __init__(self): "M-SS-O": (-4.50, 3.50, 270), } - self.columns = ["pose_start_x", "pose_start_y", "pose_end_x", "pose_end_y", "machine_start", "machine_end", "time_start", "time_end", "time_diff"] + self.columns = [ + "pose_start_x", + "pose_start_y", + "pose_end_x", + "pose_end_y", + "machine_start", + "machine_end", + "time_start", + "time_end", + "time_diff", + ] self.table = pd.DataFrame(columns=self.columns) self.columns_ = ["pose_x", "pose_y"] @@ -85,7 +93,7 @@ def __init__(self): self.record_data = False def rosout_callback(self, msg): - if msg.name == self.namespace + '.bt_navigator': + if msg.name == self.namespace + ".bt_navigator": if self.msg_check(msg.msg, "Begin navigating"): self.goal_start_time = self.get_time(msg) self.pose_data = self.machine_pose_check(msg.msg) @@ -98,14 +106,14 @@ def rosout_callback(self, msg): "pose_end_y": [self.pose_data[3]], "machine_start": [self.machine_start], "machine_end": [self.machine_end], - "time_start": [self.goal_start_time] + "time_start": [self.goal_start_time], } self.table = pd.concat([self.table, pd.DataFrame(data_row)], ignore_index=True) if msg.msg == "Goal succeeded": self.goal_end_time = self.get_time(msg) self.table.at[self.counter, "time_end"] = self.goal_end_time - #self.table.at[self.counter, "time_diff"] = self.goal_end_time - self.goal_start_time + # self.table.at[self.counter, "time_diff"] = self.goal_end_time - self.goal_start_time self.machine_start = self.machine_end self.counter += 1 if self.counter in [1, 5, 9, 13, 17]: @@ -117,10 +125,9 @@ def rosout_callback(self, msg): # self.table.to_csv(self.namespace+"_pose_data.csv", index=False) # self.get_logger().info(f'Pose data saved, check rosbag status and shutdown the node') - def msg_check(self, message, check): words = message.split() - first_two = ' '.join(words[:2]) + first_two = " ".join(words[:2]) return first_two == check def machine_pose_check(self, message): @@ -132,7 +139,7 @@ def machine_pose_check(self, message): for key, value in self.machine_pose.items(): if end_pose_x == value[0] and end_pose_y == value[1]: machine_end = key - self.get_logger().info(f'machine end: {machine_end}') + self.get_logger().info(f"machine end: {machine_end}") return [start_pose_x, start_pose_y, end_pose_x, end_pose_y, machine_end] def get_time(self, msg): @@ -141,47 +148,44 @@ def get_time(self, msg): def on_timer(self): if self.record_data: try: - t = self.tf_buffer.lookup_transform('map', self.target_frame, rclpy.time.Time()) + t = self.tf_buffer.lookup_transform("map", self.target_frame, rclpy.time.Time()) except TransformException as ex: - self.get_logger().info( - f'Could not transform {self.target_frame} to map: {ex}') + self.get_logger().info(f"Could not transform {self.target_frame} to map: {ex}") return self.robot_pose_x.append(t.transform.translation.x) self.robot_pose_y.append(t.transform.translation.y) - data_row = { - "pose_x": [t.transform.translation.x], - "pose_y": [t.transform.translation.y] - } + data_row = {"pose_x": [t.transform.translation.x], "pose_y": [t.transform.translation.y]} self.table_ = pd.concat([self.table_, pd.DataFrame(data_row)], ignore_index=True) - if self.plot_data: quo, rem = divmod(self.counter, 4) plt.figure(quo) plt.clf() - plt.plot(self.robot_pose_x, self.robot_pose_y,'-b') - plt.xlabel('X_Position') - plt.ylabel('Y_Position') - plt.title(self.namespace+'_iteration'+str(quo)+'_followed_Path') + plt.plot(self.robot_pose_x, self.robot_pose_y, "-b") + plt.xlabel("X_Position") + plt.ylabel("Y_Position") + plt.title(self.namespace + "_iteration" + str(quo) + "_followed_Path") plt.grid(True) plt.xlim(-6, 6) plt.ylim(0, 6) - plt.gca().set_aspect('equal') + plt.gca().set_aspect("equal") - plt.text(self.robot_pose_x[0], self.robot_pose_y[0], 'Start', fontsize=12, color='green', ha='right') - plt.text(self.robot_pose_x[-1], self.robot_pose_y[-1], 'End', fontsize=12, color='red', ha='right') + plt.text(self.robot_pose_x[0], self.robot_pose_y[0], "Start", fontsize=12, color="green", ha="right") + plt.text(self.robot_pose_x[-1], self.robot_pose_y[-1], "End", fontsize=12, color="red", ha="right") - plt.savefig(self.namespace+'_iteration'+str(quo)+'_followed_path'+'.png') # Save the plot as PNG file - self.table_.to_csv(self.namespace+'_iteration'+str(quo)+'_pose_data.csv', index=False) + plt.savefig( + self.namespace + "_iteration" + str(quo) + "_followed_path" + ".png" + ) # Save the plot as PNG file + self.table_.to_csv(self.namespace + "_iteration" + str(quo) + "_pose_data.csv", index=False) self.columns_ = ["pose_x", "pose_y"] self.table_ = pd.DataFrame(columns=self.columns_) - self.get_logger().info(f'Plotting followed path data for iteration {quo}') + self.get_logger().info(f"Plotting followed path data for iteration {quo}") plt.close() @@ -221,16 +225,18 @@ def on_timer(self): # self.plan_pose_x = [] # self.plan_pose_y = [] + def main(args=None): rclpy.init(args=args) node = DataExtractorNode() try: - node.get_logger().info('Beginning data extraction node, shut down with CTRL-C') + node.get_logger().info("Beginning data extraction node, shut down with CTRL-C") rclpy.spin(node) except KeyboardInterrupt: - node.get_logger().info('Keyboard interrupt, shutting down.\n') + node.get_logger().info("Keyboard interrupt, shutting down.\n") node.destroy_node() rclpy.shutdown() -if __name__ == '__main__': + +if __name__ == "__main__": main() diff --git a/robotino_simulation/robotino_simulation/mps_publisher.py b/robotino_simulation/robotino_simulation/mps_publisher.py index e77a370..359d1b2 100755 --- a/robotino_simulation/robotino_simulation/mps_publisher.py +++ b/robotino_simulation/robotino_simulation/mps_publisher.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Author: Saurabh Borse(saurabh.borse@alumni.fh-aachen.de) # MIT License # Copyright (c) 2023 Saurabh Borse diff --git a/robotino_simulation/robotino_simulation/robotino_driver_plugin.py b/robotino_simulation/robotino_simulation/robotino_driver_plugin.py index a14ba62..78e59b6 100755 --- a/robotino_simulation/robotino_simulation/robotino_driver_plugin.py +++ b/robotino_simulation/robotino_simulation/robotino_driver_plugin.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Author: Saurabh Borse(saurabh.borse@alumni.fh-aachen.de) # MIT License # Copyright (c) 2023 Saurabh Borse diff --git a/robotino_simulation/robotino_simulation/robotinobase1_driver_plugin.py b/robotino_simulation/robotino_simulation/robotinobase1_driver_plugin.py index d824ef2..c40e3d4 100755 --- a/robotino_simulation/robotino_simulation/robotinobase1_driver_plugin.py +++ b/robotino_simulation/robotino_simulation/robotinobase1_driver_plugin.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Author: Saurabh Borse(saurabh.borse@alumni.fh-aachen.de) # MIT License # Copyright (c) 2023 Saurabh Borse diff --git a/robotino_simulation/robotino_simulation/robotinobase2_driver_plugin.py b/robotino_simulation/robotino_simulation/robotinobase2_driver_plugin.py index 3130f15..f24c007 100755 --- a/robotino_simulation/robotino_simulation/robotinobase2_driver_plugin.py +++ b/robotino_simulation/robotino_simulation/robotinobase2_driver_plugin.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Author: Saurabh Borse(saurabh.borse@alumni.fh-aachen.de) # MIT License # Copyright (c) 2023 Saurabh Borse diff --git a/robotino_simulation/robotino_simulation/robotinobase3_driver_plugin.py b/robotino_simulation/robotino_simulation/robotinobase3_driver_plugin.py index c6a30e9..f9826b6 100755 --- a/robotino_simulation/robotino_simulation/robotinobase3_driver_plugin.py +++ b/robotino_simulation/robotino_simulation/robotinobase3_driver_plugin.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Author: Saurabh Borse(saurabh.borse@alumni.fh-aachen.de) # MIT License # Copyright (c) 2023 Saurabh Borse diff --git a/robotino_simulation/src/robotino_driver.cpp b/robotino_simulation/src/robotino_driver.cpp index fa7dee8..d2be412 100644 --- a/robotino_simulation/src/robotino_driver.cpp +++ b/robotino_simulation/src/robotino_driver.cpp @@ -1,3 +1,5 @@ +// Licensed under GPLv2. See LICENSE file. Copyright Carologistics. + #include "robotino_simulation/robotino_driver.hpp" #include @@ -116,7 +118,7 @@ void RobotinoDriver::init( publish_odom(time_stamp_, time_diff_); } last_sample_time_ = curr_time_; - } + } { std::lock_guard lock(vel_msg_mutex_); this->cmd_vel_msg.linear.x = 0.0; @@ -141,7 +143,7 @@ void RobotinoDriver::read_data() { } void RobotinoDriver::publish_data() { - //read_data(); + // read_data(); double curr_time = wb_robot_get_time(); // Convert double time to seconds and nanoseconds @@ -151,10 +153,10 @@ void RobotinoDriver::publish_data() { time_stamp.sec = sec; time_stamp.nanosec = nanosec; - if (odom_source_ == "gps"){ + if (odom_source_ == "gps") { publish_odom_from_sensors(time_stamp); } - //publish_odom_from_sensors(time_stamp); + // publish_odom_from_sensors(time_stamp); publish_joint_state(time_stamp); publish_ir(time_stamp); publish_laser(time_stamp); @@ -247,12 +249,16 @@ void RobotinoDriver::publish_odom(const TimeStamp &time_stamp, auto velocity = inverse_kinematics(w0, w1, w2); double phi = prev_odom_omega_ + (velocity[2] * time_diff); - double x = prev_odom_x_ + (((velocity[0] * cos(phi)) - (velocity[1] * sin(phi))) * time_diff); - //double x = prev_odom_x_ + (velocity[0]*time_diff); - //RCLCPP_INFO(node_->get_logger(), "Position_x: %f ", x); - double y = prev_odom_y_ + (((velocity[0] * sin(phi)) + (velocity[1] * cos(phi))) * time_diff); - //double y = prev_odom_y_ + (velocity[1]*time_diff); - //RCLCPP_INFO(node_->get_logger(), "Position_y: %f", y); + double x = + prev_odom_x_ + + (((velocity[0] * cos(phi)) - (velocity[1] * sin(phi))) * time_diff); + // double x = prev_odom_x_ + (velocity[0]*time_diff); + // RCLCPP_INFO(node_->get_logger(), "Position_x: %f ", x); + double y = + prev_odom_y_ + + (((velocity[0] * sin(phi)) + (velocity[1] * cos(phi))) * time_diff); + // double y = prev_odom_y_ + (velocity[1]*time_diff); + // RCLCPP_INFO(node_->get_logger(), "Position_y: %f", y); std::vector q = {0.0, 0.0, sin(phi / 2.), cos(phi / 2.)}; prev_odom_x_ = x; @@ -333,7 +339,7 @@ std::vector RobotinoDriver::kinematics() { double v_y = this->cmd_vel_msg.linear.y; double omega = this->cmd_vel_msg.angular.z; - double k = (60.0*GEER_RATIO*0.009375)/(2.0*M_PI*WHEEL_RADIUS); + double k = (60.0 * GEER_RATIO * 0.009375) / (2.0 * M_PI * WHEEL_RADIUS); omega = omega * WHEEL_DISTANCE; double m1 = (((sqrt(3.) / 2.) * v_x) - (0.5 * v_y) - omega) * k; @@ -346,13 +352,15 @@ std::vector RobotinoDriver::kinematics() { std::vector RobotinoDriver::inverse_kinematics(const double &w0, const double &w1, const double &w2) { - double k_inv = (2*M_PI*WHEEL_RADIUS)/(60*GEER_RATIO*0.009375); + double k_inv = (2 * M_PI * WHEEL_RADIUS) / (60 * GEER_RATIO * 0.009375); double vx = ((w0 - w2) / sqrt(3.0)) * k_inv; - double vy = (-((1.0 / 3.0) * w0) + ((2.0 / 3.0) * w1)- ((1.0 / 3.0) * w2)) * k_inv; + double vy = + (-((1.0 / 3.0) * w0) + ((2.0 / 3.0) * w1) - ((1.0 / 3.0) * w2)) * k_inv; double const_val = 3.0 * WHEEL_DISTANCE; double omega = (-((1.0 / const_val) * w0) - ((1.0 / const_val) * w1) - - ((1.0 / const_val) * w2)) * k_inv; + ((1.0 / const_val) * w2)) * + k_inv; return {vx, vy, omega}; } diff --git a/robotino_simulation/test/test_copyright.py b/robotino_simulation/test/test_copyright.py index 9b530ba..fc233b6 100644 --- a/robotino_simulation/test/test_copyright.py +++ b/robotino_simulation/test/test_copyright.py @@ -1,3 +1,4 @@ +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); diff --git a/robotino_simulation/test/test_flake8.py b/robotino_simulation/test/test_flake8.py index 528eab7..58468f4 100644 --- a/robotino_simulation/test/test_flake8.py +++ b/robotino_simulation/test/test_flake8.py @@ -1,3 +1,4 @@ +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Copyright 2017 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); diff --git a/robotino_simulation/test/test_pep257.py b/robotino_simulation/test/test_pep257.py index 4bc0366..945cdbf 100644 --- a/robotino_simulation/test/test_pep257.py +++ b/robotino_simulation/test/test_pep257.py @@ -1,3 +1,4 @@ +# Licensed under GPLv2. See LICENSE file. Copyright Carologistics. # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License");