diff --git a/.github/workflows/update-ghcr.yml b/.github/workflows/update-ghcr.yml index d884c2bc..6336f010 100644 --- a/.github/workflows/update-ghcr.yml +++ b/.github/workflows/update-ghcr.yml @@ -12,6 +12,7 @@ permissions: jobs: build-and-push: + if: ${{ github.repository == 'KTH-SML/svea' }} runs-on: ubuntu-latest steps: - name: Checkout code diff --git a/docker/Dockerfile b/docker/Dockerfile index b8d092d8..f9942408 100755 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -20,7 +20,7 @@ RUN apt-get update -y # This is for any added ROS packages COPY src ./src -COPY requirements.txt ./ +COPY entrypoint requirements.txt ./ RUN rosdep install \ --rosdistro $ROSDISTRO \ --from-paths src \ diff --git a/docker/Dockerfile.base b/docker/Dockerfile.base index ee4b0ed7..77c5238e 100755 --- a/docker/Dockerfile.base +++ b/docker/Dockerfile.base @@ -69,7 +69,7 @@ RUN mkdir -p /var/run/sshd # Bash reminder: # username="${creds%%:*}" # remove everything after the first ':' # password="${creds#*:}" # remove everything before the first ':' -RUN echo "root:${USER_CREDENTIALS#*:}" | chpasswd +RUN echo "${USER_CREDENTIALS%%:*}:${USER_CREDENTIALS#*:}" | chpasswd # SSH config: allow root + password login (keep pubkey auth on too) RUN sed -ri 's/^#?PasswordAuthentication .*/PasswordAuthentication yes/' /etc/ssh/sshd_config && \ @@ -99,6 +99,12 @@ RUN if [ -f src/mocap4ros2_qualisys/dependency_repos.repos ]; then \ vcs import src < src/mocap4ros2_qualisys/dependency_repos.repos; \ fi +# Zenoh +RUN echo "deb [trusted=yes] https://download.eclipse.org/zenoh/debian-repo/ /" | tee -a /etc/apt/sources.list > /dev/null && \ + apt update -y && \ + apt install --no-install-recommends -y zenoh-bridge-ros2dds && \ + rm -rf /var/lib/apt/lists/* + # Install dependencies via rosdep and pip RUN apt-get update -y && \ rosdep update \ @@ -141,4 +147,4 @@ RUN chmod +x ./entrypoint ENTRYPOINT ["./entrypoint"] -CMD ["bash"] +CMD ["bash"] \ No newline at end of file diff --git a/entrypoint b/entrypoint index 6a236f23..04255c87 100755 --- a/entrypoint +++ b/entrypoint @@ -24,6 +24,6 @@ if [ "$MICROROS_DISABLE_SHM" = "1" ] ; then fi # To build anything that has been added by volume... -colcon build --symlink-install +colcon build --symlink-install || true exec "$@" diff --git a/requirements.txt b/requirements.txt index 973c93a8..8cb58e80 100755 --- a/requirements.txt +++ b/requirements.txt @@ -8,7 +8,8 @@ # You can read more here: https://pip.pypa.io/en/stable/reference/requirements-file-format/#requirements-file-format numpy -matplotlib +matplotlib # deprecated +pillow # This is for MPC casadi==3.6.7 diff --git a/src/el2425/launch/traffic_light.xml b/src/el2425/launch/traffic_light.xml new file mode 100644 index 00000000..dcfd58fc --- /dev/null +++ b/src/el2425/launch/traffic_light.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/src/el2425/package.xml b/src/el2425/package.xml new file mode 100644 index 00000000..c4ef871b --- /dev/null +++ b/src/el2425/package.xml @@ -0,0 +1,16 @@ + + + + el2425 + 0.0.0 + The el2425 package + Kaj Munhoz Arfvidsson + TODO: License declaration + + svea_core + rclpy + + + ament_python + + diff --git a/src/el2425/resource/el2425 b/src/el2425/resource/el2425 new file mode 100644 index 00000000..e69de29b diff --git a/src/el2425/scripts/traffic_light.py b/src/el2425/scripts/traffic_light.py new file mode 100755 index 00000000..19f641fe --- /dev/null +++ b/src/el2425/scripts/traffic_light.py @@ -0,0 +1,107 @@ +#! /usr/bin/env python3 + +# imports +import random + +from rclpy.clock import Clock, Duration +from std_msgs.msg import String, Float32 + +from svea_core import rosonic as rx +from svea_core.interfaces import ShowMarker + +class traffic_light(rx.Node): + + ## Constants + + GREEN_TIME = 5. # [s] + YELLOW_TIME = 2. # [s] + RED_TIME = 5. # [s] + + # dictionary for states + TRANSITIONS = { + 'Rd': 'YG', # Red -> Yellow-Green + 'YG': 'Gr', # Yellow-Green -> Green + 'Gr': 'YR', # Green -> Yellow-Red + 'YR': 'Rd', # Yellow-Red -> Red + } + + ## Parameters + + rate = rx.Parameter(10) + alpha = rx.Parameter(1.0) + + pos_x = rx.Parameter(0.0) + pos_y = rx.Parameter(0.0) + + ## Publishers + + state_pub = rx.Publisher(String, '~/state') + time_pub = rx.Publisher(Float32, '~/time_left') + + ## Interfaces + + marker = ShowMarker() + + ## Methods + + def on_startup(self): + + assert 0 <= self.alpha <= min(self.RED_TIME, self.GREEN_TIME) + + self.switch('Rd') # initial state is red + self._loop_tmr = self.create_timer(1.0 / self.rate, self.loop) + + def loop(self): + + now = Clock().now() + + # calculate time remaining for current state + time_left = (self._time0 + self._delta - now).nanoseconds / 1e9 + + # if time has elapsed, then switch to next state + if time_left <= 0: + self.switch() + + # publish current state and time remaining + msg = String() + msg.data = self._state + self.state_pub.publish(msg) + + msg = Float32() + msg.data = time_left + self.time_pub.publish(msg) + + # visualization for the lights + self.visualize_traffic_lights() + + def switch(self, to=None): + """ + Change the traffic light to the next state + """ + self._state = (to if to is not None else + self.TRANSITIONS[self._state]) + + delta = (self.RED_TIME if self._state == 'Rd' else + self.GREEN_TIME if self._state == 'Gr' else + self.YELLOW_TIME) + if self._state in ('Rd', 'Gr'): + delta += random.uniform(-self.alpha, +self.alpha) + + # delta determines how long each state lasts + self._delta = Duration(seconds=delta) + + self._time0 = Clock().now() + + def visualize_traffic_lights(self): + """Publish the positions of traffic lights for visualization in RViz.""" + + color = ('red' if self._state == 'Rd' else + 'green' if self._state == 'Gr' else + 'yellow' if self._state in ('YG', 'YR') else + 'black') + + self.marker.place([self.pos_x, self.pos_y, 1.0], color=color) + +if __name__ == '__main__': + + traffic_light.main() diff --git a/src/el2425/setup.py b/src/el2425/setup.py new file mode 100644 index 00000000..a9fed363 --- /dev/null +++ b/src/el2425/setup.py @@ -0,0 +1,27 @@ +import os +from glob import glob +import xml.etree.ElementTree as ET +from setuptools import find_packages, setup + +package = ET.parse('package.xml').getroot() +name = package.find('name').text + +setup( + name=name, + version='0.0.0', + packages=find_packages(include=[name, f"{name}.*"]), + data_files=[ + ('share/ament_index/resource_index/packages', [f'resource/{name}']), + (f'share/{name}', ['package.xml']), + (f'share/{name}/launch', glob('launch/*.xml')), + (f'lib/{name}', glob('scripts/*.py')), + (os.path.join('share', name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer=package.find('maintainer').text, + maintainer_email=package.find('maintainer').get('email'), + description=package.find('description').text, + license=package.find('license').text, + tests_require=['pytest'], +) diff --git a/src/svea_core/launch/lli_test.xml b/src/svea_core/_launch/lli_test.xml similarity index 100% rename from src/svea_core/launch/lli_test.xml rename to src/svea_core/_launch/lli_test.xml diff --git a/src/svea_core/launch/plot_map.xml b/src/svea_core/_launch/plot_map.xml similarity index 100% rename from src/svea_core/launch/plot_map.xml rename to src/svea_core/_launch/plot_map.xml diff --git a/src/svea_core/launch/save_map.xml b/src/svea_core/_launch/save_map.xml similarity index 100% rename from src/svea_core/launch/save_map.xml rename to src/svea_core/_launch/save_map.xml diff --git a/src/svea_core/launch/state_publisher_launch.xml b/src/svea_core/_launch/state_publisher_launch.xml similarity index 100% rename from src/svea_core/launch/state_publisher_launch.xml rename to src/svea_core/_launch/state_publisher_launch.xml diff --git a/src/svea_core/launch/svea_vizualization.launch.py b/src/svea_core/_launch/svea_vizualization.launch.py similarity index 100% rename from src/svea_core/launch/svea_vizualization.launch.py rename to src/svea_core/_launch/svea_vizualization.launch.py diff --git a/src/svea_core/launch/map_and_foxglove.xml b/src/svea_core/launch/map_and_foxglove.xml index 1baeba3b..2f5e5ab4 100644 --- a/src/svea_core/launch/map_and_foxglove.xml +++ b/src/svea_core/launch/map_and_foxglove.xml @@ -1,19 +1,20 @@ - + + - + - + + - + - diff --git a/src/svea_core/launch/simulation.xml b/src/svea_core/launch/simulation.xml index 3b9d9649..0878f4b8 100644 --- a/src/svea_core/launch/simulation.xml +++ b/src/svea_core/launch/simulation.xml @@ -1,16 +1,38 @@ + + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - + - - - - diff --git a/src/svea_core/launch/svea.xml b/src/svea_core/launch/svea.xml new file mode 100644 index 00000000..c9cf728c --- /dev/null +++ b/src/svea_core/launch/svea.xml @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/svea_core/launch/svea_launch.xml b/src/svea_core/launch/svea_launch.xml deleted file mode 100644 index 84af3c14..00000000 --- a/src/svea_core/launch/svea_launch.xml +++ /dev/null @@ -1,54 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/svea_core/launch/visualization.xml b/src/svea_core/launch/visualization.xml new file mode 100644 index 00000000..d64f0261 --- /dev/null +++ b/src/svea_core/launch/visualization.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/svea_core/params/zenoh.json5 b/src/svea_core/params/zenoh.json5 new file mode 100644 index 00000000..71af5656 --- /dev/null +++ b/src/svea_core/params/zenoh.json5 @@ -0,0 +1,52 @@ +{ + plugins: { + ros2dds: { + + //// + //// allow / deny: Specify the lists of ROS 2 interfaces that are allowed or denied to be routed over Zenoh. + //// Each element of the lists is a regular expression that must match the full interface name. + //// You cannot set both 'allow' and 'deny' in the same configuration. + //// If neither 'allow' nor 'deny' are set, all interfaces are allowed. + //// Use 'allow' to allow only the specified interfaces. If an interface type is set to an empty list + //// or is not specified at all, it means that NO such interface is allowed. + //// Use 'deny' to allow all except the specified interfaces. If an interface type is set to an empty list + //// or is not specified at all, it means that ALL such interface are allowed. + allow: { + publishers: ["/fleet/.+"], + subscribers: ["/fleet/.+"], + service_servers: ["/fleet/.+"], + service_clients: ["/fleet/.+"], + action_servers: ["/fleet/.+"], + action_clients: ["/fleet/.+"], + }, + + "timestamp_check": { + "enabled": false + } + + }, + + }, + + //// + //// Which endpoints to connect to. E.g. tcp/localhost:7447. + //// By configuring the endpoints, it is possible to tell zenoh which remote router or other zenoh-bridge-ros2dds to connect to at startup. + //// + connect: { + endpoints: [ + "tcp/labs.itrl.kth.se:7447" + ] + }, + + //// + //// Configure the scouting mechanisms and their behaviours + //// + scouting: { + /// The UDP multicast scouting configuration. + multicast: { + /// Whether multicast scouting is enabled or not + enabled: false, + }, + }, + +} diff --git a/src/svea_core/scripts/encoder_filter.py b/src/svea_core/scripts/encoder_filter.py index 17763c05..2a71a704 100755 --- a/src/svea_core/scripts/encoder_filter.py +++ b/src/svea_core/scripts/encoder_filter.py @@ -32,8 +32,10 @@ class encoder_filter(rx.Node): override = None encoder_time = None + frame_id = rx.Parameter('wheel_encoder') + ## Publishers ## - encoder_re_pub = rx.Publisher(TwistWithCovarianceStamped, '/encoder/filtered', qos_pubber) + encoder_re_pub = rx.Publisher(TwistWithCovarianceStamped, '/lli/filtered/encoders', qos_pubber) ## Subscribers ## @rx.Subscriber(Bool, '/lli/remote/override', qos_subber) @@ -56,6 +58,7 @@ def ctrl_throttle_sub(self, ctrl_throttle_msg): @rx.Subscriber(TwistWithCovarianceStamped, '/lli/sensor/encoders', qos_subber) def encoder_sub(self, encoder_msg): + encoder_msg.header.frame_id = self.frame_id current_time = encoder_msg.header.stamp.sec + encoder_msg.header.stamp.nanosec * 1e-9 # 如果是第一次回调，直接记录时间，不计算dt diff --git a/src/svea_core/scripts/imu_bias_remover.py b/src/svea_core/scripts/imu_bias_remover.py index 036d638a..7bb09c37 100755 --- a/src/svea_core/scripts/imu_bias_remover.py +++ b/src/svea_core/scripts/imu_bias_remover.py @@ -30,8 +30,10 @@ class imu_bias_remove(rx.Node): sample_count = 100 sample_counter = 0 + frame_id = rx.Parameter('imu') + ## Publishers ## - imu_re_pub = rx.Publisher(Imu, '/imu/filtered', qos_pubber) + imu_re_pub = rx.Publisher(Imu, '/lli/filtered/imu', qos_pubber) ## Subscribers ## @rx.Subscriber(Imu, '/lli/sensor/imu', qos_subber) @@ -53,6 +55,7 @@ def imu_sub(self, imu_msg): self.get_logger().info(f"IMU bias sampled: angular_z: {self.bias_angular_z}, linear_x: {self.bias_linear_x}, linear_y: {self.bias_linear_y}") else: + imu_msg.header.frame_id = self.frame_id imu_msg.angular_velocity.z -= self.bias_angular_z imu_msg.angular_velocity.z = imu_msg.angular_velocity.z / 4.0 imu_msg.linear_acceleration.x -= self.bias_linear_x diff --git a/src/svea_core/scripts/lidar_timer.py b/src/svea_core/scripts/lidar_timer.py index 38f47642..f1e50930 100755 --- a/src/svea_core/scripts/lidar_timer.py +++ b/src/svea_core/scripts/lidar_timer.py @@ -1,9 +1,9 @@ #! /usr/bin/env python3 - from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, QoSHistoryPolicy from sensor_msgs.msg import LaserScan -from rclpy.time import Duration - +from std_msgs.msg import Header +from rclpy.time import Duration, Time +from tf2_ros import Buffer, TransformListener, LookupException, ConnectivityException, ExtrapolationException from svea_core import rosonic as rx qos_pubber = QoSProfile( @@ -13,7 +13,6 @@ depth=1, ) - qos_subber = QoSProfile( reliability=QoSReliabilityPolicy.BEST_EFFORT, # BEST_EFFORT history=QoSHistoryPolicy.KEEP_LAST, # Keep the last N messages @@ -21,26 +20,72 @@ depth=10, # Size of the queue ) - class lidar_filter(rx.Node): """ Correcting Encoder Diraction """ + from_topic = rx.Parameter('scan') + to_topic = rx.Parameter('scan/filtered') + + target_frame = rx.Parameter('base_link') + source_frame = rx.Parameter('odom') + ## Publishers ## - encoder_re_pub = rx.Publisher(LaserScan, '/scan/filtered', qos_pubber) + encoder_re_pub = rx.Publisher(LaserScan, to_topic, qos_pubber) ## Subscribers ## - @rx.Subscriber(LaserScan, '/scan', qos_subber) + @rx.Subscriber(LaserScan, from_topic, qos_subber) def laser_callback(self, laser_msg): - now = self.get_clock().now() - adjusted_time = now - Duration(seconds=0.01) - laser_msg.header.stamp = adjusted_time.to_msg() + laser_msg.header.stamp = self.header.stamp self.encoder_re_pub.publish(laser_msg) def on_startup(self): - pass + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) +<<<<<<< HEAD + +======= +>>>>>>> el2425/main + # Use a timer to check periodically + self.timer = self.create_timer(0.03, self.get_latest_transform_header) + self.header = Header() + self.header.stamp = self.get_clock().now().to_msg() +<<<<<<< HEAD + +======= + +>>>>>>> el2425/main + def get_latest_transform_header(self): + try: + # "latest" means we don't specify a time + transform = self.tf_buffer.lookup_transform( +<<<<<<< HEAD + target_frame='base_link', # target + source_frame='map', # source + time=Time() # latest available + ) + +======= + target_frame=self.target_frame, # target + source_frame=self.source_frame, # source + time=Time() # latest available + ) +>>>>>>> el2425/main + self.header = transform.header + # self.get_logger().info( + # f"Latest transform header:\n" + # f" Frame ID: {self.header.frame_id}\n" + # f" Child Frame ID: {transform.child_frame_id}\n" + # f" Stamp: {self.header.stamp.sec}.{self.header.stamp.nanosec}" + # ) +<<<<<<< HEAD + +======= +>>>>>>> el2425/main + except (LookupException, ConnectivityException, ExtrapolationException) as e: + self.get_logger().warn(f"Transform not available: {e}") if __name__ == '__main__': - lidar_filter.main() + lidar_filter.main() \ No newline at end of file diff --git a/src/svea_core/scripts/model_viz.py b/src/svea_core/scripts/model_viz.py deleted file mode 100755 index 8501175d..00000000 --- a/src/svea_core/scripts/model_viz.py +++ /dev/null @@ -1,87 +0,0 @@ -#! /usr/bin/env python3 - -from math import sin, cos, pi -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSProfile -from geometry_msgs.msg import Quaternion -from tf2_ros import TransformBroadcaster, TransformStamped - -class Publish3Dcar(Node): - - def __init__(self): - rclpy.init() - super().__init__('Publish3Dcar') - - qos_profile = QoSProfile(depth=10) - self.broadcaster = TransformBroadcaster(self, qos=qos_profile) - self.nodeName = self.get_name() - self.get_logger().info("{0} started".format(self.nodeName)) - - degree = pi / 180.0 - loop_rate = self.create_rate(30) - - # robot state - tilt = 0. - tinc = degree - swivel = 0. - angle = 0. - height = 0. - hinc = 0.005 - - # message declarations - namespace = self.get_namespace() - self.get_logger().info("Namespace: {0}".format(namespace)) - frame_id = namespace + '/odom' if namespace != '/' else 'odom' - child_id = namespace + '/axis' if namespace != '/' else 'axis' - odom_trans = TransformStamped() - odom_trans.header.frame_id = frame_id - odom_trans.child_frame_id = child_id - - try: - while rclpy.ok(): - rclpy.spin_once(self) - - # update joint_state - now = self.get_clock().now() - - # update transform - # (moving in a circle with radius=2) - odom_trans.header.stamp = now.to_msg() - odom_trans.transform.translation.x = cos(angle)*2 - odom_trans.transform.translation.y = sin(angle)*2 - odom_trans.transform.translation.z = 0.7 - odom_trans.transform.rotation = \ - euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw - - # send the joint state and transform - self.broadcaster.sendTransform(odom_trans) - - # Create new robot state - tilt += tinc - if tilt < -0.5 or tilt > 0.0: - tinc *= -1 - height += hinc - if height > 0.2 or height < 0.0: - hinc *= -1 - swivel += degree - angle += degree/4 - - # This will adjust as needed per iteration - loop_rate.sleep() - - except KeyboardInterrupt: - pass - -def euler_to_quaternion(roll, pitch, yaw): - qx = sin(roll/2) * cos(pitch/2) * cos(yaw/2) - cos(roll/2) * sin(pitch/2) * sin(yaw/2) - qy = cos(roll/2) * sin(pitch/2) * cos(yaw/2) + sin(roll/2) * cos(pitch/2) * sin(yaw/2) - qz = cos(roll/2) * cos(pitch/2) * sin(yaw/2) - sin(roll/2) * sin(pitch/2) * cos(yaw/2) - qw = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/2) * sin(pitch/2) * sin(yaw/2) - return Quaternion(x=qx, y=qy, z=qz, w=qw) - -def main(): - node = Publish3Dcar() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/svea_core/scripts/sim_lidar.py b/src/svea_core/scripts/sim_lidar.py index 34c20c4b..aaeadf75 100755 --- a/src/svea_core/scripts/sim_lidar.py +++ b/src/svea_core/scripts/sim_lidar.py @@ -76,12 +76,13 @@ class sim_lidar(rx.Node): ## Parameters ## odometry_top = rx.Parameter('odometry/local') + laser_frame = rx.Parameter('laser') _viz_points_topic = 'viz_lidar_points' _viz_rays_topic = 'viz_lidar_rays' _viz_edges_topic = 'viz_edges' ## Publishers ## - _scan_pub = rx.Publisher(LaserScan, '/scan', qos_pubber) + _scan_pub = rx.Publisher(LaserScan, 'scan', qos_pubber) _viz_points_pub = rx.Publisher(PointCloud, _viz_points_topic, qos_pubber) _viz_rays_pub = rx.Publisher(Marker, _viz_rays_topic, qos_pubber) _viz_edges_pub = rx.Publisher(Marker, _viz_edges_topic, qos_pubber) @@ -129,7 +130,7 @@ def on_startup(self): self._scan_msg = LaserScan() self._scan_msg.header.stamp = rclpy.clock.Clock().now().to_msg() - self._scan_msg.header.frame_id = 'laser' + self._scan_msg.header.frame_id = self.laser_frame self._scan_msg.angle_min = self.ANGLE_MIN self._scan_msg.angle_max = self.ANGLE_MAX self._scan_msg.angle_increment = self.INCREMENT diff --git a/src/svea_core/scripts/sim_svea.py b/src/svea_core/scripts/sim_svea.py index ffd98aad..65daca82 100755 --- a/src/svea_core/scripts/sim_svea.py +++ b/src/svea_core/scripts/sim_svea.py @@ -5,7 +5,10 @@ subscriptions and publications that match the real car platform. Intended for debugging code BEFORE running on a real car. -Author: Frank Jiang +Contributors: +- Frank Jiang +- Kaj Munhoz Arfvidsson +- Li Chen """ import math @@ -30,7 +33,6 @@ depth=1, ) - qos_subber = QoSProfile( reliability=QoSReliabilityPolicy.RELIABLE, # Reliable history=QoSHistoryPolicy.KEEP_LAST, # Keep the last N messages @@ -68,20 +70,24 @@ class sim_svea(rx.Node): ## Parameters ## time_step = 0.025 - publish_tf = rx.Parameter(True) + initial_pose_x = rx.Parameter(0.0) + initial_pose_y = rx.Parameter(0.0) + initial_pose_a = rx.Parameter(0.0) + + odometry_top = rx.Parameter('odometry/local') + + # In simulation, we want lli to be namespaced steering_request_top = rx.Parameter('lli/ctrl/steering') throttle_request_top = rx.Parameter('lli/ctrl/throttle') highgear_request_top = rx.Parameter('lli/ctrl/highgear') diff_request_top = rx.Parameter('lli/ctrl/diff') - odometry_top = rx.Parameter('odometry/local') - map_frame = rx.Parameter('map') odom_frame = rx.Parameter('odom') - self_frame = rx.Parameter('base_link') - - state = rx.Parameter([0.0, 0.0, 0.0, 0.0]) # x, y, yaw, velocity + base_frame = rx.Parameter('base_link') + + publish_tf = rx.Parameter(True) ## Publishers ## odometry_pub = rx.Publisher(Odometry, odometry_top, qos_pubber) @@ -92,12 +98,12 @@ class sim_svea(rx.Node): def steering_request_cb(self, steering_request_msg): self.steering_req = steering_request_msg.data * -1 self.last_ctrl_time = self.clock.now().to_msg() - + @rx.Subscriber(Int8, throttle_request_top, qos_subber) def throttle_request_cb(self, throttle_request_msg): self.velocity_req = throttle_request_msg.data self.last_ctrl_time = self.clock.now().to_msg() - + @rx.Subscriber(Bool, highgear_request_top, qos_subber) def highgear_request_cb(self, highgear_request_msg): self.highgear = highgear_request_msg.data @@ -116,16 +122,17 @@ def on_startup(self): self.last_ctrl_time = self.clock.now().to_msg() self.last_pub_time = self.clock.now().to_msg() - self.model = Bicycle4DWithESC(initial_state=self.state) + self.model = Bicycle4DWithESC(initial_state=[ + self.initial_pose_x, + self.initial_pose_y, + self.initial_pose_a, + 0.0, + ]) self.steering_req = 0.0 self.velocity_req = 0.0 self.highgear = False self.diff = False - namespace = self.get_namespace() - self.odom_frame = namespace + '/' + self.odom_frame if namespace != '/' else self.odom_frame - self.self_frame = namespace + '/' + self.self_frame if namespace != '/' else self.self_frame - if self.publish_tf: # for broadcasting fake tf tree self.tf_br = tf2_ros.TransformBroadcaster(self) @@ -154,7 +161,7 @@ def sim_loop(self): odom_msg = Odometry() odom_msg.header.stamp = curr_time odom_msg.header.frame_id = self.map_frame - odom_msg.child_frame_id = self.self_frame + odom_msg.child_frame_id = self.base_frame odom_msg.header.stamp = curr_time odom_msg.pose.pose.position.x = x odom_msg.pose.pose.position.y = y @@ -202,7 +209,7 @@ def _broadcast_tf(self, odom_msg): odom2base = TransformStamped() odom2base.header.stamp = odom_msg.header.stamp odom2base.header.frame_id = self.odom_frame - odom2base.child_frame_id = self.self_frame + odom2base.child_frame_id = self.base_frame odom2base.transform.translation.x = odom_msg.pose.pose.position.x odom2base.transform.translation.y = odom_msg.pose.pose.position.y odom2base.transform.translation.z = odom_msg.pose.pose.position.z diff --git a/src/svea_core/setup.py b/src/svea_core/setup.py index 42caf9d8..e105ac96 100644 --- a/src/svea_core/setup.py +++ b/src/svea_core/setup.py @@ -15,7 +15,7 @@ (f'share/{name}', ['package.xml']), (f'share/{name}/launch', glob('launch/*.xml')), (os.path.join('share', name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), - (f'share/{name}/params', glob('params/*.yaml')), + (f'share/{name}/params', glob('params/*') + glob('params/**/*')), (f'share/{name}/maps', glob('maps/*')), (f'lib/{name}', glob('scripts/*.py')), (f'share/{name}/urdf', glob('urdf/*')), diff --git a/src/svea_core/svea_core/__pycache__/__init__.cpython-312.pyc b/src/svea_core/svea_core/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 00000000..0935f54a Binary files /dev/null and b/src/svea_core/svea_core/__pycache__/__init__.cpython-312.pyc differ diff --git a/src/svea_core/svea_core/controllers/__pycache__/__init__.cpython-312.pyc b/src/svea_core/svea_core/controllers/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 00000000..25bc83df Binary files /dev/null and b/src/svea_core/svea_core/controllers/__pycache__/__init__.cpython-312.pyc differ diff --git a/src/svea_core/svea_core/controllers/__pycache__/pure_pursuit.cpython-312.pyc b/src/svea_core/svea_core/controllers/__pycache__/pure_pursuit.cpython-312.pyc new file mode 100644 index 00000000..84d4f79a Binary files /dev/null and b/src/svea_core/svea_core/controllers/__pycache__/pure_pursuit.cpython-312.pyc differ diff --git a/src/svea_core/svea_core/interfaces/__init__.py b/src/svea_core/svea_core/interfaces/__init__.py index 208baab4..3e9351c5 100644 --- a/src/svea_core/svea_core/interfaces/__init__.py +++ b/src/svea_core/svea_core/interfaces/__init__.py @@ -1,2 +1,8 @@ +# Core Interfaces from .actuation import * from .localization import * + +# Utility Interfaces +from .path import * +from .marker import * + diff --git a/src/svea_core/svea_core/interfaces/__pycache__/__init__.cpython-312.pyc b/src/svea_core/svea_core/interfaces/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 00000000..85bdd284 Binary files /dev/null and b/src/svea_core/svea_core/interfaces/__pycache__/__init__.cpython-312.pyc differ diff --git a/src/svea_core/svea_core/interfaces/__pycache__/actuation.cpython-312.pyc b/src/svea_core/svea_core/interfaces/__pycache__/actuation.cpython-312.pyc new file mode 100644 index 00000000..bf4787db Binary files /dev/null and b/src/svea_core/svea_core/interfaces/__pycache__/actuation.cpython-312.pyc differ diff --git a/src/svea_core/svea_core/interfaces/__pycache__/localization.cpython-312.pyc b/src/svea_core/svea_core/interfaces/__pycache__/localization.cpython-312.pyc new file mode 100644 index 00000000..f3f9d24a Binary files /dev/null and b/src/svea_core/svea_core/interfaces/__pycache__/localization.cpython-312.pyc differ diff --git a/src/svea_core/svea_core/interfaces/actuation.py b/src/svea_core/svea_core/interfaces/actuation.py index bdb5936f..d2846a94 100644 --- a/src/svea_core/svea_core/interfaces/actuation.py +++ b/src/svea_core/svea_core/interfaces/actuation.py @@ -58,10 +58,17 @@ class ActuationInterface(rx.Field): is_sim = rx.Parameter(False) - steering_pub = rx.Publisher(Int8, 'lli/ctrl/steering', qos_profile=QoS_DEFAULT) - throttle_pub = rx.Publisher(Int8, 'lli/ctrl/throttle', qos_profile=QoS_DEFAULT) - highgear_pub = rx.Publisher(Bool, 'lli/ctrl/highgear', qos_profile=QoS_RELIABLE) - diff_pub = rx.Publisher(Bool, 'lli/ctrl/diff', qos_profile=QoS_RELIABLE) + steering_top = rx.Parameter('/lli/ctrl/steering') + steering_pub = rx.Publisher(Int8, steering_top, qos_profile=QoS_DEFAULT) + + throttle_top = rx.Parameter('/lli/ctrl/throttle') + throttle_pub = rx.Publisher(Int8, throttle_top, qos_profile=QoS_DEFAULT) + + highgear_top = rx.Parameter('/lli/ctrl/highgear') + highgear_pub = rx.Publisher(Bool, highgear_top, qos_profile=QoS_RELIABLE) + + diff_top = rx.Parameter('/lli/ctrl/diff') + diff_pub = rx.Publisher(Bool, diff_top, qos_profile=QoS_RELIABLE) def __init__(self, rate=20, use_acceleration=False, highgear=False, diff=False): self.acceleration = use_acceleration diff --git a/src/svea_core/svea_core/interfaces/localization.py b/src/svea_core/svea_core/interfaces/localization.py index 71ddd8a5..7067a785 100644 --- a/src/svea_core/svea_core/interfaces/localization.py +++ b/src/svea_core/svea_core/interfaces/localization.py @@ -36,10 +36,13 @@ class LocalizationInterface(rx.Field): as state information is available. """ - class _InterfaceParameters(rx.NamedField): - odom_top = rx.Parameter('odometry/local') + localization = rx.namespace( + map_frame = rx.Parameter('map'), + odom_frame = rx.Parameter('self/odom'), + base_frame = rx.Parameter('self/base_link'), + odom_top = rx.Parameter('odometry/local'), + ) - _params = _InterfaceParameters(name='localization') _odom_msg = Odometry() def __init__(self) -> None: @@ -57,7 +60,7 @@ def on_startup(self): self.node.get_logger().info("Localization interface is ready.") return self._is_started - @rx.Subscriber(Odometry, _params.odom_top, qos_profile=qos_profile) + @rx.Subscriber(Odometry, localization.odom_top, qos_profile=qos_profile) def _odom_cb(self, msg: Odometry) -> None: if not self._is_started(): return @@ -72,8 +75,8 @@ def _odom_cb(self, msg: Odometry) -> None: def transform_odom( self, odom: Odometry, - pose_target: str = "map", - twist_target: str = "base_link", + pose_target: str | None = None, + twist_target: str | None = None, timeout_s: float = 0.2, ) -> Odometry: """ @@ -88,6 +91,11 @@ def transform_odom( assert self._is_started(), 'Localization interface not started yet' + if pose_target is None: + pose_target = self.localization.map_frame + if twist_target is None: + twist_target = self.localization.base_frame + # ---- Source frames from the incoming message ---- pose_source = odom.header.frame_id or "" twist_source = odom.child_frame_id or pose_source or "" diff --git a/src/svea_core/svea_core/interfaces/marker.py b/src/svea_core/svea_core/interfaces/marker.py new file mode 100644 index 00000000..b5a330cc --- /dev/null +++ b/src/svea_core/svea_core/interfaces/marker.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python +from typing import Sequence + +import tf_transformations as tf +from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, QoSHistoryPolicy +from visualization_msgs.msg import Marker +from PIL import ImageColor + +from .. import rosonic as rx + +qos_pubber = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.VOLATILE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, +) + +class ShowMarker(rx.NamedField): + + # {parent} will expand to name of interface + _pub = rx.Publisher(Marker, '~/{parent}', qos_profile=qos_pubber) + + def place( + self, + position: Sequence[float] | None = None, + orientation: Sequence[float] | None = None, + frame_id: str = "map", + color: str | Sequence[int] = 'white', + shape: int = Marker.SPHERE, + scale: float | Sequence[float] = 0.5, + **kwds, + ) -> None: + + if position is None: + position = [0.0, 0.0, 0.0] + elif len(position) == 2: + position = [position[0], position[1], 0.0] + elif len(position) != 3: + raise ValueError("Position must be a sequence of three floats.") + + if orientation is None: + orientation = [0.0, 0.0, 0.0, 1.0] + elif len(orientation) == 3: + x, y, z = tf.quaternion_from_euler(orientation[0], orientation[1], orientation[2]) + orientation = [x, y, z, 1.0] + elif len(orientation) != 4: + raise ValueError("Orientation must be a sequence of three (Euler) or four (quaternion) floats.") + + if isinstance(scale, (int, float)): + scale = [scale] * 3 + elif len(scale) != 3: + raise ValueError("Scale must be a float or a sequence of three floats.") + + r, g, b, a = self.parse_color(color) + + self.node.get_logger().debug(f"Placing marker at {position}") + marker = Marker() + marker.header.frame_id = frame_id + marker.header.stamp = self.node.get_clock().now().to_msg() + marker.type = shape + marker.action = Marker.ADD + marker.pose.position.x = position[0] + marker.pose.position.y = position[1] + marker.pose.position.z = position[2] if len(position) > 2 else 0.0 + marker.pose.orientation.x = orientation[0] + marker.pose.orientation.y = orientation[1] + marker.pose.orientation.z = orientation[2] + marker.pose.orientation.w = orientation[3] + marker.scale.x = scale[0] + marker.scale.y = scale[1] + marker.scale.z = scale[2] + marker.color.r = r + marker.color.g = g + marker.color.b = b + marker.color.a = a + + self._pub.publish(marker) + + @staticmethod + def parse_color(input_color, default_alpha=1.0): + """ + Parse color input and return in RGBA format. + + Args: + input_color: Input color, can be: + - Color name string (e.g., 'red', 'blue') + - Hex string (e.g., '#FF0000', '#FF0000FF') + - RGB/RGBA tuple (e.g., (255, 0, 0) or (1.0, 0.0, 0.0, 0.5)) + default_alpha: Default alpha value when input color has no alpha channel (0.0-1.0) + + Returns: + RGBA tuple with all components in 0.0–1.0 range. + """ + + # If already a tuple or list + if isinstance(input_color, (tuple, list)) and len(input_color) in (3, 4): + if all(isinstance(c, int) and 0 <= c <= 255 for c in input_color[:3]): + r, g, b = [c / 255.0 for c in input_color[:3]] + a = input_color[3] / 255.0 if len(input_color) == 4 else default_alpha + return r, g, b, a + elif all(isinstance(c, float) and 0.0 <= c <= 1.0 for c in input_color[:3]): + r, g, b = input_color[:3] + a = input_color[3] if len(input_color) == 4 else default_alpha + return r, g, b, a + + # If string input (color name or hex) + if isinstance(input_color, str): + try: + # Pillow returns RGB or RGBA as integers 0–255 + rgb = ImageColor.getrgb(input_color) + if len(rgb) == 3: + r, g, b = [c / 255.0 for c in rgb] + return r, g, b, default_alpha + elif len(rgb) == 4: + r, g, b, a = [c / 255.0 for c in rgb] + return r, g, b, a + except ValueError: + pass # unrecognized string + + # Fallback — return white + return 1.0, 1.0, 1.0, 1.0 diff --git a/src/svea_core/svea_core/interfaces/path.py b/src/svea_core/svea_core/interfaces/path.py new file mode 100644 index 00000000..9c4ae100 --- /dev/null +++ b/src/svea_core/svea_core/interfaces/path.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python + +from collections import deque + +from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, QoSHistoryPolicy +from rclpy.time import Time +from rclpy.clock import Clock +from nav_msgs.msg import Path, Odometry +from geometry_msgs.msg import PoseStamped +import tf_transformations as tf + +from .. import rosonic as rx + +qos_subber = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, # Reliable + durability=QoSDurabilityPolicy.VOLATILE, # Volatile + history=QoSHistoryPolicy.KEEP_LAST, # Keep the last N messages + depth=10, # Size of the queue +) + +qos_pubber = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.VOLATILE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, +) + +class ShowPath(rx.Field): + """ + TODO: + - Build more general interface for navigation (with nav2 topics?) + """ + + PATH_MAX_LEN = 50 + + odometry_top = rx.Parameter('odometry/local') + + past_path_pub = rx.Publisher(Path, 'past_path', qos_pubber) + path_plan_pub = rx.Publisher(Path, 'path_plan', qos_pubber) + + @rx.Subscriber(Odometry, odometry_top, qos_subber) + def odometry_callback(self, msg): + self.past_path.append(self._odom_to_pose(msg)) + path = Path() + path.header = msg.header + path.poses = self.past_path + self.past_path_pub.publish(path) + + def __init__(self): + + self.past_path = deque(maxlen=self.PATH_MAX_LEN) + + def publish_path(self, x_list, y_list, yaw_list=None, t_list=None): + """Publish trajectory visualization + + Args: + x_list: x trajectory in [m] + y_list: y trajecotory in [m] + yaw_list: yaw trajectory in [rad], defaults to None + t_list: time trajectory in [s], defaults to None + """ + + path = Path() + path.header.stamp = Clock().now().to_msg() + path.header.frame_id = 'map' + + poses = [] + for i, (x, y) in enumerate(zip(x_list, y_list)): + + curr_pose = PoseStamped() + curr_pose.header.frame_id = path.header.frame_id + curr_pose.pose.position.x = x + curr_pose.pose.position.y = y + + if yaw_list is not None: + yaw = yaw_list[i] + quat = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw) + curr_pose.pose.orientation.x = quat[0] + curr_pose.pose.orientation.y = quat[1] + curr_pose.pose.orientation.z = quat[2] + curr_pose.pose.orientation.w = quat[3] + + if t_list is not None: + t = t_list[i] + curr_pose.header.stamp = Time(secs = t).to_msg() + else: + curr_pose.header.stamp = path.header.stamp + + poses.append(curr_pose) + + path.poses = poses + self.path_plan_pub.publish(path) + + def _odom_to_pose(self, msg): + """Odometry -> PoseStamped""" + pose = PoseStamped() + pose.header = msg.header + pose.pose = msg.pose.pose + return pose + diff --git a/src/svea_core/svea_core/models/__pycache__/__init__.cpython-312.pyc b/src/svea_core/svea_core/models/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 00000000..6be19764 Binary files /dev/null and b/src/svea_core/svea_core/models/__pycache__/__init__.cpython-312.pyc differ diff --git a/src/svea_core/svea_core/models/__pycache__/bicycle.cpython-312.pyc b/src/svea_core/svea_core/models/__pycache__/bicycle.cpython-312.pyc new file mode 100644 index 00000000..6da4c8a4 Binary files /dev/null and b/src/svea_core/svea_core/models/__pycache__/bicycle.cpython-312.pyc differ diff --git a/src/svea_core/svea_core/rosonic.py b/src/svea_core/svea_core/rosonic.py index 26b8a02a..531ac2d6 100644 --- a/src/svea_core/svea_core/rosonic.py +++ b/src/svea_core/svea_core/rosonic.py @@ -236,7 +236,9 @@ def __rosonic_register__(self, owner: 'Resource', *, name: str | None = None): Args: owner (Resource): The parent resource that owns this one. - name (str | None): Optional name override for this resource. + name (str | Callable[[], str] | None): + Optional name override for this resource. Can be lazy-evaluated + via a callable returning a string. ns (str | None): Optional namespace override for this resource. Raises: @@ -252,11 +254,13 @@ def __rosonic_register__(self, owner: 'Resource', *, name: str | None = None): if owner is self: assert self.__rosonic_name__ is not None, f"Root resource '{self}' must have a name" + assert not callable(self.__rosonic_name__), f"Root resource '{self}' name cannot be lazy-evaluated" else: assert owner._is_registered(), f"Resource '{owner}' owning '{self}' is not registered" # The following check only makes sense for non-root, named resources - if self.__rosonic_name__ is not None: + # NOTE: Does this make sense at all? Some resources may want the same name, e.g. pub/sub pairs. + if self.__rosonic_name__ is not None and not callable(self.__rosonic_name__): fullname = f"{owner.__rosonic_fullname__}/{self.__rosonic_name__}" assert fullname not in self.__rosonic_lookup__, \ f"Resource '{fullname}' already registered" @@ -286,6 +290,9 @@ def __rosonic_fullname__(self) -> str: name = self.__rosonic_name__ owner = self.__rosonic_owner__ + if callable(name): + name = '?' + if name is None: return owner.__rosonic_fullname__ elif self._is_root() or self._is_absolute_name(name): @@ -312,6 +319,9 @@ def __rosonic_relname__(self) -> str: name = self.__rosonic_name__ owner = self.__rosonic_owner__ + if callable(name): + name = '?' + if name is None: return owner.__rosonic_relname__ elif self._is_root(): @@ -362,6 +372,16 @@ def __rosonic_startup__(self, node: NodeBase) -> None: self.__rosonic_node__ = node + # resolve lazy name if any + if callable(self.__rosonic_name__): + self.__rosonic_name__ = self.__rosonic_name__() + + if isinstance(self.__rosonic_name__, str): + self.__rosonic_name__ = self.__rosonic_name__.format( + parent=self.__rosonic_owner__.__rosonic_relname__, + parent_fullname=self.__rosonic_owner__.__rosonic_fullname__, + ) + self.on_startup() self.__rosonic_started__ = True @@ -430,6 +450,23 @@ def _get_root(self) -> Resource: return (self if self._is_root() else self._get_root()) + def start(self, node: Node): + """ + Starts this resource manually. + + This method can be used to start a resource outside of the normal node + startup sequence. It invokes `__rosonic_startup__()` with the provided + node. + + Args: + node (Node): The ROS 2 node instance to associate with this + resource. + + Raises: + AssertionError: If the resource is already started. + """ + assert not self._is_started(), f"Resource '{self}' is already started" + self.__rosonic_startup__(node) # Only for typing class _RegisteredResource(Resource): @@ -443,6 +480,8 @@ class _StartedResource(_RegisteredResource): __rosonic_node__: NodeBase +### Base Resource Types + class Node(Resource, NodeBase): """ Base class for all SVEA nodes. @@ -587,6 +626,14 @@ def __set_name__(self, owner: type, name: str) -> None: owner.__rosonic_preregistered__ += (self,) + +def namespace(name=None, /, **resources): + ns = type('Namespace', (NamedField,), resources) + return ns(name=name) + + +### Resource Types + class Parameter(NamedField): """ Declarative Parameter resource. @@ -611,6 +658,7 @@ class MyNode(rx.Node): ### Notes: - The parameter is not resolved (no value) until node startup is complete. - Until then, accessing the field will return the Parameter object itself. + - TODO: Dynamically updatable parameters? """ def __init__(self, *args, name: str | None = None): @@ -640,8 +688,10 @@ def on_startup(self): node = self.__rosonic_node__ name = self.__rosonic_relname__ - if self.value is ...: + if not node.has_parameter(name): node.declare_parameter(name, *self.args) + + if self.value is ...: self.value = node.get_parameter(name).value class Publisher(NamedField): @@ -691,13 +741,20 @@ def __init__(self, msg_type, topic=None, qos_profile=None): Args: msg_type (type): The message type (e.g., `std_msgs.msg.String`). - topic (str | Parameter): Topic name as string or Parameter - reference. + topic (None | str | Parameter): Topic name as string or Parameter + reference, None to use the default resource name. qos_profile: Optional QoSProfile for the publisher. """ - super().__init__(name=topic if topic is str else None) + + # If topic is Parameter, we cannot assign the topic name as a rosonic resource name. + # In that case, we leave it '?' and let the publisher lazily evaluate the name. + def lazy_name(): + assert isinstance(topic, Parameter), "lazy_name should only be used with Parameter topics" + assert topic._is_started(), f"Resource '{self}' depend on '{topic}' which has not started yet" + return topic.value + + super().__init__(name=lazy_name if isinstance(topic, Parameter) else topic) self.msg_type = msg_type - self.topic = topic self.qos_profile = qos_profile self.publisher = None @@ -719,14 +776,9 @@ def on_startup(self): """ node = self.__rosonic_node__ msg_type = self.msg_type - topic = (self.topic if self.topic is not None else - self.__rosonic_fullname__) + topic = self.__rosonic_relname__ qos_profile = self.qos_profile or rclpy.qos.qos_profile_default - if isinstance(topic, Parameter): - assert topic._is_started(), f"Resource '{self}' depend on '{topic}' which has not started yet" - topic = topic.value - if not isinstance(msg_type, type): raise RuntimeError(f"Message type must be a class, not {type(msg_type)}") if not isinstance(topic, str): @@ -779,15 +831,22 @@ def __init__(self, msg_type, topic=None, qos_profile=None): Args: msg_type (type): The message type (e.g., `std_msgs.msg.String`). - name (str | Parameter): Topic name as string or Parameter - reference. + name (None | str | Parameter): + Topic name as string or Parameter reference. None to use the default + resource name. qos_profile: Optional QoSProfile for the subscriber. """ - super().__init__(name=topic if topic is str else None) + # If topic is Parameter, we cannot assign the topic name as a rosonic resource name. + # In that case, we leave it '?' and let the publisher lazily evaluate the name. + def lazy_name(): + assert isinstance(topic, Parameter), "lazy_name should only be used with Parameter topics" + assert topic._is_started(), f"Resource '{self}' depend on '{topic}' which has not started yet" + return topic.value + + super().__init__(name=lazy_name if isinstance(topic, Parameter) else topic) self.subscriber = None self.msg_type = msg_type - self.topic = topic self.qos_profile = qos_profile self.subscriber = None self.callback = None @@ -812,14 +871,9 @@ def on_startup(self): node = self.__rosonic_node__ msg_type = self.msg_type - topic = (self.topic if self.topic is not None else - self.__rosonic_fullname__) + topic = self.__rosonic_relname__ qos_profile = self.qos_profile or rclpy.qos.qos_profile_default - if isinstance(topic, Parameter): - assert topic._is_started(), f"Resource '{self}' depend on '{topic}' which has not started yet" - topic = topic.value - if not isinstance(msg_type, type): raise RuntimeError(f"Message type must be a class, not {type(msg_type)}") if not isinstance(topic, str): @@ -915,7 +969,7 @@ def on_startup(self): owner = self.__rosonic_owner__ period = self.period if isinstance(period, Parameter): - assert _is_started(period), f"Resource '{self}' depend on '{period}' which has not started yet" + assert self._is_started(period), f"Resource '{self}' depend on '{period}' which has not started yet" period = period.value wrapped_callback = lambda: self.callback(owner) self.tmr = node.create_timer(period, wrapped_callback, **self.kwds) diff --git a/src/svea_core/svea_core/utils/__init__.py b/src/svea_core/svea_core/utils/__init__.py index b0c88f49..e69de29b 100644 --- a/src/svea_core/svea_core/utils/__init__.py +++ b/src/svea_core/svea_core/utils/__init__.py @@ -1,2 +0,0 @@ -from .markers import * -from .path import * \ No newline at end of file diff --git a/src/svea_core/svea_core/utils/markers.py b/src/svea_core/svea_core/utils/markers.py deleted file mode 100755 index c3f906b8..00000000 --- a/src/svea_core/svea_core/utils/markers.py +++ /dev/null @@ -1,157 +0,0 @@ -#!/usr/bin/env python -from .. import rosonic as rx -from matplotlib.colors import to_rgba, is_color_like -from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, QoSHistoryPolicy -from visualization_msgs.msg import Marker -from rclpy.clock import Clock -import tf_transformations as tf -from builtin_interfaces.msg import Duration - -qos_pubber = QoSProfile( - reliability=QoSReliabilityPolicy.RELIABLE, - durability=QoSDurabilityPolicy.VOLATILE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, -) - - -class PlaceMarker(rx.Field): - def __init__(self, **kwds) -> None: - if namespace := kwds.get('name_space', None): - self.ns = kwds.get('name_space', None) - else: - self.ns = None - - def on_startup(self): - return self - - def marker(self, name:str, color, position, orientation = [0.0, 0.0, 0.0, 0.0], shape = Marker.SPHERE, **kwds): - - topic_name = '/marker/' + name - mark_pub = self.node.create_publisher(Marker, topic_name, qos_pubber) - - marker = Marker() - - marker.header.frame_id = "map" - marker.header.stamp = self.node.get_clock().now().to_msg() - - marker.type = shape - marker.action = Marker.ADD - - # Position for mark - marker.pose.position.x = position[0] - marker.pose.position.y = position[1] - if len(position) > 2: - marker.pose.position.z = position[2] - else: - marker.pose.position.z = 0.0 - - #Orientation for mark - if len(orientation) == 1: - if isinstance(orientation, list): - orientation = orientation[0] - x, y, z, w = tf.quaternion_from_euler(ak = orientation) - elif len(orientation) == 3: - x, y, z, w = tf.quaternion_from_euler(orientation[0], orientation[1], orientation[2]) - elif len(orientation) == 4: - x = orientation[0] - y = orientation[1] - z = orientation[2] - w = orientation[3] - marker.pose.orientation.x = x - marker.pose.orientation.y = y - marker.pose.orientation.z = z - marker.pose.orientation.w = w - - # size - marker.scale.x = 0.5 - marker.scale.y = 0.5 - marker.scale.z = 0.5 - - # Color for mark - r, g, b, a = parse_color(color) - marker.color.r = r - marker.color.g = g - marker.color.b = b - marker.color.a = a - - # 发布 Marker - mark_pub.publish(marker) - - - def traffic_light_marker(self, number=0, position=[0.0,0.0], light_status='Rd'): - name = f"traffic_{number}" - position = list(position) - position.append(0.5) - position = tuple(position) - if light_status == 'Rd': - self.marker(name, "#FF0000", position) - elif light_status == 'Gr': - self.marker(name, (0.0, 1.0, 0.0), position) - else: - self.marker(name, 'yellow', position) - - - - -def parse_color(input_color, default_alpha=1.0): - """ - Parse color input and return in RGBA format - - Args: - input_color: Input color, can be: - - Color name string (e.g., 'red', 'blue') - - Hex string (e.g., '#FF0000', '#FF0000FF') - - RGB/RGBA tuple (e.g., (255, 0, 0) or (1.0, 0.0, 0.0, 0.5)) - default_alpha: Default alpha value when input color has no alpha channel (0.0-1.0) - - Returns: - RGBA tuple with all components in 0.0-1.0 range - """ - - # If input is already in RGBA format (tuple/list with 3 or 4 values), return directly - if isinstance(input_color, (tuple, list)) and len(input_color) in (3, 4): - # Check if components are integers in 0-255 range - if all(isinstance(c, int) and 0 <= c <= 255 for c in input_color[:3]): - if len(input_color) == 3: - return input_color[0]/255.0, input_color[1]/255.0, input_color[2]/255.0, default_alpha - else: - return input_color[0]/255.0, input_color[1]/255.0, input_color[2]/255.0, input_color[3]/255.0 - # Check if components are floats in 0.0-1.0 range - elif all(isinstance(c, float) and 0.0 <= c <= 1.0 for c in input_color[:3]): - if len(input_color) == 3: - return input_color[0], input_color[1], input_color[2], default_alpha - else: - return input_color[0], input_color[1], input_color[2], input_color[3] - - # Check if input is a valid color string - if isinstance(input_color, str): - if input_color.startswith('#'): - # Process hex color - hex_color = input_color.lstrip('#') - length = len(hex_color) - - if length == 3: # e.g., #RGB - r = int(hex_color[0]*2, 16) / 255.0 - g = int(hex_color[1]*2, 16) / 255.0 - b = int(hex_color[2]*2, 16) / 255.0 - return r, g, b, default_alpha - elif length == 6: # e.g., #RRGGBB - r = int(hex_color[0:2], 16) / 255.0 - g = int(hex_color[2:4], 16) / 255.0 - b = int(hex_color[4:6], 16) / 255.0 - return r, g, b, default_alpha - elif length == 8: # e.g., #RRGGBBAA - r = int(hex_color[0:2], 16) / 255.0 - g = int(hex_color[2:4], 16) / 255.0 - b = int(hex_color[4:6], 16) / 255.0 - a = int(hex_color[6:8], 16) / 255.0 - return r, g, b, a - - # Use matplotlib's color name conversion - if is_color_like(input_color): - color = to_rgba(input_color) - return color[0], color[1], color[2], color[3] - - # Raise exception if format is unrecognized - return 1.0, 1.0, 1.0, 1.0 diff --git a/src/svea_core/svea_core/utils/path.py b/src/svea_core/svea_core/utils/path.py deleted file mode 100644 index 33b45cee..00000000 --- a/src/svea_core/svea_core/utils/path.py +++ /dev/null @@ -1,170 +0,0 @@ -#!/usr/bin/env python -from .. import rosonic as rx -from matplotlib.colors import to_rgba, is_color_like -from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, QoSHistoryPolicy -from geometry_msgs.msg import Point, Point32, PolygonStamped, PointStamped -from rclpy.clock import Clock -import tf_transformations as tf -from nav_msgs.msg import Path, Odometry -from geometry_msgs.msg import Pose, PoseStamped, PoseArray -from rclpy.time import Time -from collections import deque - -qos_subber = QoSProfile( - reliability=QoSReliabilityPolicy.RELIABLE, # Reliable - history=QoSHistoryPolicy.KEEP_LAST, # Keep the last N messages - durability=QoSDurabilityPolicy.VOLATILE, # Volatile - depth=10, # Size of the queue -) - -qos_pubber = QoSProfile( - reliability=QoSReliabilityPolicy.RELIABLE, - durability=QoSDurabilityPolicy.VOLATILE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, -) - -class ShowPath(rx.Field): - - TRAVEL_DIST_THRESH = 0.1 - PATH_MAX_LEN = 100 - chassis_height=0.06 - - odometry_top = rx.Parameter('odometry/local') - - past_path_pub = rx.Publisher(Path,'past_path',qos_pubber) - path_pub = rx.Publisher(Path, 'path_plan',qos_pubber) - - @rx.Subscriber(Odometry, odometry_top, qos_subber) - def odometry_callback(self, msg): - new_path = self.odom_to_pose_stampeds(msg) - self.past_path += new_path - path = Path() - path.header = msg.header - path.poses = self.past_path - self.past_path_pub.publish(path) - - def __ini__(self): - - self.past_path = deque(maxlen=self.PATH_MAX_LEN) - - def on_startup(self): - - self.traj_x = [] - self.traj_y = [] - self.x = [] - self.y = [] - self.past_path= [] - - def odom_to_pose_stampeds(self, msg): - poses = [] - - curr_pose = PoseStamped() - curr_pose.header = msg.header - curr_pose.pose = msg.pose.pose - poses.append(curr_pose) - return poses - - def lists_to_pose_stampeds(self, x_list, y_list, yaw_list=None, t_list=None): - poses = [] - for i in range(len(x_list)): - x = x_list[i] - y = y_list[i] - - curr_pose = PoseStamped() - curr_pose.header.frame_id = 'map' - curr_pose.pose.position.x = x - curr_pose.pose.position.y = y - - if not yaw_list is None: - yaw = yaw_list[i] - quat = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw) - curr_pose.pose.orientation.x = quat[0] - curr_pose.pose.orientation.y = quat[1] - curr_pose.pose.orientation.z = quat[2] - curr_pose.pose.orientation.w = quat[3] - - if not t_list is None: - t = t_list[i] - curr_pose.header.stamp = Time(secs = t).to_msg() - else: - curr_pose.header.stamp = Clock().now().to_msg() - - poses.append(curr_pose) - return poses - - def lists_to_poses(self, x_list, y_list, yaw_list=None): - poses = [] - for i in range(len(x_list)): - x = x_list[i] - y = y_list[i] - - curr_pose = Pose() - curr_pose.position.x = x - curr_pose.position.y = y - - if not yaw_list is None: - yaw = yaw_list[i] - quat = tf.quaternion_from_euler(0.0, 0.0, yaw) - curr_pose.orientation.x = quat[0] - curr_pose.orientation.y = quat[1] - curr_pose.orientation.z = quat[2] - curr_pose.orientation.w = quat[3] - - poses.append(curr_pose) - return poses - - - def publish_path(self, x_list, y_list, yaw_list=None, t_list=None): - """Publish trajectory visualization to rviz - *t_list untested - - :param x_list: x trajectory in [m] - :type x_list: list - :param y_list: y trajecotory in [m] - :type y_list: list - :param yaw_list: yaw trajectory in [rad], defaults to None - :type yaw_list: list - :param t_list: time trajectory in [s], defaults to None - :type t_list: list - """ - - path = Path() - path.header.stamp = Clock().now().to_msg() - path.header.frame_id = 'map' - - path.poses = self.lists_to_pose_stampeds(x_list, y_list, yaw_list, t_list) - self.path_pub.publish(path) - - def publish_target(self, target_publisher, x, y): - """ - Publish target point for visualization in rviz - - :param target_publisher: ROS publisher to broadcast target with - :type target_publisher: rospy.topics.Publisher - """ - target_pt = PointStamped() - target_pt.header.stamp = Clock().now().to_msg() - target_pt.header.frame_id = 'map' - target_pt.point.x = x - target_pt.point.y = y - target_pt.point.z = self.chassis_height - target_publisher.publish(target_pt) - - def publish_pose_array(self, poses_publisher, x_list, y_list, yaw_list): - """Publish list of poses for visualization to rviz - - :param poses_publisher: ROS publisher to broadcast trajectory with - :type poses_publisher: rospy.topics.Publisher - :param x_list: x coordinates in [m] - :type x_list: list - :param y_list: y coordinates in [m] - :type y_list: list - :param yaw_list: yaw coordinates in [rad] - :type yaw_list: list - """ - pose_array = PoseArray() - pose_array.header.stamp = Clock().now().to_msg() - pose_array.header.frame_id = 'map' - pose_array.poses = self.lists_to_poses(x_list, y_list, yaw_list) - poses_publisher.publish(pose_array) \ No newline at end of file diff --git a/src/svea_examples/launch/floor2.xml b/src/svea_examples/launch/floor2.xml index a669ec1b..a9053b27 100644 --- a/src/svea_examples/launch/floor2.xml +++ b/src/svea_examples/launch/floor2.xml @@ -1,38 +1,102 @@ - + - - - - - - + + + + + + + - + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/svea_examples/scripts/pure_pursuit.py b/src/svea_examples/scripts/pure_pursuit.py index c700950a..ef06023a 100755 --- a/src/svea_examples/scripts/pure_pursuit.py +++ b/src/svea_examples/scripts/pure_pursuit.py @@ -2,17 +2,13 @@ import numpy as np -from geometry_msgs.msg import PoseWithCovarianceStamped -from tf_transformations import quaternion_from_euler - from svea_core.interfaces import LocalizationInterface from svea_core.controllers.pure_pursuit import PurePursuitController -from svea_core.interfaces import ActuationInterface +from svea_core.interfaces import ActuationInterface, ShowMarker, ShowPath from svea_core import rosonic as rx -from svea_core.utils import PlaceMarker, ShowPath -class pure_pursuit(rx.Node): # Inherit from rx.Node +class pure_pursuit(rx.Node): r"""Pure Pursuit example script for SVEA. @@ -46,24 +42,23 @@ class pure_pursuit(rx.Node): # Inherit from rx.Node points: List of points defining the path to follow. actuation: Actuation interface for sending control commands. localizer: Localization interface for receiving state information. - mark: PlaceMarker for visualizing the goal. + goal_mark: ShowMarker for visualizing the goal. path: ShowPath for visualizing the path. """ DELTA_TIME = 0.1 TRAJ_LEN = 20 - points = rx.Parameter(['[-2.3, -7.1]', '[10.5, 11.7]', '[5.7, 15.0]', '[-7.0, -4.0]']) - state = rx.Parameter([-7.4, -15.3, 0.9, 0.0]) # x, y, yaw, vel + points = rx.Parameter('[[-2.3, -7.1], [10.5, 11.7], [5.7, 15.0], [-7.0, -4.0]]') target_velocity = rx.Parameter(0.6) # Interfaces + actuation = ActuationInterface() localizer = LocalizationInterface() - # Goal Visualization - mark = PlaceMarker() - # Path Visualization - #path = ShowPath() + + goal_marker = ShowMarker() # for goal visualization + path = ShowPath() # for path visualization def on_startup(self): """ @@ -76,9 +71,8 @@ def on_startup(self): The controller is set to not finished initially, and a timer is created to call the loop method at regular intervals. """ - # Convert POINTS to numerical lists if loaded as strings - if isinstance(self.points[0], str): - self._points = [eval(point) for point in self.points] + # Convert parameter to numerical list + self._points = eval(self.points) self.controller = PurePursuitController() self.controller.target_velocity = self.target_velocity @@ -88,7 +82,7 @@ def on_startup(self): self.curr = 0 self.goal = self._points[self.curr] - self.mark.marker('goal','blue',self.goal) + self.goal_marker.place([*self.goal, 0.5], color='blue') self.update_traj(x, y) self.create_timer(self.DELTA_TIME, self.loop) @@ -110,7 +104,7 @@ def loop(self): self.update_traj(x, y) steering, velocity = self.controller.compute_control(state) - self.get_logger().info(f"Steering: {steering}, Velocity: {velocity}") + # self.get_logger().info(f"Steering: {steering}, Velocity: {velocity}") self.actuation.send_control(steering, velocity) def update_goal(self): @@ -124,7 +118,7 @@ def update_goal(self): self.goal = self._points[self.curr] self.controller.is_finished = False # Mark the goal - self.mark.marker('goal','blue',self.goal) + self.goal_marker.place([*self.goal, 0.5], color='blue') def update_traj(self, x, y): """ @@ -137,7 +131,7 @@ def update_traj(self, x, y): ys = np.linspace(y, self.goal[1], self.TRAJ_LEN) self.controller.traj_x = xs self.controller.traj_y = ys - #self.path.publish_path(xs,ys) + self.path.publish_path(xs,ys) if __name__ == '__main__': pure_pursuit.main() diff --git a/src/svea_localization/launch/localization.py b/src/svea_localization/launch/localization.py deleted file mode 100644 index 3afcb1af..00000000 --- a/src/svea_localization/launch/localization.py +++ /dev/null @@ -1,124 +0,0 @@ -# Copyright (c) 2018 Intel Corporation -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from nav2_common.launch import RewrittenYaml - - -def generate_launch_description(): - # Get the launch directory - bringup_dir = get_package_share_directory('svea_localization') - svea_core_dir = get_package_share_directory('svea_core') - - namespace = LaunchConfiguration('namespace') - map_yaml_file = LaunchConfiguration('map') - use_sim_time = LaunchConfiguration('use_sim_time') - autostart = LaunchConfiguration('autostart') - params_file = LaunchConfiguration('params_file') - # lifecycle_nodes = ['map_server', 'amcl'] - lifecycle_nodes = ['amcl'] - initial_pose_a = LaunchConfiguration('initial_pose_a') - initial_pose_x = LaunchConfiguration('initial_pose_x') - initial_pose_y = LaunchConfiguration('initial_pose_y') - - # Map fully qualified names to relative ones so the node's namespace can be prepended. - # In case of the transforms (tf), currently, there doesn't seem to be a better alternative - # https://github.com/ros/geometry2/issues/32 - # https://github.com/ros/robot_state_publisher/pull/30 - # TODO(orduno) Substitute with `PushNodeRemapping` - # https://github.com/ros2/launch_ros/issues/56 - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] - - # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'use_sim_time': use_sim_time, - 'yaml_filename': map_yaml_file, - 'initial_pose_a': initial_pose_a, - 'initial_pose_x': initial_pose_x, - 'initial_pose_y': initial_pose_y} - - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True) - - return LaunchDescription([ - # Set env var to print messages to stdout immediately - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - - DeclareLaunchArgument( - 'namespace', default_value='', - description='Top-level namespace'), - - DeclareLaunchArgument( - 'map', - default_value=os.path.join(svea_core_dir, 'maps', 'sml.yaml'), - description='Full path to map yaml file to load'), - - DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack'), - - DeclareLaunchArgument( - 'params_file', - default_value=os.path.join(bringup_dir, 'params', 'localize.yaml'), - description='Full path to the ROS2 parameters file to use'), - - DeclareLaunchArgument("initial_pose_x", default_value="0.0" ), - - DeclareLaunchArgument("initial_pose_y", default_value="0.0" ), - - DeclareLaunchArgument("initial_pose_a", default_value="1.52" ), - - # Node( - # package='nav2_map_server', - # executable='map_server', - # name='map_server', - # output='screen', - # parameters=[configured_params], - # remappings=remappings), - - Node( - package='nav2_amcl', - executable='amcl', - name='amcl', - output='screen', - parameters=[configured_params, - {'initial_pose.x': initial_pose_x}, - {'initial_pose.y': initial_pose_y}, - {'initial_pose.yaw': initial_pose_a}], - remappings=remappings), - - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_localization', - output='screen', - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]) - ]) diff --git a/src/svea_localization/launch/localization.xml b/src/svea_localization/launch/localization.xml new file mode 100644 index 00000000..67097b58 --- /dev/null +++ b/src/svea_localization/launch/localization.xml @@ -0,0 +1,127 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/svea_localization/launch/localize.xml b/src/svea_localization/launch/localize.xml deleted file mode 100644 index 47b88479..00000000 --- a/src/svea_localization/launch/localize.xml +++ /dev/null @@ -1,155 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/svea_localization/launch/sensors/lidar.xml b/src/svea_localization/launch/sensors/lidar.xml index c0a8e336..aef66a33 100644 --- a/src/svea_localization/launch/sensors/lidar.xml +++ b/src/svea_localization/launch/sensors/lidar.xml @@ -5,8 +5,8 @@ - - + + @@ -15,7 +15,7 @@ - + diff --git a/src/svea_localization/launch/transforms.xml b/src/svea_localization/launch/transforms.xml index 1ce8bae9..24eac92b 100644 --- a/src/svea_localization/launch/transforms.xml +++ b/src/svea_localization/launch/transforms.xml @@ -1,22 +1,50 @@ - - - - - - - - - - - + - - \ No newline at end of file + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/svea_localization/params/amcl/localize.yaml b/src/svea_localization/params/amcl/localize.yaml index 38a2a81e..23dc4b55 100644 --- a/src/svea_localization/params/amcl/localize.yaml +++ b/src/svea_localization/params/amcl/localize.yaml @@ -44,7 +44,7 @@ amcl: resample_interval: 1 tf_broadcast: true map_topic: map - scan_topic: /scan + scan_topic: /scan/filtered set_initial_pose: true initial_pose: x: 0.0 diff --git a/src/svea_localization/params/robot_localization/global_ekf.yaml b/src/svea_localization/params/robot_localization/global_ekf.yaml index 8b382f24..6ea81a76 100644 --- a/src/svea_localization/params/robot_localization/global_ekf.yaml +++ b/src/svea_localization/params/robot_localization/global_ekf.yaml @@ -31,6 +31,17 @@ base_link_frame: base_link # Defaults to "base_link" if unspecified world_frame: map # Defaults to the value of odom_frame if unspecified + ########################################## + # Initial State ########################## + + initial_state: [ + 0.0, 0.0, 0.0, # x, y, z, + 0.0, 0.0, 0.0, # roll, pitch, yaw, + 0.0, 0.0, 0.0, # x_dot, y_dot, z_dot, + 0.0, 0.0, 0.0, # roll_dot, pitch_dot, yaw_dot, + 0.0, 0.0, 0.0 # x_dot_dot, y_dot_dot, z_dot_dot, + ] + ########################################## # GPS Odometry ########################### odom0: odometry/gps diff --git a/util/config.sh b/util/config.sh index 7d7d1c39..094add13 100755 --- a/util/config.sh +++ b/util/config.sh @@ -9,6 +9,12 @@ # BUILD_CONFIG="base-amd64" # BUILD_CONFIG="base-arm64" +# # EL2425 - build base image +# IMAGE_TAG="ghcr.io/kth-sml/svea:el2425-base" +# BUILD_CONFIG="ghcr" + +# EL2425 - normal build +BUILD_TAG="ghcr.io/kth-sml/svea:el2425-base" main() { @@ -27,6 +33,7 @@ main() { BUILD_CONFIG="$( \ switch "$BUILD_CONFIG" \ "host" "$_host" \ + "base" "base-$_host" \ "base-host" "base-$_host" \ "$BUILD_CONFIG" \ )" @@ -45,14 +52,6 @@ main() { withdefault BUILD_TAG "ghcr.io/kth-sml/svea:latest" withdefault IMAGE_TAG "$REPOSITORY_NAME" withdefault IMAGE_PUSH "0" - elif [ "$BUILD_CONFIG" = "base-host" ]; then - # building for host platform - withdefault BUILD_PLATFORM "$(uname -m)" - withdefault BUILD_CONTEXT "$REPOSITORY_PATH" - withdefault BUILD_FILE "docker/Dockerfile.base" - withdefault BUILD_TAG "ros:$ROSDISTRO-ros-base" - withdefault IMAGE_TAG "ghcr.io/kth-sml/svea:latest" - withdefault IMAGE_PUSH "0" elif [ "$BUILD_CONFIG" = "base-amd64" ]; then # building for x86_64 withdefault BUILD_PLATFORM "linux/amd64" @@ -83,7 +82,7 @@ main() { fi if [ "$BUILD_FILE" = "docker/Dockerfile.base" ]; then - withdefault USER_CREDENTIALS "svea:SVEA-Pass!" # TODO: Setup regular user instead of root + withdefault USER_CREDENTIALS "root:SVEA-Pass!" # TODO: Setup regular user instead of root fi withdefault CONTAINER_NAME "$REPOSITORY_NAME" @@ -262,4 +261,4 @@ isempty() { ################################################################################ ################################################################################ -main +main \ No newline at end of file diff --git a/util/run b/util/run index 8a61b923..ed351310 100755 --- a/util/run +++ b/util/run @@ -20,7 +20,8 @@ fi append ARGS \ -it \ -p 2222:22 \ - -p "8765:8765" \ + -p 7447:7447 \ + -p 8765:8765 \ -v "$SHARED_VOLUME" \ -e "TERM=xterm-256color" \ --rm