Closed amodpatil1 closed 2 weeks ago
Hey @amodpatil1, have you source (source install/setup.bash
) after using colcon?
Yes, I did source the workspace before the running the command and I tried it multiple times and I also tried doing from start as well, but it did not work.
I think there is some problem with the action_client_demo.py
Have you colcon all the yasmin packages (yasmin, yasmin_msgs, yasmin_demo, yasmin_ros, yasmin_viewe) or only yasmin?
I did it for yasmin only. Shouldn't the entire package get built when I do Colcon build?
No, since they are different packages.
okay I will try that.
Failed <<< yasmin_demo [7.38s, exited with code 2]
Summary: 0 packages finished [7.67s] 1 package failed: yasmin_demo 1 package had stderr output: yasmin_demo
I built all other packages but the demo is only facing an issue
That is a problem with the ROS 2 distro. Foxy EOL was on June 20th, 2023. Thus, yasmin was updated to Humble.
I have just added support for Foxy. It should work now.
okay I will try
Failed <<< yasmin_demo [6.82s, exited with code 2]
Summary: 0 packages finished [7.10s] 1 package failed: yasmin_demo 1 package had stderr output: yasmin_demo
still the same error
Pull and try again. I have done another update.
It is working now. But now there is this ros2 run demo_nodes_py add_two_ints_servers No executable found. I am sorry for too many questions
The service demo is not running
Both the commands are not executable
You have to install the ROS 2 demo packages (ros-humble-demo-nodes-py, ros-humble-demo-nodes-cpp, ros-humble-action-tutorials-py, ros-humble-action-tutorials-cpp).
I am able to find the demos for foxy, can you please help me out?
Have you tried to install them with sudo apt install ros-foxy-demo-nodes-py, ros-foxy-demo-nodes-cpp, ros-foxy-action-tutorials-py, ros-foxy-action-tutorials-cpp
?
Yes it started working but I am not able to understand how can I build a custom FSM on yasmin. Could we please have a call on this where you can explain me the implementation of yasmin on my project? I am a Masters student at Coburg University of applied science in germany and I am currently working on an autonomous vehicle, where the vehicle needs to park itself autonomously in the parking spot and I am working on the behaviour planning component which decides the state of the vehicle.
@amodpatil1, you have several examples in the README that includes basic states, ROS 2-based states and a demo with Nav2. If you want further explanations, you can send me an email.
Do I need to have a separate package for my state machine?
Yes, you should create a new package with your code.
do I need to add the dependencies from yasmin into my package.xml as well?
No, but you can add them to the package.xml.
What would be the effieicent way to implement yasmin? Would it be via creating a package or a YAML file?
Can I use this as a reference for my custom machine?
from typing import Dict, List, Union from threading import Lock from yasmin.state import State from yasmin.blackboard import Blackboard
class StateMachine(State): def init(self, outcomes: List[str]) -> None:
super().__init__(outcomes)
self._states = {}
self._start_state = None
self.__current_state = None
self.__current_state_lock = Lock()
def add_state(
self,
name: str,
state: State,
transitions: Dict[str, str] = None
) -> None:
if not transitions:
transitions = {}
self._states[name] = {
"state": state,
"transitions": transitions
}
if not self._start_state:
self._start_state = name
def set_start_state(self, name: str) -> None:
self._start_state = name
def get_start_state(self) -> str:
return self._start_state
def cancel_state(self) -> None:
super().cancel_state()
with self.__current_state_lock:
if self.__current_state:
self._states[self.__current_state]["state"].cancel_state()
def execute(self, blackboard: Blackboard) -> str:
with self.__current_state_lock:
self.__current_state = self._start_state
while True:
with self.__current_state_lock:
state = self._states[self.__current_state]
outcome = state["state"](blackboard)
# check outcome belongs to state
if outcome not in state["state"].get_outcomes():
raise Exception(
f"Outcome ({outcome}) is not register in state {self.__current_state}")
# translate outcome using transitions
if outcome in state["transitions"]:
outcome = state["transitions"][outcome]
# outcome is an outcome of the sm
if outcome in self.get_outcomes():
with self.__current_state_lock:
self.__current_state = None
return outcome
# outcome is a state
elif outcome in self._states:
with self.__current_state_lock:
self.__current_state = outcome
# outcome is not in the sm
else:
raise Exception(f"Outcome ({outcome}) without transition")
def get_states(self) -> Dict[str, Union[State, Dict[str, str]]]:
return self._states
def get_current_state(self) -> str:
with self.__current_state_lock:
if self.__current_state:
return self.__current_state
return ""
def __str__(self) -> str:
return f"StateMachine: {self._states}"
What would be the effieicent way to implement yasmin? Would it be via creating a package or a YAML file?
I prefer using new packages. What do you mean by a YAML file?
Can I use this as a reference for my custom machine?
What do you mean by reference?
I mean can I write my state machine using the one you wrote as a base for mine custom state machine
states:
id: idle on_enter:
id: driving on_enter:
This is the example for the yaml file I have not implemented this
I'm really having implementing this to a custom machine
You can use any of the state machines from the demos but I recommend you create a new ROS 2 package with your code.
Okay I will try it
Do I need to have a blackboard as well? yasmin/include/yasmin/blackboard
The blackboard is shared between all states and nested state machines. You can create a blackboard object if you want to provide some initial data for your main state machine. Otherwise, you can avoid creating it and you can use the default blackboard in your states.
I created the code for my custom state machine but now the issue is that I am getting the following error "Traceback (most recent call last):
File "/home/amodpatil/ros2/src/behstate/behstate/behmac.py", line 21, in
That seems a problem with colcon and source. How are you running your state machine, using ros2 run
?
This is how I am running the code python3 behmac.py
That should also work. Make sure to colcon all yasmin package and use source install/setup.bash
.
It ran thank you so much
if I have any doubts, how can I reach you?
Please feel free to write me here or to my email (the one shown in my account).
Okay, I have one doubt if I want to use multiple components within the state machine then how can I incorporate them within my code?
import time import math import rclpy from yasmin import State from yasmin import Blackboard from yasmin import StateMachine from yasmin_viewer import YasminViewerPub
class IdleState(State): def init(self) -> None: super().init(["driving_state"])
def execute(self, blackboard: Blackboard) -> str:
print("Executing state IDLE")
# Simulate waiting for start trigger from adapt_vi
while not blackboard.adapt_vi_start_trigger:
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)
# Dummy condition checks
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")
return "stop_near_park"
return "drive"
class StopObstacleState(State): def init(self) -> None: super().init(["idle"])
def execute(self, blackboard: Blackboard) -> str:
print("Executing state STOP_OBSTACLE")
time.sleep(2)
return "idle"
class StopNearParkState(State): def init(self) -> None: super().init(["idle"])
def execute(self, blackboard: Blackboard) -> str:
print("Executing state STOP_NEAR_PARK")
time.sleep(2)
return "idle"
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 = None # Initialize adapt_trajp
blackboard.adapt_envmod = False # Initialize adapt_envmod as no obstacle detected
blackboard.parking_threshold = 0.5 # Distance threshold for parking spot detection
# 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={
"idle": "IDLE"
}
)
sm.add_state(
"STOP_NEAR_PARK",
StopNearParkState(),
transitions={
"idle": "IDLE"
}
)
# pub FSM info
YasminViewerPub("YASMIN_DEMO", sm)
# Set the start trigger to True after a delay to simulate an external trigger
time.sleep(2)
blackboard.adapt_vi_start_trigger = True
# Simulate vehicle driving and detecting an obstacle and nearing parking spot
time.sleep(5)
blackboard.adapt_envmod = True # Simulate obstacle detection
# execute FSM
outcome = sm.execute(blackboard)
print(outcome)
# shutdown ROS 2
rclpy.shutdown()
if name == "main": main()
What do you mean by multiple components?
Autonomous Driving And Parking Technology - Git HS-Coburg.pdf This is my project repositary and in this there are multiple components. I want to incoporate these components within my state machine
I am not getting what do you mean by components. If you mean other ROS 2 nodes, you can use YASMIN states. If you want to use other topics, actions or services, you can use the ROS 2-based states (ServiceState, ActionState). For instance, you can have a state for the navigation/driving. Here you have an example using Navigation2. Besides, you can subscribe, such as subscribing to the object detection topic; or publish, for example, twist msgs to control you car.
Yes I mean multiple nodes and I want to subscribe to multiple nodes at a time which are mentioned in the Repository. I am making a change in the https://github.com/uleroboticsgroup/yasmin/blob/main/yasmin_ros/yasmin_ros/yasmin_node.py in order to subscribe to multiple topics simultaneously.
Why do you need to modify it? You can use it as a ROS 2 node to create multiple subscribers or you can use the MonitorState.
me and professor discussed on this and he sent me a code for this https://github.com/uleroboticsgroup/yasmin/blob/main/yasmin_ros/yasmin_ros/yasmin_node.py since I was not able to subscribe to a topic and which was supposed change the state from Idle to driving https://github.com/uleroboticsgroup/yasmin/issues/19#issuecomment-2165073540
This is the code
import time import rclpy from yasmin import State from yasmin import Blackboard from yasmin import StateMachine from yasmin_ros import YasminNode from yasmin_viewer import YasminViewerPub from std_msgs.msg import String
class MinimalSubscriber(YasminNode):
def __init__(self, blackboard):
super().__init__()
self.subscription = self.create_subscription(
String,
'topic',
self.listener_callback,
10)
self.subscription # prevent unused variable warning
self.blackboard = blackboard
def listener_callback(self, msg):
self.get_logger().info('I heard: "%s"' % msg.data)
if msg.data == "trigger":
self.blackboard.trigger = True
elif msg.data == "back":
self.blackboard.trigger = False
class FooState(State): def init(self, blackboard) -> None: super().init(["outcome1", "outcome2"]) self.counter = 0 self.blackboard = blackboard
def execute(self, blackboard: Blackboard) -> str:
print("Executing state FOO")
#time.sleep(3)
while True:
if self.blackboard.trigger == True:
return "outcome1"
time.sleep(1)
class BarState(State): def init(self, blackboard) -> None: super().init(outcomes=["outcome3"]) self.blackboard = blackboard
def execute(self, blackboard: Blackboard) -> str:
print("Executing state BAR")
time.sleep(3)
while True:
if self.blackboard.trigger == False:
return "outcome3"
time.sleep(1)
def main():
print("yasmin_demo")
# init ROS 2
rclpy.init()
# create a FSM
sm = StateMachine(outcomes=["outcome4"])
blackboard = Blackboard()
blackboard.trigger = False
# add states
sm.add_state(
"FOO",
FooState(blackboard),
transitions={
"outcome1": "BAR",
"outcome2": "outcome4"
}
)
sm.add_state(
"BAR",
BarState(blackboard),
transitions={
"outcome3": "FOO"
}
)
node = MinimalSubscriber(blackboard)
# pub FSM info
YasminViewerPub("YASMIN_DEMO", sm)
# execute FSM
outcome = sm()
print(outcome)
# shutdown ROS 2
rclpy.shutdown()
if name == "main": main()
You don't need to modify or create new nodes. You have two options to subscribe to a topic using YASMIN:
Creating a state that uses the yasmin_node similar to the ROS 2-based states.
from yasmin import State
from yasmin_ros.yasmin_node import YasminNode
class TopicState(State): def init(self): super().init(outcomes) self._node = YasminNode.get_instance() self._sub = self._node.create_subscription( msg_type, topic_name, self.__callback, qos)
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.