[Solved] My first experiment with NRP: help with state machine


#1

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:

  1. help me to fill the code so I could go ahead with the tutorial;
  2. 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 = “”"

<?xml version='1.0'?> 0 0 0 0 0 0 0.057 .034 .034 """

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}
)


#2

OK. I have found the solution. Daniele


#3

Hi Daniele,

I guess you mean $HBP/Experiments/tutorial_baseball_solution. I post this path here so that it can help other users with similar problems.

Best regards,
Luc