Not able to run the demo

[amodpatil1]

colcon build --packages-select yasmin ros2 run yasmin_demo yasmin_demo.py Package 'yasmin_demo' not found not able to run the command. please help me out.

[amodpatil1]

I am facing this error

Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. [WARN]

[mgonzs13]

As stated in the log, you are using two nodes with the same name. Which nodes are you using?

[amodpatil1]

I have changed the code now and still I am not able listen to the topic

[mgonzs13]

So now what are you using, the MonitorState or a new node? Have you checked if there are msgs using echo? Are you using the correct QoS?

[amodpatil1]

I am using a new node and I checked using ros2 topic echo but the node is not able to listen to that topic

[mgonzs13]

Unfortunately, I can't help you without taking a look at your code. You can send it to me by email. The QoS of the topics may also affect if the subscriber and the publisher have different QoS.

[amodpatil1]

I have sent you the code via email. The email id from which I have sent you the email is amodpatil90@gmail.com

[mgonzs13]

I have tested your code by publishing msgs using the ROS 2 cli and it works, I mean I am able to produce transitions from the states.

[amodpatil1]

Are you able to listen to multiple topics simultaneously?

[amodpatil1]

Can you send me the screenshot of it?

[mgonzs13]

Here you have a video. I am pushing msgs in different topics and different tabs. Is this what you want?

https://github.com/uleroboticsgroup/yasmin/assets/25979134/2c39246a-5acf-4d76-8037-ad36108cf0b6

[amodpatil1]

Yes this is what I want. Did you do any changes in the yasmin_node?

[mgonzs13]

No, I haven't changed anything. I have just executed it as you gave me.

[amodpatil1]

Now I just want to listen to multiple topics simultaneously when the state machine is running, after this my state machine will complete

[amodpatil1]

I am not able to listen to a single node as well.

[mgonzs13]

Now I just want to listen to multiple topics simultaneously when the state machine is running, after this my state machine will complete

Isn't it what I just showed you in the video?

I am not able to listen to a single node as well.

What do you mean by listening to a node?

[amodpatil1]

Yes, I tried the same approach as you did but there was no change in the states.

[mgonzs13]

I don't know what could be happening. Put some logs in the subscriber callbacks to check if your node is receiving msgs. It may also be a problem with the ROS 2 distro, I am using Humble.

[amodpatil1]

It ran It was my mistake it was executing the wrong executable.

[amodpatil1]

!/usr/bin/env python3

Copyright (C) 2023 Miguel Ángel González Santamarta

This program is free software: you can redistribute it and/or modify

it under the terms of the GNU General Public License as published by

the Free Software Foundation, either version 3 of the License, or

(at your option) any later version.

This program is distributed in the hope that it will be useful,

but WITHOUT ANY WARRANTY; without even the implied warranty of

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the

GNU General Public License for more details.

You should have received a copy of the GNU General Public License

along with this program. If not, see https://www.gnu.org/licenses/.

import time import math import rclpy from rclpy.node import Node from std_msgs.msg import Bool from geometry_msgs.msg import Pose from yasmin import State from yasmin import Blackboard from yasmin import StateMachine from yasmin_viewer import YasminViewerPub

class FSMNode(Node): def init(self, blackboard): super().init('fsm_node') self.blackboard = blackboard

    self.create_subscription(Bool, '/vi_start', self.vi_start_callback, 10)
    self.create_subscription(Pose, '/loc_pose', self.loc_pose_callback, 10)
    self.create_subscription(Bool, '/stop', self.stop_callback, 10)
    self.create_subscription(Pose, '/route', self.route_callback, 10)
    self.create_subscription(Bool, '/trajpf', self.trajpf_callback, 10)
    self.create_subscription(Bool, '/trajpr', self.trajpr_callback, 10)
    print("init")

def vi_start_callback(self, msg):
    self.blackboard.adapt_vi_start_trigger = msg.data

def loc_pose_callback(self, msg):
    self.blackboard.adapt_loc = (msg.position.x, msg.position.y)

def stop_callback(self, msg):
    self.blackboard.adapt_envmod = msg.data
    # If an obstacle is detected, inform adapt_trajp to stop
    if msg.data:  # Assuming True means obstacle detected
        stop_msg = Bool()
        stop_msg.data = True
        self.blackboard.adapt_trajp_publisher.publish(stop_msg)

def route_callback(self, msg):
    self.blackboard.adapt_roucomp = (msg.position.x, msg.position.y)

def trajpf_callback(self, msg):
    self.blackboard.trajpf_complete = msg.data

def trajpr_callback(self, msg):
    self.blackboard.trajpr_complete = msg.data

define state Idle

class IdleState(State): def init(self) -> None: super().init(["driving_state"]) self.logger = rclpy.logging.get_logger('IdleState')

def execute(self, blackboard: Blackboard) -> str:
    print("Executing state IDLE")
    # Wait for start trigger from adapt_vi
    while not blackboard.adapt_vi_start_trigger:
        self.logger.info('going to sleep')
        time.sleep(0.1)
    return "driving_state"

define state Driving

class DrivingState(State): def init(self) -> None: super().init(["drive", "stop_obstacle", "stop_near_park"])

def execute(self, blackboard: Blackboard) -> str:
    print("Executing state DRIVING")
    time.sleep(3)

    # Check for obstacle detection
    if blackboard.adapt_envmod:
        print("Obstacle detected by adapt_envmod")
        return "stop_obstacle"

    # Calculate distance to parking spot
    vehicle_location = blackboard.adapt_loc
    parking_spot_location = blackboard.adapt_roucomp
    distance_to_parking_spot = math.sqrt(
        (parking_spot_location[0] - vehicle_location[0]) ** 2 +
        (parking_spot_location[1] - vehicle_location[1]) ** 2
    )

    if distance_to_parking_spot < blackboard.parking_threshold:
        print("Near parking spot")
        blackboard.adapt_trajp = True
        return "stop_near_park"

    return "drive"

define state StopObstacle

class StopObstacleState(State): def init(self) -> None: super().init(["stop"])

def execute(self, blackboard: Blackboard) -> str:
    print("Executing state STOP_OBSTACLE")
    time.sleep(2)

    # Check if adapt_envmod detects an obstacle (assuming adapt_envmod is True means obstacle detected)
    if blackboard.adapt_envmod:
        print("Obstacle detected by adapt_envmod")
        # Inform adapt_trajp to stop
        stop_msg = Bool()
        stop_msg.data = True
        blackboard.adapt_trajp_publisher.publish(stop_msg)

    return "stop"

define state StopNearPark

class StopNearParkState(State): def init(self) -> None: super().init(["park"])

def execute(self, blackboard: Blackboard) -> str:
    print("Executing state STOP_NEAR_PARK")
    time.sleep(2)
    # Trigger parking procedure
    blackboard.adapt_trajp = True
    return "park"

define state Park

class ParkState(State): def init(self) -> None: super().init(["parked"])

def execute(self, blackboard: Blackboard) -> str:
    print("Executing state PARK")

    while not blackboard.trajpf_complete:
        time.sleep(0.1)

    while not blackboard.trajpr_complete:
        time.sleep(0.1)

    print("Vehicle parked")
    return "parked"

main

def main():

print("yasmin_demo")

# init ROS 2
rclpy.init()

# create a FSM
sm = StateMachine(outcomes=["finished"])

# create and set blackboard
blackboard = Blackboard()
blackboard.obstacle_detected = False
blackboard.near_parking_spot = False
blackboard.adapt_vi_start_trigger = False  # Initial state of the start trigger
blackboard.adapt_loc = (0, 0)  # Initialize adapt_loc with a tuple representing coordinates
blackboard.adapt_obj = None  # Initialize adapt_obj
blackboard.adapt_livtrac = None  # Initialize adapt_livtrac
blackboard.adapt_roucomp = (10, 10)  # Initialize adapt_roucomp with a dummy parking spot location
blackboard.adapt_trajp = False  # Initialize adapt_trajp
blackboard.adapt_envmod = False  # Initialize adapt_envmod as no obstacle detected
blackboard.parking_threshold = 1.0  # Distance threshold for parking spot detection
blackboard.trajpf_complete = False  # Initialize trajpf_complete
blackboard.trajpr_complete = False  # Initialize trajpr_complete

# init ROS 2 node
fsm_node = FSMNode(blackboard)
blackboard.adapt_trajp_publisher = fsm_node.create_publisher(Bool, '/adapt_trajp', 10)

# add states
sm.add_state(
    "IDLE",
    IdleState(),
    transitions={
        "driving_state": "DRIVING"
    }
)
sm.add_state(
    "DRIVING",
    DrivingState(),
    transitions={
        "drive": "DRIVING",
        "stop_obstacle": "STOP_OBSTACLE",
        "stop_near_park": "STOP_NEAR_PARK"
    }
)
sm.add_state(
    "STOP_OBSTACLE",
    StopObstacleState(),
    transitions={
        "stop": "STOP_OBSTACLE"  # Ensure outcome is "stop" when obstacle detected
    }
)
sm.add_state(
    "STOP_NEAR_PARK",
    StopNearParkState(),
    transitions={
        "park": "PARK"
    }
)
sm.add_state(
    "PARK",
    ParkState(),
    transitions={
        "parked": "PARKED"  # Changed outcome to "parked"
    }
)

# pub FSM info
YasminViewerPub("YASMIN_DEMO", sm)

# execute FSM
import threading
def execute_fsm():
    outcome = sm.execute(blackboard)
    print(outcome)

fsm_thread = threading.Thread(target=execute_fsm)
fsm_thread.start()

# Spin ROS 2 node
rclpy.spin(fsm_node)

# Shutdown
fsm_thread.join()
rclpy.shutdown()

if name == "main": main()

This is the code and now I am not able to run the viewer node.

[amodpatil1]

It was running before

[mgonzs13]

I have just run it and everything works. Which environment are you using (Ubuntu version, ROS 2 distro, etc)?

[amodpatil1]

ubuntu 20.04 and ros2 foxy

[amodpatil1]

I am able to see all the state changes

[amodpatil1]

https://github.com/uleroboticsgroup/yasmin/assets/168272759/167fb78a-b839-405c-84d8-f98815cb19d0

[mgonzs13]

Update yasmin by using git pull and try it again.

[amodpatil1]

Okay

[amodpatil1]

I will try now

[amodpatil1]

[amodpatil1]

There is no change

[amodpatil1]

I will try by cloning it again

[amodpatil1]

Not working even after cloning it again

[mgonzs13]

Have you used colcon and source after updating?

[amodpatil1]

Yes

[mgonzs13]

I have just found an error with the transitions of your state machine. You are trying to move to a state/outcome named "PARKED" that does not exist. Changing it to an existing outcome or state solves your error. Here you have an example:

#!/usr/bin/env python3

import threading
import time
import math
import rclpy
from rclpy.node import Node
from std_msgs.msg import Bool
from geometry_msgs.msg import Pose
from yasmin import State
from yasmin import Blackboard
from yasmin import StateMachine
from yasmin_viewer import YasminViewerPub

class FSMNode(Node):
    def __init__(self, blackboard):
        super().__init__('fsm_node')
        self.blackboard = blackboard

        self.create_subscription(Bool, '/vi_start', self.vi_start_callback, 10)
        self.create_subscription(Pose, '/loc_pose', self.loc_pose_callback, 10)
        self.create_subscription(Bool, '/stop', self.stop_callback, 10)
        self.create_subscription(Pose, '/route', self.route_callback, 10)
        self.create_subscription(Bool, '/trajpf', self.trajpf_callback, 10)
        self.create_subscription(Bool, '/trajpr', self.trajpr_callback, 10)
        print("init")

    def vi_start_callback(self, msg):
        self.blackboard.adapt_vi_start_trigger = msg.data

    def loc_pose_callback(self, msg):
        self.blackboard.adapt_loc = (msg.position.x, msg.position.y)

    def stop_callback(self, msg):
        self.blackboard.adapt_envmod = msg.data
        # If an obstacle is detected, inform adapt_trajp to stop
        if msg.data:  # Assuming True means obstacle detected
            stop_msg = Bool()
            stop_msg.data = True
            self.blackboard.adapt_trajp_publisher.publish(stop_msg)

    def route_callback(self, msg):
        self.blackboard.adapt_roucomp = (msg.position.x, msg.position.y)

    def trajpf_callback(self, msg):
        self.blackboard.trajpf_complete = msg.data

    def trajpr_callback(self, msg):
        self.blackboard.trajpr_complete = msg.data

class IdleState(State):
    def __init__(self) -> None:
        super().__init__(["driving_state"])
        self.logger = rclpy.logging.get_logger('IdleState')

    def execute(self, blackboard: Blackboard) -> str:
        print("Executing state IDLE")
        # Wait for start trigger from adapt_vi
        while not blackboard.adapt_vi_start_trigger:
            self.logger.info('going to sleep')
            time.sleep(0.1)
        return "driving_state"

class DrivingState(State):
    def __init__(self) -> None:
        super().__init__(["drive", "stop_obstacle", "stop_near_park"])

    def execute(self, blackboard: Blackboard) -> str:
        print("Executing state DRIVING")
        time.sleep(3)

        # Check for obstacle detection
        if blackboard.adapt_envmod:
            print("Obstacle detected by adapt_envmod")
            return "stop_obstacle"

        # Calculate distance to parking spot
        vehicle_location = blackboard.adapt_loc
        parking_spot_location = blackboard.adapt_roucomp
        distance_to_parking_spot = math.sqrt(
            (parking_spot_location[0] - vehicle_location[0]) ** 2 +
            (parking_spot_location[1] - vehicle_location[1]) ** 2
        )

        if distance_to_parking_spot < blackboard.parking_threshold:
            print("Near parking spot")
            blackboard.adapt_trajp = True
            return "stop_near_park"

        return "drive"

class StopObstacleState(State):
    def __init__(self) -> None:
        super().__init__(["stop"])

    def execute(self, blackboard: Blackboard) -> str:
        print("Executing state STOP_OBSTACLE")
        time.sleep(2)

        # Check if adapt_envmod detects an obstacle (assuming adapt_envmod is True means obstacle detected)
        if blackboard.adapt_envmod:
            print("Obstacle detected by adapt_envmod")
            # Inform adapt_trajp to stop
            stop_msg = Bool()
            stop_msg.data = True
            blackboard.adapt_trajp_publisher.publish(stop_msg)

        return "stop"

class StopNearParkState(State):
    def __init__(self) -> None:
        super().__init__(["park"])

    def execute(self, blackboard: Blackboard) -> str:
        print("Executing state STOP_NEAR_PARK")
        time.sleep(2)
        # Trigger parking procedure
        blackboard.adapt_trajp = True
        return "park"

class ParkState(State):
    def __init__(self) -> None:
        super().__init__(["parked"])

    def execute(self, blackboard: Blackboard) -> str:
        print("Executing state PARK")

        while not blackboard.trajpf_complete:
            time.sleep(0.1)

        while not blackboard.trajpr_complete:
            time.sleep(0.1)

        print("Vehicle parked")
        return "parked"

def main():
    print("yasmin_demo")

    # init ROS 2
    rclpy.init()

    # create a FSM
    sm = StateMachine(outcomes=["finished"])

    # create and set blackboard
    blackboard = Blackboard()
    blackboard.obstacle_detected = False
    blackboard.near_parking_spot = False
    blackboard.adapt_vi_start_trigger = False  # Initial state of the start trigger
    # Initialize adapt_loc with a tuple representing coordinates
    blackboard.adapt_loc = (0, 0)
    blackboard.adapt_obj = None  # Initialize adapt_obj
    blackboard.adapt_livtrac = None  # Initialize adapt_livtrac
    # Initialize adapt_roucomp with a dummy parking spot location
    blackboard.adapt_roucomp = (10, 10)
    blackboard.adapt_trajp = False  # Initialize adapt_trajp
    blackboard.adapt_envmod = False  # Initialize adapt_envmod as no obstacle detected
    # Distance threshold for parking spot detection
    blackboard.parking_threshold = 1.0
    blackboard.trajpf_complete = False  # Initialize trajpf_complete
    blackboard.trajpr_complete = False  # Initialize trajpr_complete

    # init ROS 2 node
    fsm_node = FSMNode(blackboard)
    blackboard.adapt_trajp_publisher = fsm_node.create_publisher(
        Bool, '/adapt_trajp', 10)

    # add states
    sm.add_state(
        "IDLE",
        IdleState(),
        transitions={
            "driving_state": "DRIVING"
        }
    )
    sm.add_state(
        "DRIVING",
        DrivingState(),
        transitions={
            "drive": "DRIVING",
            "stop_obstacle": "STOP_OBSTACLE",
            "stop_near_park": "STOP_NEAR_PARK"
        }
    )
    sm.add_state(
        "STOP_OBSTACLE",
        StopObstacleState(),
        transitions={
            "stop": "STOP_OBSTACLE"  # Ensure outcome is "stop" when obstacle detected
        }
    )
    sm.add_state(
        "STOP_NEAR_PARK",
        StopNearParkState(),
        transitions={
            "park": "PARK"
        }
    )
    sm.add_state(
        "PARK",
        ParkState(),
        transitions={
            "parked": "finished"  # Changed outcome to "parked"
        }
    )

    # pub FSM info
    YasminViewerPub("YASMIN_DEMO", sm)

    # execute FSM

    def execute_fsm():
        outcome = sm.execute(blackboard)
        print(outcome)

    fsm_thread = threading.Thread(target=execute_fsm)
    fsm_thread.start()

    # Spin ROS 2 node
    rclpy.spin(fsm_node)

    # Shutdown
    fsm_thread.join()
    rclpy.shutdown()

if __name__ == "__main__":
    main()

[amodpatil1]

Hi, I have one issue that I want to stay one state only and that state should not have a transition. How do I do that?

[mgonzs13]

Hey @amodpatil1, all states should have transitions and a path to your state machine's final outcome. You can also make a transition from a state to itself, creating a loop and staying in that state till the transition to another state or a final outcome triggers.

[amodpatil1]

Okay