start_controller.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Copyright (c) 2015, Fetch Robotics Inc.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions are met:
8 #
9 # * Redistributions of source code must retain the above copyright
10 # notice, this list of conditions and the following disclaimer.
11 # * Redistributions in binary form must reproduce the above copyright
12 # notice, this list of conditions and the following disclaimer in the
13 # documentation and/or other materials provided with the distribution.
14 # * Neither the name of the Fetch Robotics Inc. nor the names of its
15 # contributors may be used to endorse or promote products derived from
16 # this software without specific prior written permission.
17 #
18 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 # ARE DISCLAIMED. IN NO EVENT SHALL FETCH ROBOTICS INC. BE LIABLE FOR ANY
22 # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
23 # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
24 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
25 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
27 # THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 
29 # Author: Michael Ferguson
30 
31 import sys
32 import rospy
33 import actionlib
34 from actionlib_msgs.msg import GoalStatus
35 from robot_controllers_msgs.msg import QueryControllerStatesAction, \
36  QueryControllerStatesGoal, \
37  ControllerState
38 
39 ACTION_NAME = "/query_controller_states"
40 
41 if __name__ == "__main__":
42 
43  if len(sys.argv) < 2:
44  print("usage: start_controller.py <controller_name> [optional_controller_type]")
45  exit(-1)
46 
47  rospy.init_node("start_robot_controllers")
48 
49  rospy.loginfo("Connecting to %s..." % ACTION_NAME)
50  client = actionlib.SimpleActionClient(ACTION_NAME, QueryControllerStatesAction)
51  client.wait_for_server()
52  rospy.loginfo("Done.")
53 
54  state = ControllerState()
55  state.name = sys.argv[1]
56  if len(sys.argv) > 2:
57  state.type = sys.argv[2]
58  state.state = state.RUNNING
59 
60  goal = QueryControllerStatesGoal()
61  goal.updates.append(state)
62 
63  rospy.loginfo("Requesting that %s be started..." % state.name)
64  client.send_goal(goal)
65  client.wait_for_result()
66  if client.get_state() == GoalStatus.SUCCEEDED:
67  rospy.loginfo("Done.")
68  elif client.get_state() == GoalStatus.ABORTED:
69  rospy.logerr(client.get_goal_status_text())


robot_controllers_interface
Author(s): Michael Ferguson
autogenerated on Sun Sep 27 2020 03:22:36