Skip to content

Commit 67a57d9

Browse files
mleegwtmleegwt
authored
Merge pull request #21 from mleegwt/simulation
Simulation
2 parents fb86da9 + 837bed4 commit 67a57d9

File tree

7 files changed

+488
-30
lines changed

7 files changed

+488
-30
lines changed

‎ros_ws/src/boatcontrol/boatcontrol/boatcontrolnode.py‎

Lines changed: 6 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@
44
import math
55
import rclpy
66
from rclpy.node import Node
7-
from geometry_msgs.msg import TwistStamped
7+
from geometry_msgs.msg import Twist
88

99
# Initialize serial communication for actuator control
1010
serial_port = "/dev/ttyUSB0" # Update with your port
@@ -21,7 +21,7 @@ class BoatControlNode(Node):
2121

2222
def __init__(self):
2323
super().__init__('boat_control_node')
24-
self.cmd_sub = self.create_subscription(TwistStamped, '/cmd_vel', self.props_callback, 10)
24+
self.cmd_sub = self.create_subscription(Twist, '/cmd_vel', self.props_callback, 10)
2525

2626
def start(self):
2727
time.sleep(0.5)
@@ -30,20 +30,18 @@ def start(self):
3030
response = ser.readline().decode().strip() # Read response from the actuator
3131
self.get_logger().info(f"Received: {response}")
3232
self.send_enable_command()
33-
time.sleep(0.1)
34-
self.send_speed_command(30)
3533

3634
# Controls the propellors for the boat via duty cycle control.
3735
def props_callback(self, cmd_vel):
3836
self.send_enable_command()
3937

4038
# Converting twist message to differntial drive control, includes clipping
4139
# This is an electronic speed controller
42-
v = cmd_vel.twist.linear.x # (m/s)
43-
w = cmd_vel.twist.angular.z # (rad/s)
40+
v = cmd_vel.linear.x # (m/s)
41+
w = cmd_vel.angular.z # (rad/s)
4442

45-
B = 0.3 # (m)
46-
K = 0.05 # To be determined!
43+
B = 0.3 # (m)
44+
K = 1 # To be determined!
4745
left = (v - w*B/2) / K
4846
right = (v + w*B/2) / K
4947

‎ros_ws/src/navigation/config/karaburan.xacro‎

Lines changed: 174 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,174 @@
1+
<?xml version="1.0" ?>
2+
3+
<robot name="karaburan" xmlns:xacro="http://www.ros.org/wiki/xacro">
4+
<material name="black">
5+
<color rgba="0.9 0.9 0.9 0.8" />
6+
</material>
7+
8+
<!-- Box inertia (COM op link-origin) -->
9+
<xacro:macro name="inertial_box" params="m x y z">
10+
<inertial>
11+
<origin xyz="0 0 ${z*0.4}" rpy="0 0 0"/>
12+
<mass value="${m}"/>
13+
<inertia
14+
ixx="${m*(y*y+z*z)/12.0}" ixy="0" ixz="0"
15+
iyy="${m*(x*x+z*z)/12.0}" iyz="0"
16+
izz="${m*(x*x+y*y)/12.0}"/>
17+
</inertial>
18+
</xacro:macro>
19+
20+
<xacro:macro name="inertial_cylinder_z" params="m r l">
21+
<inertial>
22+
<origin xyz="0 0 0" rpy="0 0 0"/>
23+
<mass value="${m}"/>
24+
<inertia
25+
ixx="${m*(3*r*r + l*l)/12.0}" ixy="0" ixz="0"
26+
iyy="${m*(3*r*r + l*l)/12.0}" iyz="0"
27+
izz="${0.5*m*r*r}"/>
28+
</inertial>
29+
</xacro:macro>
30+
31+
<link name="base_footprint"/>
32+
<link name="base_link">
33+
<visual>
34+
<geometry>
35+
<box size="0.6 0.4 0.2" />
36+
</geometry>
37+
<origin xyz="0 0 0.1" rpy="0 0 0" />
38+
<material name="black" />
39+
</visual>
40+
<collision>
41+
<geometry>
42+
<box size="0.6 0.4 0.2" />
43+
</geometry>
44+
<origin xyz="0 0 0.1" rpy="0 0 0" />
45+
</collision>
46+
<xacro:inertial_box m="4.0" x="0.6" y="0.4" z="0.2" />
47+
</link>
48+
49+
<link name="imu_link">
50+
<visual>
51+
<geometry>
52+
<box size="0.03 0.03 0.001"/>
53+
</geometry>
54+
<origin xyz="0 0 0" rpy="0 0 0" />
55+
<material name="black" />
56+
</visual>
57+
58+
<sensor name="imu_sensor" type="imu">
59+
<always_on>1</always_on>
60+
<update_rate>1</update_rate>
61+
<visualize>true</visualize>
62+
<topic>imu</topic>
63+
</sensor>
64+
</link>
65+
66+
<link name="gps">
67+
<visual>
68+
<geometry>
69+
<box size="0.03 0.03 0.01"/>
70+
</geometry>
71+
<origin xyz="0 0 0" rpy="0 0 0" />
72+
<material name="black" />
73+
</visual>
74+
</link>
75+
76+
<link name="lidar">
77+
<visual>
78+
<geometry>
79+
<cylinder radius="0.05" length="0.05" />
80+
</geometry>
81+
<origin xyz="0 0 0" rpy="0 0 0" />
82+
<material name="black" />
83+
</visual>
84+
<collision>
85+
<geometry>
86+
<box size="0.1 0.1 0.05" />
87+
</geometry>
88+
<origin xyz="0 0 0" rpy="0 0 0" />
89+
<material name="black" />
90+
</collision>
91+
92+
<sensor name="lidar" type="gpu_lidar">
93+
<always_on>true</always_on>
94+
<visualize>true</visualize>
95+
<update_rate>5</update_rate>
96+
<topic>scan</topic>
97+
<gz_frame_id>lidar_link</gz_frame_id>
98+
<lidar>
99+
<scan>
100+
<horizontal>
101+
<samples>360</samples>
102+
<resolution>1.000000</resolution>
103+
<min_angle>0.000000</min_angle>
104+
<max_angle>6.280000</max_angle>
105+
</horizontal>
106+
</scan>
107+
<range>
108+
<min>0.120000</min>
109+
<max>3.5</max>
110+
<resolution>0.015000</resolution>
111+
</range>
112+
<noise>
113+
<type>gaussian</type>
114+
<mean>0.0</mean>
115+
<stddev>0.01</stddev>
116+
</noise>
117+
</lidar>
118+
</sensor>
119+
</link>
120+
121+
<joint name="base_footprint_to_base_link" type="fixed">
122+
<parent link="base_footprint"/>
123+
<child link="base_link"/>
124+
<origin xyz="0 0 0" rpy="0 0 0"/>
125+
</joint>
126+
127+
<joint name="base_imu_joint" type="fixed">
128+
<parent link="base_link" />
129+
<child link="imu_link" />
130+
<origin xyz="0.25 0 0.201" rpy="0 0 0" />
131+
</joint>
132+
133+
<joint name="base_gps_joint" type="fixed">
134+
<parent link="base_link" />
135+
<child link="gps" />
136+
<origin xyz="-0.25 0 0.201" rpy="0 0 0" />
137+
</joint>
138+
139+
<xacro:macro name="prop_link" params="prefix">
140+
<link name="${prefix}_prop">
141+
<visual>
142+
<geometry>
143+
<cylinder radius="0.04" length="0.025" />
144+
</geometry>
145+
<origin xyz="0 0 0" rpy="0 1.57 0" />
146+
<material name="black" />
147+
</visual>
148+
<xacro:inertial_cylinder_z m="0.05" r="0.03" l="0.01"/>
149+
</link>
150+
</xacro:macro>
151+
152+
<xacro:prop_link prefix="left" />
153+
<xacro:prop_link prefix="right" />
154+
155+
<joint name="base_lidar_joint" type="fixed">
156+
<parent link="base_link" />
157+
<child link="lidar" />
158+
<origin xyz="0 0 0.225" rpy="0 0 0" />
159+
</joint>
160+
161+
<joint name="base_left_prop_joint" type="continuous">
162+
<parent link="base_link" />
163+
<child link="left_prop" />
164+
<origin xyz="-0.25 0.1 -0.045" rpy="0 0 0" />
165+
<axis xyz="1 0 0" />
166+
</joint>
167+
168+
<joint name="base_right_prop_joint" type="continuous">
169+
<parent link="base_link" />
170+
<child link="right_prop" />
171+
<origin xyz="-0.25 -0.1 -0.045" rpy="0 0 0" />
172+
<axis xyz="1 0 0" />
173+
</joint>
174+
</robot>

‎ros_ws/src/navigation/config/karaburan_boat.sdf‎

Lines changed: 128 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,128 @@
1+
<?xml version="1.0"?>
2+
<sdf version="1.10">
3+
<model name="karaburan">
4+
<static>false</static>
5+
<pose>0 0 0 0 0 0</pose>
6+
7+
<link name="hull">
8+
<!-- Ondervlak van de doos precies op het link-origin -->
9+
<collision name="hull_col">
10+
<geometry><box><size>0.6 0.4 0.2</size></box></geometry>
11+
<pose>0 0 0.1 0 0 0</pose>
12+
</collision>
13+
<visual name="hull_vis">
14+
<geometry><box><size>0.6 0.4 0.2</size></box></geometry>
15+
<pose>0 0 0.1 0 0 0</pose>
16+
</visual>
17+
18+
<!-- Massa/inertie van de romp; COM iets lager dan midden (0.07) -->
19+
<inertial>
20+
<pose>0 0 -0.02 0 0 0</pose>
21+
<mass>4.0</mass>
22+
<inertia>
23+
<ixx>0.0666667</ixx>
24+
<iyy>0.1333333</iyy>
25+
<izz>0.1733333</izz>
26+
<ixy>0</ixy><ixz>0</ixz><iyz>0</iyz>
27+
</inertia>
28+
</inertial>
29+
<velocity_decay>
30+
<linear>0.02</linear>
31+
<angular>0.15</angular>
32+
</velocity_decay>
33+
</link>
34+
35+
<!-- LEFT PROPELLER (link + joint) -->
36+
<link name="left_prop">
37+
<inertial>
38+
<pose>0 0 0 0 0 0</pose>
39+
<mass>0.05</mass>
40+
<inertia>
41+
<ixx>1e-5</ixx><iyy>1e-5</iyy><izz>1e-5</izz>
42+
<ixy>0</ixy><ixz>0</ixz><iyz>0</iyz>
43+
</inertia>
44+
</inertial>
45+
<visual name="left_prop_vis">
46+
<geometry><cylinder><radius>0.04</radius><length>0.02</length></cylinder></geometry>
47+
<!-- achter de romp (x<0), bakboord (y>0), op halve hoogte z=0.10 -->
48+
<pose>-0.35 0.18 0.10 0 0 0</pose>
49+
</visual>
50+
<!-- GEEN collision -> geen extra drijfkracht -->
51+
</link>
52+
53+
<joint name="left_prop_joint" type="revolute">
54+
<parent>hull</parent>
55+
<child>left_prop</child>
56+
<!-- as = +X in child-frame: “vooruit” is +thrust -->
57+
<pose>-0.35 0.18 0.10 0 0 0</pose>
58+
<axis>
59+
<xyz>1 0 0</xyz>
60+
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
61+
</axis>
62+
</joint>
63+
64+
<!-- RIGHT PROPELLER -->
65+
<link name="right_prop">
66+
<inertial>
67+
<pose>0 0 0 0 0 0</pose>
68+
<mass>0.05</mass>
69+
<inertia>
70+
<ixx>1e-5</ixx><iyy>1e-5</iyy><izz>1e-5</izz>
71+
<ixy>0</ixy><ixz>0</ixz><iyz>0</iyz>
72+
</inertia>
73+
</inertial>
74+
<visual name="right_prop_vis">
75+
<geometry><cylinder><radius>0.04</radius><length>0.02</length></cylinder></geometry>
76+
<pose>-0.35 -0.18 0.10 0 0 0</pose>
77+
</visual>
78+
</link>
79+
80+
<joint name="right_prop_joint" type="revolute">
81+
<parent>hull</parent>
82+
<child>right_prop</child>
83+
<pose>-0.35 -0.18 0.10 0 0 0</pose>
84+
<axis>
85+
<xyz>1 0 0</xyz>
86+
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
87+
</axis>
88+
</joint>
89+
90+
<!-- Thruster plugins: 1 per joint -->
91+
<plugin filename="gz-sim-thruster-system" name="gz::sim::systems::Thruster">
92+
<namespace>karaburan</namespace>
93+
<joint_name>left_prop_joint</joint_name>
94+
<fluid_density>1000</fluid_density>
95+
<propeller_diameter>0.06</propeller_diameter>
96+
<thrust_coefficient>0.0004</thrust_coefficient>
97+
<!-- standaard: cmd_thrust (N), joint wordt visueel gesponnen -->
98+
</plugin>
99+
100+
<plugin filename="gz-sim-thruster-system" name="gz::sim::systems::Thruster">
101+
<namespace>karaburan</namespace>
102+
<joint_name>right_prop_joint</joint_name>
103+
<fluid_density>1000</fluid_density>
104+
<propeller_diameter>0.06</propeller_diameter>
105+
<thrust_coefficient>0.0004</thrust_coefficient>
106+
</plugin>
107+
108+
109+
<plugin name="gz::sim::systems::Hydrodynamics" filename="gz-sim-hydrodynamics-system">
110+
<link_name>hull</link_name>
111+
<water_density>998</water_density>
112+
<disable_coriolis>true</disable_coriolis>
113+
<disable_added_mass>true</disable_added_mass>
114+
115+
<xDotU>0</xDotU> <yDotV>0</yDotV> <zDotW>0</zDotW>
116+
<kDotP>0</kDotP> <mDotQ>0</mDotQ> <nDotR>0</nDotR>
117+
118+
<xU>-2.0</xU> <yV>-3.0</yV> <zW>-80.0</zW>
119+
120+
<xUabsU>-0.6</xUabsU> <yVabsV>-1.0</yVabsV> <zWabsW>-9.0</zWabsW>
121+
122+
<kP>-4.0</kP> <kPabsP>-10.0</kPabsP> <!-- roll -->
123+
<mQ>-4.0</mQ> <mQabsQ>-10.0</mQabsQ> <!-- pitch -->
124+
<nR>-2.0</nR> <nRabsR>-3.0</nRabsR> <!-- yaw -->
125+
</plugin>
126+
</model>
127+
</sdf>
128+

0 commit comments

Comments
 (0)