Dear All,
I’m completely new with ROS and state machine…I’m doing the first tutorial and I should add few lines of code in the file below (see “Insert code here”). I think that for a user who knows state machine is very easy but I’m completely new on this … may you please:
- help me to fill the code so I could go ahead with the tutorial;
- give me some suggestions/materials to quickly learn how to deal with state machine and ROS stuff.
Thank you so much!
Daniele
import hbp_nrp_excontrol.nrp_states as states
from smach import StateMachine
from smach.state import State
from gazebo_msgs.srv import ApplyBodyWrench, GetModelState, DeleteModel, SpawnEntity, SpawnEntityRequest
from geometry_msgs.msg import Wrench, Vector3, Point
import rospy
from rospy import ServiceProxy, wait_for_service
from hbp_nrp_excontrol.logs import clientLogger
FINISHED = ‘FINISHED’
ERROR = ‘ERROR’
PREEMPTED = ‘PREEMPTED’
sm = StateMachine(outcomes=[FINISHED, ERROR, PREEMPTED])
import hbp_nrp_excontrol.nrp_states as states
ball_name = “ball”
ball_sdf_xml = “”"
class ThrowBallState(State):
def init(self, ball_name, sdf_xml, outcomes=[‘success’, ‘aborted’]):
super(ThrowBallState, self).init(outcomes=outcomes)
self._ball_name = ball_name
self._spawn_proxy = rospy.ServiceProxy(’/gazebo/spawn_sdf_entity’,
SpawnEntity, persistent=True)
self._wrench_proxy = ServiceProxy(’/gazebo/apply_body_wrench’,
ApplyBodyWrench, persistent=True)
self._ball_msg = SpawnEntityRequest()
self._ball_msg.entity_name = self._ball_name
self._ball_msg.entity_xml = sdf_xml.format(ball_name=ball_name)
self._ball_msg.initial_pose.position.x = 3.
self._ball_msg.initial_pose.position.y = -0.28
self._ball_msg.initial_pose.position.z = 0.7
self._ball_msg.reference_frame = "world"
def execute(self, userdata):
self._spawn_proxy(self._ball_msg)
force = Vector3(-5., 0., 1.5)
wrench = Wrench(force, Vector3(0.0, 0.0, 0.0))
self._wrench_proxy(self._ball_name+"::ball", "world", Point(),
wrench, rospy.Time(0.), rospy.Duration(0.1))
return 'success'
class FlyingBallState(State):
def init(self, ball_name, rate=1., outcomes=[‘success’, ‘aborted’], ):
super(FlyingBallState, self).init(outcomes=outcomes)
self._rate = rospy.Rate(rate)
self._ball_name = ball_name
self._state_proxy = ServiceProxy(’/gazebo/get_model_state’,
GetModelState, persistent=True)
self._delete_proxy = ServiceProxy(’/gazebo/delete_model’,
DeleteModel, persistent=True)
def execute(self, userdata):
start_time = rospy.Time.now()
while not self.ball_lower_than(0.1):
self._rate.sleep()
#################################################
# Insert code here:
# Delete the ball by calling self._delete_proxy
# You can find out the arguments by typing in a terminal:
# rosservice info /gazebo/delete_model
#################################################
return 'success'
def ball_lower_than(self, z_threshold):
current_ball_state = self._state_proxy(ball_name, "world")
clientLogger.info("Ball position: {}".format(current_ball_state.pose.position))
#################################################
# Insert code here:
# Return True when the ball is too low
#################################################
return False
with sm:
#################################################
# Insert code here:
# Add the ThrowBallState to the state machine states
# Make the FlyingBallState and ThrowBallState alternate
#################################################
StateMachine.add(
“flying_ball”,
FlyingBallState(ball_name),
transitions = {“success”: FINISHED,
“aborted”: ERROR}
)