mgonzs13
commented
3 months ago Hey @amodpatil1, have you source (source install/setup.bash) after using colcon?
Closed amodpatil1 closed 2 weeks ago
mgonzs13
commented
3 months ago Hey @amodpatil1, have you source (source install/setup.bash) after using colcon?
amodpatil1
commented
3 months ago 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.
amodpatil1
commented
3 months ago I think there is some problem with the action_client_demo.py
mgonzs13
commented
3 months ago Have you colcon all the yasmin packages (yasmin, yasmin_msgs, yasmin_demo, yasmin_ros, yasmin_viewe) or only yasmin?
amodpatil1
commented
3 months ago I did it for yasmin only. Shouldn't the entire package get built when I do Colcon build?
mgonzs13
commented
3 months ago No, since they are different packages.
amodpatil1
commented
3 months ago okay I will try that.
amodpatil1
commented
3 months ago 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
mgonzs13
commented
3 months ago That is a problem with the ROS 2 distro. Foxy EOL was on June 20th, 2023. Thus, yasmin was updated to Humble.
mgonzs13
commented
3 months ago I have just added support for Foxy. It should work now.
amodpatil1
commented
3 months ago okay I will try
amodpatil1
commented
3 months ago 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
mgonzs13
commented
3 months ago Pull and try again. I have done another update.
amodpatil1
commented
3 months ago 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
amodpatil1
commented
3 months ago The service demo is not running
amodpatil1
commented
3 months ago Both the commands are not executable
mgonzs13
commented
3 months ago 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).
amodpatil1
commented
3 months ago I am able to find the demos for foxy, can you please help me out?
mgonzs13
commented
3 months ago 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?
amodpatil1
commented
3 months ago 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.
mgonzs13
commented
2 months ago @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.
amodpatil1
commented
2 months ago Do I need to have a separate package for my state machine?
mgonzs13
commented
2 months ago Yes, you should create a new package with your code.
amodpatil1
commented
2 months ago do I need to add the dependencies from yasmin into my package.xml as well?
mgonzs13
commented
2 months ago No, but you can add them to the package.xml.
amodpatil1
commented
2 months ago What would be the effieicent way to implement yasmin? Would it be via creating a package or a YAML file?
amodpatil1
commented
2 months ago 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?
amodpatil1
commented
2 months ago I mean can I write my state machine using the one you wrote as a base for mine custom state machine
amodpatil1
commented
2 months ago states:
id: idle on_enter:
id: driving on_enter:
This is the example for the yaml file I have not implemented this
amodpatil1
commented
2 months ago I'm really having implementing this to a custom machine
mgonzs13
commented
2 months ago You can use any of the state machines from the demos but I recommend you create a new ROS 2 package with your code.
amodpatil1
commented
2 months ago Okay I will try it
amodpatil1
commented
2 months ago Do I need to have a blackboard as well? yasmin/include/yasmin/blackboard
mgonzs13
commented
2 months ago 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.
amodpatil1
commented
2 months ago 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
mgonzs13
commented
2 months ago That seems a problem with colcon and source. How are you running your state machine, using ros2 run?
amodpatil1
commented
2 months ago This is how I am running the code python3 behmac.py
mgonzs13
commented
2 months ago That should also work. Make sure to colcon all yasmin package and use source install/setup.bash.
amodpatil1
commented
2 months ago It ran thank you so much
amodpatil1
commented
2 months ago if I have any doubts, how can I reach you?
mgonzs13
commented
2 months ago Please feel free to write me here or to my email (the one shown in my account).
amodpatil1
commented
2 months ago 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()
mgonzs13
commented
2 months ago What do you mean by multiple components?
amodpatil1
commented
2 months ago 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
mgonzs13
commented
2 months ago 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.
amodpatil1
commented
2 months ago 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.
mgonzs13
commented
2 months ago 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.
amodpatil1
commented
2 months ago 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 = Falseclass 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()
mgonzs13
commented
2 months ago 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 YasminNodeclass 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.