Skip to content

Commit a12d854

Browse files
SaltyLemonzSaltyLemonz
committed
first commit
0 parents  commit a12d854

File tree

12 files changed

+402
-0
lines changed

12 files changed

+402
-0
lines changed

‎.vscode/c_cpp_properties.json

Lines changed: 71 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,71 @@
1+
{
2+
"configurations": [
3+
{
4+
"browse": {
5+
"databaseFilename": "${default}",
6+
"limitSymbolsToIncludedHeaders": false
7+
},
8+
"includePath": [
9+
"/home/sid/ros2_ws/install/zed_interfaces/include/**",
10+
"/home/sid/ros2_ws/install/velocity_controllers/include/**",
11+
"/home/sid/ros2_ws/install/tut_interfaces/include/**",
12+
"/home/sid/ros2_ws/install/tricycle_steering_controller/include/**",
13+
"/home/sid/ros2_ws/install/tricycle_controller/include/**",
14+
"/home/sid/ros2_ws/install/ros2_control_demo_example_8/include/**",
15+
"/home/sid/ros2_ws/install/transmission_interface/include/**",
16+
"/home/sid/ros2_ws/install/test_control/include/**",
17+
"/home/sid/ros2_ws/install/bicycle_steering_controller/include/**",
18+
"/home/sid/ros2_ws/install/ackermann_steering_controller/include/**",
19+
"/home/sid/ros2_ws/install/steering_controllers_library/include/**",
20+
"/home/sid/ros2_ws/install/ros2_control_demo_example_9/include/**",
21+
"/home/sid/ros2_ws/install/ros2_control_demo_example_7/include/**",
22+
"/home/sid/ros2_ws/install/ros2_control_demo_example_6/include/**",
23+
"/home/sid/ros2_ws/install/ros2_control_demo_example_5/include/**",
24+
"/home/sid/ros2_ws/install/ros2_control_demo_example_4/include/**",
25+
"/home/sid/ros2_ws/install/ros2_control_demo_example_3/include/**",
26+
"/home/sid/ros2_ws/install/ros2_control_demo_example_2/include/**",
27+
"/home/sid/ros2_ws/install/ros2_control_demo_example_14/include/**",
28+
"/home/sid/ros2_ws/install/ros2_control_demo_example_12/include/**",
29+
"/home/sid/ros2_ws/install/ros2_control_demo_example_10/include/**",
30+
"/home/sid/ros2_ws/install/ros2_control_demo_example_1/include/**",
31+
"/home/sid/ros2_ws/install/please_work/include/**",
32+
"/home/sid/ros2_ws/install/gz_ros2_control/include/**",
33+
"/home/sid/ros2_ws/install/gazebo_ros2_control/include/**",
34+
"/home/sid/ros2_ws/install/controller_manager/include/**",
35+
"/home/sid/ros2_ws/install/ros2_control_test_assets/include/**",
36+
"/home/sid/ros2_ws/install/robot_control_system/include/**",
37+
"/home/sid/ros2_ws/install/range_sensor_broadcaster/include/**",
38+
"/home/sid/ros2_ws/install/position_controllers/include/**",
39+
"/home/sid/ros2_ws/install/admittance_controller/include/**",
40+
"/home/sid/ros2_ws/install/joint_trajectory_controller/include/**",
41+
"/home/sid/ros2_ws/install/joint_state_broadcaster/include/**",
42+
"/home/sid/ros2_ws/install/imu_sensor_broadcaster/include/**",
43+
"/home/sid/ros2_ws/install/gripper_controllers/include/**",
44+
"/home/sid/ros2_ws/install/effort_controllers/include/**",
45+
"/home/sid/ros2_ws/install/forward_command_controller/include/**",
46+
"/home/sid/ros2_ws/install/force_torque_sensor_broadcaster/include/**",
47+
"/home/sid/ros2_ws/install/diff_drive_controller/include/**",
48+
"/home/sid/ros2_ws/install/control_toolbox/include/**",
49+
"/home/sid/ros2_ws/install/realtime_tools/include/**",
50+
"/home/sid/ros2_ws/install/polygon_plugins/include/**",
51+
"/home/sid/ros2_ws/install/polygon_base/include/**",
52+
"/home/sid/ros2_ws/install/omnidirectional_controllers/include/**",
53+
"/home/sid/ros2_ws/install/multi_msg/include/**",
54+
"/home/sid/ros2_ws/install/kinematics_interface/include/**",
55+
"/home/sid/ros2_ws/install/joint_limits/include/**",
56+
"/home/sid/ros2_ws/install/controller_interface/include/**",
57+
"/home/sid/ros2_ws/install/hardware_interface/include/**",
58+
"/home/sid/ros2_ws/install/controller_manager_msgs/include/**",
59+
"/home/sid/ros2_ws/install/control_msgs/include/**",
60+
"/opt/ros/iron/include/**",
61+
"/usr/include/**"
62+
],
63+
"name": "ROS",
64+
"intelliSenseMode": "gcc-x64",
65+
"compilerPath": "/usr/bin/gcc",
66+
"cStandard": "gnu11",
67+
"cppStandard": "c++14"
68+
}
69+
],
70+
"version": 4
71+
}

‎.vscode/settings.json

Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
{
2+
"python.autoComplete.extraPaths": [
3+
"/home/sid/ros2_ws/install/zed_interfaces/lib/python3.10/site-packages",
4+
"/home/sid/ros2_ws/install/tut_interfaces/lib/python3.10/site-packages",
5+
"/home/sid/ros2_ws/build/rqt_joint_trajectory_controller",
6+
"/home/sid/ros2_ws/install/rqt_joint_trajectory_controller/lib/python3.10/site-packages",
7+
"/home/sid/ros2_ws/build/rqt_controller_manager",
8+
"/home/sid/ros2_ws/install/rqt_controller_manager/lib/python3.10/site-packages",
9+
"/home/sid/ros2_ws/build/ros2controlcli",
10+
"/home/sid/ros2_ws/install/ros2controlcli/lib/python3.10/site-packages",
11+
"/home/sid/ros2_ws/build/ros2_controllers_test_nodes",
12+
"/home/sid/ros2_ws/install/ros2_controllers_test_nodes/lib/python3.10/site-packages",
13+
"/home/sid/ros2_ws/install/r2_controller/lib/python3.10/site-packages",
14+
"/home/sid/ros2_ws/install/project_eva_controller/lib/python3.10/site-packages",
15+
"/home/sid/ros2_ws/install/controller_manager/lib/python3.10/site-packages",
16+
"/home/sid/ros2_ws/build/r2_test",
17+
"/home/sid/ros2_ws/install/r2_test/lib/python3.10/site-packages",
18+
"/home/sid/ros2_ws/build/r2_drive",
19+
"/home/sid/ros2_ws/install/r2_drive/lib/python3.10/site-packages",
20+
"/home/sid/ros2_ws/build/project_eva_drive",
21+
"/home/sid/ros2_ws/install/project_eva_drive/lib/python3.10/site-packages",
22+
"/home/sid/ros2_ws/build/piRobot_base",
23+
"/home/sid/ros2_ws/install/piRobot_base/lib/python3.10/site-packages",
24+
"/home/sid/ros2_ws/build/my_sphere_pkg",
25+
"/home/sid/ros2_ws/install/my_sphere_pkg/lib/python3.10/site-packages",
26+
"/home/sid/ros2_ws/install/my_first_pkg/lib/python3.10/site-packages",
27+
"/home/sid/ros2_ws/build/my_environment_pkg",
28+
"/home/sid/ros2_ws/install/my_environment_pkg/lib/python3.10/site-packages",
29+
"/home/sid/ros2_ws/build/my_doosan_pkg",
30+
"/home/sid/ros2_ws/install/my_doosan_pkg/lib/python3.10/site-packages",
31+
"/home/sid/ros2_ws/build/msg_pubsub",
32+
"/home/sid/ros2_ws/install/msg_pubsub/lib/python3.10/site-packages",
33+
"/home/sid/ros2_ws/build/custom_drive",
34+
"/home/sid/ros2_ws/install/custom_drive/lib/python3.10/site-packages",
35+
"/home/sid/ros2_ws/install/multi_msg/lib/python3.10/site-packages",
36+
"/home/sid/ros2_ws/install/controller_manager_msgs/lib/python3.10/site-packages",
37+
"/home/sid/ros2_ws/install/control_msgs/lib/python3.10/site-packages",
38+
"/opt/ros/iron/lib/python3.10/site-packages"
39+
],
40+
"python.analysis.extraPaths": [
41+
"/home/sid/ros2_ws/install/zed_interfaces/lib/python3.10/site-packages",
42+
"/home/sid/ros2_ws/install/tut_interfaces/lib/python3.10/site-packages",
43+
"/home/sid/ros2_ws/build/rqt_joint_trajectory_controller",
44+
"/home/sid/ros2_ws/install/rqt_joint_trajectory_controller/lib/python3.10/site-packages",
45+
"/home/sid/ros2_ws/build/rqt_controller_manager",
46+
"/home/sid/ros2_ws/install/rqt_controller_manager/lib/python3.10/site-packages",
47+
"/home/sid/ros2_ws/build/ros2controlcli",
48+
"/home/sid/ros2_ws/install/ros2controlcli/lib/python3.10/site-packages",
49+
"/home/sid/ros2_ws/build/ros2_controllers_test_nodes",
50+
"/home/sid/ros2_ws/install/ros2_controllers_test_nodes/lib/python3.10/site-packages",
51+
"/home/sid/ros2_ws/install/r2_controller/lib/python3.10/site-packages",
52+
"/home/sid/ros2_ws/install/project_eva_controller/lib/python3.10/site-packages",
53+
"/home/sid/ros2_ws/install/controller_manager/lib/python3.10/site-packages",
54+
"/home/sid/ros2_ws/build/r2_test",
55+
"/home/sid/ros2_ws/install/r2_test/lib/python3.10/site-packages",
56+
"/home/sid/ros2_ws/build/r2_drive",
57+
"/home/sid/ros2_ws/install/r2_drive/lib/python3.10/site-packages",
58+
"/home/sid/ros2_ws/build/project_eva_drive",
59+
"/home/sid/ros2_ws/install/project_eva_drive/lib/python3.10/site-packages",
60+
"/home/sid/ros2_ws/build/piRobot_base",
61+
"/home/sid/ros2_ws/install/piRobot_base/lib/python3.10/site-packages",
62+
"/home/sid/ros2_ws/build/my_sphere_pkg",
63+
"/home/sid/ros2_ws/install/my_sphere_pkg/lib/python3.10/site-packages",
64+
"/home/sid/ros2_ws/install/my_first_pkg/lib/python3.10/site-packages",
65+
"/home/sid/ros2_ws/build/my_environment_pkg",
66+
"/home/sid/ros2_ws/install/my_environment_pkg/lib/python3.10/site-packages",
67+
"/home/sid/ros2_ws/build/my_doosan_pkg",
68+
"/home/sid/ros2_ws/install/my_doosan_pkg/lib/python3.10/site-packages",
69+
"/home/sid/ros2_ws/build/msg_pubsub",
70+
"/home/sid/ros2_ws/install/msg_pubsub/lib/python3.10/site-packages",
71+
"/home/sid/ros2_ws/build/custom_drive",
72+
"/home/sid/ros2_ws/install/custom_drive/lib/python3.10/site-packages",
73+
"/home/sid/ros2_ws/install/multi_msg/lib/python3.10/site-packages",
74+
"/home/sid/ros2_ws/install/controller_manager_msgs/lib/python3.10/site-packages",
75+
"/home/sid/ros2_ws/install/control_msgs/lib/python3.10/site-packages",
76+
"/opt/ros/iron/lib/python3.10/site-packages"
77+
]
78+
}

‎package.xml

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>r2</name>
5+
<version>0.0.0</version>
6+
<description>TODO: Package description</description>
7+
<maintainer email="[email protected]">sid</maintainer>
8+
<license>TODO: License declaration</license>
9+
10+
<test_depend>ament_copyright</test_depend>
11+
<test_depend>ament_flake8</test_depend>
12+
<test_depend>ament_pep257</test_depend>
13+
<test_depend>python3-pytest</test_depend>
14+
15+
<export>
16+
<build_type>ament_python</build_type>
17+
</export>
18+
</package>

‎r2/Joy_twist.py

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
import rclpy
2+
from rclpy.node import Node
3+
from sensor_msgs.msg import Joy
4+
from geometry_msgs.msg import Twist
5+
6+
class JoyToTwist(Node):
7+
8+
def __init__(self):
9+
super().__init__('joy_to_twist')
10+
self.subscription = self.create_subscription(
11+
Joy,
12+
'/joy', # Replace with your Joy topic name
13+
self.joy_callback,
14+
10)
15+
self.publisher = self.create_publisher(
16+
Twist,
17+
'piRobot/cmd_vel', # Replace with your Twist topic name
18+
10)
19+
20+
def joy_callback(self, msg):
21+
twist_msg = Twist()
22+
23+
twist_msg.linear.x = msg.axes[1]
24+
twist_msg.linear.y = msg.axes[0]
25+
twist_msg.angular.z = msg.axes[3]
26+
self.publisher.publish(twist_msg)
27+
28+
def main(args=None):
29+
rclpy.init(args=args)
30+
joy_to_twist_node = JoyToTwist()
31+
rclpy.spin(joy_to_twist_node)
32+
joy_to_twist_node.destroy_node()
33+
rclpy.shutdown()
34+
35+
if __name__ == '__main__':
36+
main()

‎r2/Uart.py

Lines changed: 95 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,95 @@
1+
import rclpy
2+
import serial
3+
from rclpy.node import Node
4+
from geometry_msgs.msg import Twist
5+
import numpy as np
6+
import math
7+
from threading import Lock
8+
9+
class Uart(Node):
10+
def __init__(self):
11+
super().__init__('uart_node')
12+
13+
self.ROBOT_WIDTH = 0.229
14+
self.ROBOT_LENGTH = 0.550
15+
self.WHEEL_RADIUS = 0.127
16+
self.WHEEL_DIS_FROM_CENTER = 0.229
17+
self.WHEEL_GEOMETRY = 0.550 + 0.229 # Height and width in meters
18+
19+
self.pico_pwm = 254
20+
self.max_pwm = 20 / 100 * self.pico_pwm
21+
22+
self.loop_rate=0.020 # miliseconds
23+
self.MotorPWM:[int,int,int,int]=[0,0,0,0]
24+
25+
26+
self.mutex = Lock()
27+
self.ser = serial.Serial('/dev/ttyACM0', 115200)
28+
self.uart_timer=self.create_timer(self.loop_rate,self.uart_timer_callback)
29+
self.subscription = self.create_subscription(Twist,'piRobot/cmd_vel',self.twist_callback,10)
30+
# self.encoder=self.create_publis++++++++++++++++++++++++++++++++++++++++her(Motor_encoder,'/drive/raw_encoder',10)
31+
32+
33+
def twist_callback(self, msg):
34+
Vx = msg.linear.x
35+
# Vy = msg.linear.y
36+
Vy = msg.angular.z
37+
# omega = msg.angular.z
38+
omega = msg.linear.y
39+
40+
self.MotorPWM[0] = (Vx - Vy - self.WHEEL_GEOMETRY * omega) / self.WHEEL_RADIUS
41+
42+
self.MotorPWM[1] = (Vx + Vy + self.WHEEL_GEOMETRY * omega) / self.WHEEL_RADIUS
43+
44+
self.MotorPWM[2] = (Vx + Vy - self.WHEEL_GEOMETRY * omega) / self.WHEEL_RADIUS
45+
46+
self.MotorPWM[3] = (Vx - Vy + self.WHEEL_GEOMETRY * omega) / self.WHEEL_RADIUS
47+
48+
49+
# self.MotorPWM[0] = (Vx - Vy - self.WHEEL_DIS_FROM_CENTER * omega) / self.WHEEL_RADIUS # Forward left
50+
51+
# self.MotorPWM[1] = (Vx + Vy + self.WHEEL_DIS_FROM_CENTER * omega) / self.WHEEL_RADIUS # Forward right
52+
53+
# self.MotorPWM[2] = (Vx + Vy - self.WHEEL_DIS_FROM_CENTER * omega) / self.WHEEL_RADIUS # backward left
54+
55+
# self.MotorPWM[3] = (Vx - Vy + self.WHEEL_DIS_FROM_CENTER * omega) / self.WHEEL_RADIUS # backward right
56+
57+
self.MotorPWM[0] = -int(self.MotorPWM[0] / 7.87 * self.max_pwm)
58+
self.MotorPWM[1] = -int(self.MotorPWM[1] / 7.87 * self.max_pwm)
59+
self.MotorPWM[2] = int(self.MotorPWM[2] / 7.87 * self.max_pwm)
60+
self.MotorPWM[3] = int(self.MotorPWM[3] / 7.87 * self.max_pwm)
61+
62+
self.get_logger().info('Motor PWM values: Motor1={}, Motor2={}, Motor3={}, Motor4={}'.format(self.MotorPWM[0], self.MotorPWM[1], self.MotorPWM[2], self.MotorPWM[3]))
63+
64+
def uart_timer_callback(self):
65+
66+
self.mutex.acquire()
67+
68+
try:
69+
70+
dataToSend = f"{self.MotorPWM[0]},{self.MotorPWM[1]},{self.MotorPWM[2]},{self.MotorPWM[3]},\n".encode('utf-8')
71+
self.ser.write('m'.encode('utf-8'))
72+
self.ser.write(dataToSend)
73+
74+
# response = self.ser.readline().strip()
75+
# self.get_logger().info(f"sent {dataToSend} rec {response}")
76+
# response=0
77+
78+
self.ser.write('e'.encode('utf-8'))
79+
# response = self.ser.readline().strip() # read(3) will read 3 bytes
80+
# print(f"revived {response}")
81+
82+
# todo: publish encoder data here
83+
# encoder_data:[int,int,int]=[int(num) for num in response.split()]
84+
85+
finally:
86+
self.mutex.release()
87+
88+
def main(args=None):
89+
rclpy.init(args=args)
90+
node = Uart()
91+
rclpy.spin(node)
92+
rclpy.shutdown()
93+
94+
if __name__ == '__main__':
95+
main()

‎r2/__init__.py

Whitespace-only changes.

‎resource/r2

Whitespace-only changes.

‎setup.cfg

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
[develop]
2+
script_dir=$base/lib/r2
3+
[install]
4+
install_scripts=$base/lib/r2

‎setup.py

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
from setuptools import find_packages, setup
2+
3+
package_name = 'r2'
4+
5+
setup(
6+
name=package_name,
7+
version='0.0.0',
8+
packages=find_packages(exclude=['test']),
9+
data_files=[
10+
('share/ament_index/resource_index/packages',
11+
['resource/' + package_name]),
12+
('share/' + package_name, ['package.xml']),
13+
],
14+
install_requires=['setuptools'],
15+
zip_safe=True,
16+
maintainer='sid',
17+
maintainer_email='[email protected]',
18+
description='TODO: Package description',
19+
license='TODO: License declaration',
20+
tests_require=['pytest'],
21+
entry_points={
22+
'console_scripts': [
23+
'Uart = r2.Uart:main'
24+
'joy2twist = r2.Joy_twist:main'
25+
],
26+
},
27+
)

‎test/test_copyright.py

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
# Copyright 2015 Open Source Robotics Foundation, Inc.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
from ament_copyright.main import main
16+
import pytest
17+
18+
19+
# Remove the `skip` decorator once the source file(s) have a copyright header
20+
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
21+
@pytest.mark.copyright
22+
@pytest.mark.linter
23+
def test_copyright():
24+
rc = main(argv=['.', 'test'])
25+
assert rc == 0, 'Found errors'

0 commit comments

Comments
 (0)