def execute(self, ud):
#comment this at last
#self.resources=Interaction()
#-------------
rospy.loginfo("waiting for buoy_detect Server")
buoyClient=SimpleActionClient(header.BUOY_DETECT_ACTION_SERVER, buoyAction)
buoyClient.wait_for_server()
rospy.loginfo("connected to server")
goal=buoyGoal()
goal.order=ip_header.ALLIGN_BUOY
buoyClient.send_goal(goal, feedback_cb= self.feedback_cb)
result=buoyClient.wait_for_result(rospy.Duration(ud.time_out))
if not result:
buoyClient.cancel_goal()
if buoyClient.get_state()==GoalStatus.SUCCEEDED:
rospy.loginfo("successfully aligned the vehicle with the gate ")
return 'aligned'
else:
rospy.logerr("buoy not allogned and timed out before alliging it!!!")
return 'timed_out'
class TravelState(StateMachine):
def __init__(self):
StateMachine.__init__(self,
input_keys=['target_pose'],
outcomes=['succeeded', 'preempted', 'aborted']
)
self.sm_move = Concurrence(
input_keys=['target_pose'],
output_keys=['ball_pose'],
outcomes=['succeeded', 'preempted', 'aborted'],
default_outcome='aborted',
child_termination_cb = self.child_termination_cb,
outcome_cb = self.outcome_cb
)
self.action_client = SimpleActionClient('move_base', MoveBaseAction)
#self.action_client = PretendActionClient()
self.move_state = Move(self.action_client, 0.2)
self.chaser_state = BallChaser(self.action_client, 0.2)
self.userdata.msg_in = None
self.watcher = BallTracker(self.move_state, self.chaser_state)
with self.sm_move:
Concurrence.add('BALL_WATCHER', self.watcher)
Concurrence.add('MOVE_TO_GOAL', self.move_state)
with self:
StateMachine.add('SM_MOVE', self.sm_move)
StateMachine.add('CHASE_BALLS', self.move_state,
transitions={
'succeeded':'SM_MOVE',
'aborted':'SM_MOVE',
'preempted':'SM_MOVE'
},
remapping={'target_pose':'ball_pose'}
)
def execute(self, ud=None):
if ud==None:
ud = self.userdata
self.action_client.wait_for_server()
return Concurrence.execute(self, ud)
def child_termination_cb(self, outmap):
if outmap['CHASE_BALLS']!=:
self.watcher.request_preempt()
return True
else:
rospy.logwarn('Unexpected termination')
return False
def outcome_cb(self, outmap):
return outmap['SM_MOVE']
class Planner:
zones = collections.deque([])
def __init__(self):
rospy.init_node('wilson_ros_planner')
self.client = SimpleActionClient('move_base', MoveBaseAction)
rospy.loginfo("Waiting 5s for move_base action server...")
self.client.wait_for_server(rospy.Duration(5))
rospy.loginfo("Connected to move base server")
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['failed', 'stoped'])
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('WaitForZone',
WaitForZone(self.zones),
transitions={
'got_zone': 'GotZone',
'waiting_for_zone': 'WaitForZone'
})
smach.StateMachine.add('GotZone',
GotZone(self.zones),
transitions={
'zone_empty': 'WaitForZone',
'got_waypoint': 'GotWaypoint'
})
smach.StateMachine.add('GotWaypoint',
GotWaypoint(self.zones, self.client),
transitions={
'move_to_waypoint': 'GotWaypoint',
'at_waypoint': 'GotZone'
})
# Start Introspection Server
sis = smach_ros.IntrospectionServer('sis', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def execute(self, ud):
#comment this at last
#self.resources=Interaction()
#-------------
ipClient=SimpleActionClient(header.MARKER_DETECT_ACTION_SERVER, markerAction)
rospy.loginfo("Waiting for IP marker action server")
ipClient.wait_for_server()
goal=markerGoal()
goal.order=ip_header.DETECT_MARKER
ipClient.cancel_all_goals()
ipClient.send_goal(goal, done_cb=self.doneCb)
result=ipClient.wait_for_result()
if ipClient.get_state()==GoalStatus.SUCCEEDED:
rospy.loginfo("succesfully detected marker :)")
return 'marker_found'
def execute(self, ud):
#comment this at last
#self.resources=Interaction()
#-------------
ipClient = SimpleActionClient(header.MARKER_DETECT_ACTION_SERVER,
markerAction)
rospy.loginfo("Waiting for IP marker action server")
ipClient.wait_for_server()
goal = markerGoal()
goal.order = ip_header.DETECT_MARKER
ipClient.cancel_all_goals()
ipClient.send_goal(goal, done_cb=self.doneCb)
result = ipClient.wait_for_result()
if ipClient.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo("succesfully detected marker :)")
return 'marker_found'
class ActionDispatcher(object):
def __init__(self, name):
rospy.loginfo("Starting %s" % name)
self.client = None
action_list = rospy.get_param("han_action_dispatcher")["han_actions"]
self.default_action = rospy.get_param("~default_action")
self.dyn_srv = DynServer(HanActionDispatcherConfig, self.dyn_callback)
self.servers = {}
for a in action_list.items():
name = a[0]
self.servers[name] = SimpleActionServer(
name,
MoveBaseAction,
execute_cb=(lambda x: lambda msg: self.execute_cb(
msg, x[0], x[1]["action"]))(a),
auto_start=False)
self.servers[name].register_preempt_callback(self.preempt_cb)
self.servers[name].start()
rospy.loginfo("done")
def execute_cb(self, msg, name, action):
a = action if not self.use_default else self.default_action
self.client = SimpleActionClient(a, MoveBaseAction)
rospy.logdebug("Waiting for action server:" + a)
self.client.wait_for_server()
rospy.logdebug("Sending goal to:" + a)
self.client.send_goal_and_wait(msg)
self.servers[name].set_succeeded()
self.client = None
def preempt_cb(self):
if self.client != None:
self.client.cancel_all_goals()
self.client = None
def dyn_callback(self, config, level):
self.use_default = config["use_default"]
return config
def test_server_online(self):
"""Check that server is online
move-base is very opaque compared to the move-base-flex framework;
We cannot simply test, if a planner has been loaded successfully.
However, we know that the move_base node crashes and dies, if the
planner is ill-defined.
Hence, we can just test, if the main action is online, to verify, that
the loading-process finished properly.
"""
move_base = SimpleActionClient('/move_base', MoveBaseAction)
self.assertTrue(move_base.wait_for_server(rospy.Duration(60)),
"{} server offline".format(move_base.action_client.ns))
def test_server_online(self):
"""Check that server is online
In this simple setup, we don't really want to generate a plan.
Instread, we can ask for a plan on our planner (gpp_gp), and verify,
that the generated error-message is not INVALID_PLUGIN
See https://github.com/magazino/move_base_flex/blob/596ed881bfcbd847e9d296c6d38e4d3fa3b74a4d/mbf_msgs/action/GetPath.action
for reference.
"""
# setup the client
get_path = SimpleActionClient('move_base_flex/get_path', GetPathAction)
self.assertTrue(get_path.wait_for_server(rospy.Duration(60)),
"{} server offline".format(get_path.action_client.ns))
# send a dummy goal with the right planner
goal = GetPathGoal()
goal.planner = 'gpp_gp'
get_path.send_goal_and_wait(goal, rospy.Duration(1))
result = get_path.get_result()
# we are happy as long the plugin is known
self.assertNotEqual(result.outcome, GetPathResult.INVALID_PLUGIN)
class NavStall():
def __init__(self):
rospy.init_node('nav_stall', anonymous=True)
rospy.on_shutdown(self.shutdown)
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
# Subscribe to the move_base action server
self.move_base = SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server()
rospy.loginfo("Connected to move base server")
#initiate bucket counter variables
self.stallCounterBucket = 0
self.STOP_MAX_STALL_BUCKET_COUNT = 10
self.MAX_STALL_BUCKET_COUNT = 3
self.hasGivenUp = 0
self.current_topic = rospy.get_param("~current_topic")
self.stall_current = rospy.get_param("~stall_current", 0.1)
self.recovery_speed = rospy.get_param("~recovery_speed", 0.1)
self.recovery_time = rospy.get_param("~recovery_time", 2)
rospy.loginfo("Time: " + str(self.recovery_time))
# Wait for motor current topics to become available
rospy.loginfo("Waiting for motor current topic to become available...")
rospy.wait_for_message(self.current_topic, AnalogFloat)
#subscribe to motor current topics
rospy.Subscriber(self.current_topic, AnalogFloat, self.detect_stall)
rospy.loginfo("Stall detection started on " + self.current_topic)
def shutdown(self):
self.cancelAndStop()
'''
Implement leaky bucket for stall detection
'''
def detect_stall(self, msg):
now = rospy.Time.now()
if msg.header.stamp.secs < (now.secs-1):
#skip messages that are older than 1 sec (stale)
return
'''
if (self.hasGivenUp):
self.givenUp()
return
#if we've been stalled for too long time, just give up
if (self.stallCounterBucket > self.STOP_MAX_STALL_BUCKET_COUNT):
self.hasGivenUp = 1
self.givenUp()
return
'''
if (msg.value > self.stall_current):
self.stallCounterBucket+=2;
rospy.loginfo("Potential stall condition detected at current: " + str(msg.value) + " (stall current: " + str(self.stall_current) + ") - incremented Stall Counter to " + str(self.stallCounterBucket))
else:
if (self.stallCounterBucket > 0):
self.stallCounterBucket-=1; #decrement bucket
rospy.loginfo("Decremented Stall Counter to " + str(self.stallCounterBucket))
if (self.stallCounterBucket > self.MAX_STALL_BUCKET_COUNT):
rospy.logwarn("Stall conditition detected! Trying to recover...")
self.cancelAndStop()
self.recover()
rospy.logwarn("Stall recovery completed.")
def recover(self):
rospy.loginfo("Initiating recovery... Speed: " + str(self.recovery_speed) + " Time: " + str(self.recovery_time))
cmd_vel = Twist()
#move back for one second at low speed
cmd_vel.linear.x = -self.recovery_speed
#need to repeat this multiple times, as base_controller will timeout after 0.5 sec
for x in range(0, self.recovery_time*4):
self.cmd_vel_pub.publish(cmd_vel)
rospy.sleep(0.25)
#stop
rospy.loginfo("Stopping the robot after recovery move...")
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
def cancelAndStop(self):
rospy.loginfo("Canceling all goals...")
self.move_base.cancel_all_goals()
rospy.sleep(1)
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
'''
class SimpleActionState(State):
"""Simple action client state.
Use this class to represent an actionlib as a state in a state machine.
"""
# Meta-states for this action
WAITING_FOR_SERVER = 0
INACTIVE = 1
ACTIVE = 2
PREEMPTING = 3
COMPLETED = 4
def __init__(self,
# Action info
action_name,
action_spec,
# Default goal
goal = None,
goal_key = None,
goal_slots = [],
goal_cb = None,
goal_cb_args = [],
goal_cb_kwargs = {},
# Result modes
result_key = None,
result_slots = [],
result_cb = None,
result_cb_args = [],
result_cb_kwargs = {},
# Keys
input_keys = [],
output_keys = [],
outcomes = [],
# Timeouts
exec_timeout = None,
preempt_timeout = rospy.Duration(60.0),
server_wait_timeout = rospy.Duration(60.0)
):
"""Constructor for SimpleActionState action client wrapper.
@type action_name: string
@param action_name: The name of the action as it will be broadcast over ros.
@type action_spec: actionlib action msg
@param action_spec: The type of action to which this client will connect.
@type goal: actionlib goal msg
@param goal: If the goal for this action does not need to be generated at
runtime, it can be passed to this state on construction.
@type goal_key: string
@param goal_key: Pull the goal message from a key in the userdata.
This will be done before calling the goal_cb if goal_cb is defined.
@type goal_slots: list of string
@param goal_slots: Pull the goal fields (__slots__) from like-named
keys in userdata. This will be done before calling the goal_cb if
goal_cb is defined.
@type goal_cb: callable
@param goal_cb: If the goal for this action needs tobe generated at
runtime, a callback can be stored which modifies the default goal
object. The callback is passed two parameters:
- userdata
- current stored goal
The callback returns a goal message.
@type result_key: string
@param result_key: Put the result message into the userdata with
the given key. This will be done after calling the result_cb
if result_cb is defined.
@type result_slots: list of strings
@param result_slots: Put the result message fields (__slots__)
into the userdata with keys like the field names. This will be done
after calling the result_cb if result_cb is defined.
@type result_cb: callable
@param result_cb: If result information from this action needs to be
stored or manipulated on reception of a result from this action, a
callback can be stored which is passed this information. The callback
is passed three parameters:
- userdata (L{UserData<smach.user_data.UserData>})
- result status (C{actionlib.GoalStatus})
- result (actionlib result msg)
@type exec_timeout: C{rospy.Duration}
@param exec_timeout: This is the timeout used for sending a preempt message
to the delegate action. This is C{None} by default, which implies no
timeout.
@type preempt_timeout: C{rospy.Duration}
@param preempt_timeout: This is the timeout used for aborting after a
preempt has been sent to the action and no result has been received. This
timeout begins counting after a preempt message has been sent.
@type server_wait_timeout: C{rospy.Duration}
@param server_wait_timeout: This is the timeout used for aborting while
waiting for an action server to become active.
"""
# Initialize base class
State.__init__(self, outcomes=['succeeded','aborted','preempted'])
# Set action properties
self._action_name = action_name
self._action_spec = action_spec
# Set timeouts
self._goal_status = 0
self._exec_timeout = exec_timeout
self._preempt_timeout = preempt_timeout
self._server_wait_timeout = server_wait_timeout
# Set goal generation policy
if goal and hasattr(goal, '__call__'):
raise smach.InvalidStateError("Goal object given to SimpleActionState that IS a function object")
if not all([s in action_spec().action_goal.goal.__slots__ for s in goal_slots]):
raise smach.InvalidStateError("Goal slots specified are not valid slots.")
if goal_cb and not hasattr(goal_cb, '__call__'):
raise smach.InvalidStateError("Goal callback object given to SimpleActionState that IS NOT a function object")
# Static goal
if goal is None:
self._goal = copy.copy(action_spec().action_goal.goal)
else:
self._goal = goal
# Goal from userdata key
self._goal_key = goal_key
if goal_key is not None:
self.register_input_keys([goal_key])
# Goal slots from userdata keys
self._goal_slots = goal_slots
self.register_input_keys(goal_slots)
# Goal generation callback
self._goal_cb = goal_cb
self._goal_cb_args = goal_cb_args
self._goal_cb_kwargs = goal_cb_kwargs
if smach.has_smach_interface(goal_cb):
self._goal_cb_input_keys = goal_cb.get_registered_input_keys()
self._goal_cb_output_keys = goal_cb.get_registered_output_keys()
self.register_input_keys(self._goal_cb_input_keys)
self.register_output_keys(self._goal_cb_output_keys)
else:
self._goal_cb_input_keys = input_keys
self._goal_cb_output_keys = output_keys
# Set result processing policy
if result_cb and not hasattr(result_cb, '__call__'):
raise smach.InvalidStateError("Result callback object given to SimpleActionState that IS NOT a function object")
if not all([s in action_spec().action_result.result.__slots__ for s in result_slots]):
raise smach.InvalidStateError("Result slots specified are not valid slots.")
# Result callback
self._result_cb = result_cb
self._result_cb_args = result_cb_args
self._result_cb_kwargs = result_cb_kwargs
if smach.has_smach_interface(result_cb):
self._result_cb_input_keys = result_cb.get_registered_input_keys()
self._result_cb_output_keys = result_cb.get_registered_output_keys()
self._result_cb_outcomes = result_cb.get_registered_outcomes()
self.register_input_keys(self._result_cb_input_keys)
self.register_output_keys(self._result_cb_output_keys)
self.register_outcomes(self._result_cb_outcomes)
else:
self._result_cb_input_keys = input_keys
self._result_cb_output_keys = output_keys
self._result_cb_outcomes = outcomes
# Result to userdata key
self._result_key = result_key
if result_key is not None:
self.register_output_keys([result_key])
# Result slots to userdata keys
self._result_slots = result_slots
self.register_output_keys(result_slots)
# Register additional input and output keys
self.register_input_keys(input_keys)
self.register_output_keys(output_keys)
self.register_outcomes(outcomes)
# Declare some status variables
self._activate_time = rospy.Time.now()
self._preempt_time = rospy.Time.now()
self._duration = rospy.Duration(0.0)
self._status = SimpleActionState.WAITING_FOR_SERVER
# Construct action client, and wait for it to come active
self._action_client = SimpleActionClient(action_name,action_spec)
self._action_wait_thread = threading.Thread(name=self._action_name+'/wait_for_server',target = self._wait_for_server)
self._action_wait_thread.start()
self._execution_timer_thread = None
# Condition variables for threading synchronization
self._done_cond = threading.Condition()
def _wait_for_server(self):
"""Internal method for waiting for the action server
This is run in a separate thread and allows construction of this state
to not block the construction of other states.
"""
while self._status == SimpleActionState.WAITING_FOR_SERVER and not rospy.is_shutdown():
try:
if self._action_client.wait_for_server(rospy.Duration(1.0)):#self._server_wait_timeout):
self._status = SimpleActionState.INACTIVE
if self.preempt_requested():
return
except:
if not rospy.core._in_shutdown: # This is a hack, wait_for_server should not throw an exception just because shutdown was called
rospy.logerr("Failed to wait for action server '%s'" % (self._action_name))
def _execution_timer(self):
"""Internal method for cancelling a timed out goal after a timeout."""
while self._status == SimpleActionState.ACTIVE and not rospy.is_shutdown():
try:
rospy.sleep(0.1)
except:
if not rospy.is_shutdown():
rospy.logerr("Failed to sleep while running '%s'" % self._action_name)
if rospy.Time.now() - self._activate_time > self._exec_timeout:
rospy.logwarn("Action %s timed out after %d seconds." % (self._action_name, self._exec_timeout.to_sec()))
# Cancel the goal
self._action_client.cancel_goal()
### smach State API
def request_preempt(self):
rospy.loginfo("Preempt requested on action '%s'" % (self._action_name))
smach.State.request_preempt(self)
if self._status == SimpleActionState.ACTIVE:
rospy.loginfo("Preempt on action '%s' cancelling goal: \n%s" % (self._action_name, str(self._goal)))
# Cancel the goal
self._action_client.cancel_goal()
def execute(self, ud):
"""Called when executing a state.
This calls the goal_cb if it is defined, and then dispatches the
goal with a non-blocking call to the action client.
"""
# Make sure we're connected to the action server
if self._status is SimpleActionState.WAITING_FOR_SERVER:
rospy.logwarn("Still waiting for action server '%s' to start... is it running?" % self._action_name)
if self._action_wait_thread.is_alive():
# Block on joining the server wait thread (This can be preempted)
self._action_wait_thread.join()
else:
# Wait for the server in this thread (This can also be preempted)
self._wait_for_server()
rospy.loginfo("Connected to action server '%s'." % self._action_name)
# Check for preemption before executing
if self.preempt_requested():
rospy.loginfo("Preempting %s before sending goal." % self._action_name)
self.service_preempt()
return 'preempted'
# We should only get here if we have connected to the server
if self._status is SimpleActionState.WAITING_FOR_SERVER:
rospy.logfatal("Action server for "+self._action_name+" is not running.")
return 'aborted'
else:
self._status = SimpleActionState.INACTIVE
# Grab goal key, if set
if self._goal_key is not None:
self._goal = ud[self._goal_key]
# Write goal fields from userdata if set
for key in self._goal_slots:
setattr(self._goal,key,ud[key])
# Call user-supplied callback, if set, to get a goal
if self._goal_cb is not None:
try:
goal_update = self._goal_cb(
smach.Remapper(
ud,
self._goal_cb_input_keys,
self._goal_cb_output_keys,
[]),
self._goal,
*self._goal_cb_args,
**self._goal_cb_kwargs)
if goal_update is not None:
self._goal = goal_update
except:
rospy.logerr("Could not execute goal callback: "+traceback.format_exc())
return 'aborted'
# Make sure the necessary paramters have been set
if self._goal is None and self._goal_cb is None:
rospy.logerr("Attempting to activate action "+self._action_name+" with no goal or goal callback set. Did you construct the SimpleActionState properly?")
return 'aborted'
# Dispatch goal via non-blocking call to action client
self._activate_time = rospy.Time.now()
self._status = SimpleActionState.ACTIVE
# Wait on done condition
self._done_cond.acquire()
self._action_client.send_goal(self._goal, self._goal_done_cb, self._goal_active_cb, self._goal_feedback_cb)
# Preempt timeout watch thread
if self._exec_timeout:
self._execution_timer_thread = threading.Thread(name=self._action_name+'/preempt_watchdog',target = self._execution_timer)
self._execution_timer_thread.start()
# Wait for action to finish
self._done_cond.wait()
# Call user result callback if defined
result_cb_outcome = None
if self._result_cb is not None:
try:
result_cb_outcome = self._result_cb(
smach.Remapper(
ud,
self._result_cb_input_keys,
self._result_cb_output_keys,
[]),
self._goal_status,
self._goal_result)
if result_cb_outcome is not None and result_cb_outcome not in self.get_registered_outcomes():
rospy.logerr("Result callback for action "+self._action_name+", "+str(self._result_cb)+" was not registered with the result_cb_outcomes argument. The result callback returned '"+str(result_cb_outcome)+"' but the only registered outcomes are: "+str(self.get_registered_outcomes()))
return 'aborted'
except:
rospy.logerr("Could not execute result callback: "+traceback.format_exc())
return 'aborted'
if self._result_key is not None:
ud[self._result_key] = self._goal_result
for key in self._result_slots:
ud[key] = getattr(self._goal_result,key)
# Check status
if self._status == SimpleActionState.INACTIVE:
# Set the outcome on the result state
if self._goal_status == GoalStatus.SUCCEEDED:
outcome = 'succeeded'
elif self._goal_status == GoalStatus.PREEMPTED and self.preempt_requested():
outcome = 'preempted'
self.service_preempt()
else:
# All failures at this level are captured by aborting, even if we timed out
# This is an important distinction between local preemption, and preemption
# from above.
outcome = 'aborted'
else:
# We terminated without going inactive
rospy.logwarn("Action state terminated without going inactive first.")
outcome = 'aborted'
# Check custom result cb outcome
if result_cb_outcome is not None:
outcome = result_cb_outcome
# Set status inactive
self._status = SimpleActionState.INACTIVE
self._done_cond.release()
return outcome
### Action client callbacks
def _goal_active_cb(self):
"""Goal Active Callback
This callback starts the timer that watches for the timeout specified for this action.
"""
rospy.logdebug("Action "+self._action_name+" has gone active.")
def _goal_feedback_cb(self,feedback):
"""Goal Feedback Callback"""
rospy.logdebug("Action "+self._action_name+" sent feedback.")
def _goal_done_cb(self, result_state, result):
"""Goal Done Callback
This callback resets the active flags and reports the duration of the action.
Also, if the user has defined a result_cb, it is called here before the
method returns.
"""
def get_result_str(i):
strs = ('PENDING','ACTIVE','PREEMPTED','SUCCEEDED','ABORTED','REJECTED','LOST')
if i <len(strs):
return strs[i]
else:
return 'UNKNOWN ('+str(i)+')'
# Calculate duration
self._duration = rospy.Time.now() - self._activate_time
rospy.logdebug("Action "+self._action_name+" terminated after "\
+str(self._duration.to_sec())+" seconds with result "\
+get_result_str(self._action_client.get_state())+".")
# Store goal state
self._goal_status = result_state
self._goal_result = result
# Rest status
self._status = SimpleActionState.INACTIVE
# Notify done
self._done_cond.acquire()
self._done_cond.notify()
self._done_cond.release()
set_hebi_group = rospy.ServiceProxy('/hebiros/add_group_from_names', AddGroupFromNamesSrv)
# Topic to receive feedback from a group
hebi_group_feedback_topic = "/hebiros/" + hebi_group_name + "/feedback/joint_state"
rospy.loginfo(" hebi_group_feedback_topic: %s", "/hebiros/" + hebi_group_name + "/feedback/joint_state")
# Topic to send commands to a group
hebi_group_command_topic = "/hebiros/" + hebi_group_name + "/command/joint_state"
rospy.loginfo(" hebi_group_command_topic: %s", "/hebiros/" + hebi_group_name + "/command/joint_state")
# Call the /hebiros/add_group_from_names service to create a group
rospy.loginfo(" Waiting for AddGroupFromNamesSrv at %s ...", '/hebiros/add_group_from_names')
rospy.wait_for_service('/hebiros/add_group_from_names') # block until service server starts
rospy.loginfo(" AddGroupFromNamesSrv AVAILABLE.")
set_hebi_group(hebi_group_name, hebi_names, hebi_families)
trajectory_action_client = SimpleActionClient("/hebiros/"+hebi_group_name+"/trajectory", TrajectoryAction)
rospy.loginfo(" Waiting for TrajectoryActionServer at %s ...", "/hebiros/"+hebi_group_name+"/trajectory")
trajectory_action_client.wait_for_server() # block until action server starts
rospy.loginfo(" TrajectoryActionServer AVAILABLE.")
# Create a service client to set group settings
change_group_settings = rospy.ServiceProxy("/hebiros/"+hebi_group_name+"/send_command_with_acknowledgement",
SendCommandWithAcknowledgementSrv)
rospy.loginfo(" Waiting for SendCommandWithAcknowledgementSrv at %s ...", "/hebiros/"+hebi_group_name+"/send_command_with_acknowledgement")
rospy.wait_for_service("/hebiros/"+hebi_group_name+"/send_command_with_acknowledgement") # block until service server starts
cmd_msg = CommandMsg()
cmd_msg.name = hebi_mapping_flat
cmd_msg.settings.name = cmd_msg.name
cmd_msg.settings.velocity_gains.name = cmd_msg.name
cmd_msg.settings.position_gains.kp = [0.5, 1.5, 1.5, 0.5, 1.5, 1.5]
cmd_msg.settings.position_gains.ki = [0]*6
cmd_msg.settings.position_gains.kd = [0.1]*6
cmd_msg.settings.position_gains.i_clamp = [0]*6
class Interaction(object):
"""
this is the entry point for interaction with outside resources to the state
machine. it contains all the service proxies and servers, meant to be passed
to all the states. gets rid of global variables.
the boolean values are checked in states
"""
def __init__(self):
self.heading = None
"""
Setting default values
"""
self.depth = None
self.forward = None
rospy.loginfo("Initializing mission planner interaction module...")
rospy.Subscriber(name=header.ESTIMATED_POSE_TOPIC_NAME,data_class= krakenPose , callback=self.positionCallback, queue_size=100)
self.setPointController=SimpleActionClient(header.CONTROLLER_SERVER, controllerAction)
rospy.loginfo("waiting for setPoint controller action Server")
self.setPointController.wait_for_server()
rospy.loginfo("Got controller server ")
self.advancedControllerClient=SimpleActionClient(header.ADVANCED_CONTROLLER_SERVER, advancedControllerAction)
rospy.loginfo("Waiting for advancedController action server")
self.advancedControllerClient.wait_for_server()
rospy.loginfo("Got advanced Controller Action Server ..")
self.ipControllerPublisher=rospy.Publisher(name=header.IP_CONTROLLER_PUBLISHING_TOPIC,
data_class=ipControllererror, queue_size=100)
self.moveAlongService=rospy.ServiceProxy(name=header.MOVE_ALONG_SERVICE_NAME,
service_class=moveAlongLine)
rospy.loginfo("waiting for MoveAlongLine Service")
self.moveAlongService.wait_for_service()
rospy.loginfo("Got move along line service !!")
self.resetPoseService=rospy.ServiceProxy(name=header.RESET_POSE_SERVICE, service_class=krakenResetPose)
rospy.loginfo("waiting for Reset Position Service")
self.resetPoseService.wait_for_service()
rospy.loginfo("Got move reser pose service !!")
self.premapMarkerLocationService=rospy.ServiceProxy(name=header.PREMAP_LOCATION_SERVICE, service_class=getLocation)
rospy.loginfo("waiting for premap location Service")
self.moveAlongService.wait_for_service()
rospy.loginfo("Got move premap location service !!")
#also do on and off camera services--To be implemented
############--------------------------
rospy.loginfo("succesfully got all publishers and subsrcibers to mission planner !! ")
def positionCallback(self,msg):
self.pose=msg.data
self.pose.header=msg.header
def __init__(self, test):
self.test = test
#print "recorder_core: self.test.name:", self.test.name
recorder_config = self.load_data(
rospkg.RosPack().get_path("atf_recorder_plugins") +
"/config/recorder_plugins.yaml")
try:
# delete test_results directories and create new ones
if os.path.exists(self.test.generation_config["bagfile_output"]):
# shutil.rmtree(self.tests.generation_config"bagfile_output"]) #FIXME will fail if multiple test run concurrently
pass
else:
os.makedirs(self.test.generation_config["bagfile_output"])
except OSError:
pass
# lock for self variables of this class
self.lock = Lock()
# create bag file writer handle
self.lock_write = Lock()
self.bag = rosbag.Bag(
self.test.generation_config["bagfile_output"] + self.test.name +
".bag", 'w')
self.bag_file_writer = BagfileWriter(self.bag, self.lock_write)
# Init metric recorder
self.recorder_plugin_list = []
#print "recorder_config", recorder_config
if len(recorder_config) > 0:
for value in list(recorder_config.values()):
#print "value=", value
self.recorder_plugin_list.append(
getattr(atf_recorder_plugins, value)(self.lock_write,
self.bag_file_writer))
#print "self.recorder_plugin_list", self.recorder_plugin_list
#rospy.Service(self.topic + "recorder_command", RecorderCommand, self.command_callback)
self.diagnostics = None
rospy.Subscriber("/diagnostics_toplevel_state", DiagnosticStatus,
self.diagnostics_callback)
rospy.on_shutdown(self.shutdown)
# wait for topics, services, actions and tfs to become active
if test.robot_config != None and 'wait_for_topics' in test.robot_config:
for topic in test.robot_config["wait_for_topics"]:
rospy.loginfo("Waiting for topic '%s'...", topic)
rospy.wait_for_message(topic, rospy.AnyMsg)
rospy.loginfo("... got message on topic '%s'.", topic)
if test.robot_config != None and 'wait_for_services' in test.robot_config:
for service in test.robot_config["wait_for_services"]:
rospy.loginfo("Waiting for service '%s'...", service)
rospy.wait_for_service(service)
rospy.loginfo("... service '%s' available.", service)
if test.robot_config != None and 'wait_for_actions' in test.robot_config:
for action in test.robot_config["wait_for_actions"]:
rospy.loginfo("Waiting for action '%s'...", action)
# wait for action status topic
rospy.wait_for_message(action + "/status", rospy.AnyMsg)
# get action type of goal topic
topic_type = rostopic._get_topic_type(action + "/goal")[0]
# remove "Goal" string from action type
if topic_type == None or not "Goal" in topic_type: ## pylint: disable=unsupported-membership-test
msg = "Could not get type for action %s. type is %s" % (
action, topic_type)
rospy.logerr(msg)
raise ATFRecorderError(msg)
# remove "Goal" from type
topic_type = topic_type[0:len(topic_type) - 4] ## pylint: disable=unsubscriptable-object
client = SimpleActionClient(
action, roslib.message.get_message_class(topic_type))
# wait for action server
client.wait_for_server()
rospy.loginfo("... action '%s' available.", action)
if test.robot_config != None and 'wait_for_tfs' in test.robot_config:
listener = tf.TransformListener()
for root_frame, measured_frame in test.robot_config[
"wait_for_tfs"]:
rospy.loginfo(
"Waiting for transformation from '%s' to '%s' ...",
root_frame, measured_frame)
listener.waitForTransform(root_frame, measured_frame,
rospy.Time(), rospy.Duration(1))
rospy.loginfo(
"... transformation from '%s' to '%s' available.",
root_frame, measured_frame)
if test.robot_config != None and 'wait_for_diagnostics' in test.robot_config and test.robot_config[
"wait_for_diagnostics"]:
rospy.loginfo("Waiting for diagnostics to become OK ...")
r = rospy.Rate(100)
while not rospy.is_shutdown(
) and self.diagnostics != None and self.diagnostics.level != 0:
rospy.logdebug("... waiting for diagnostics to become OK ...")
r.sleep()
rospy.loginfo("... diagnostics are OK.")
self.active_topics = {}
# special case for tf: make sure tf_buffer is already filled (even before the testblocks are started). Thus we need to record /tf and /tf_static all the time, even if there is no active testblock using tf.
for testblock in self.test.testblocks:
topics = self.get_topics_of_testblock(testblock.name)
if "/tf" in topics or "/tf_static" in topics:
self.active_topics["/tf"] = ["always"]
self.active_topics["/tf_static"] = ["always"]
self.subscribers = {}
self.tf_static_message = TFMessage()
rospy.Timer(rospy.Duration(0.1), self.create_subscriber_callback)
rospy.Timer(rospy.Duration(0.1), self.tf_static_timer_callback)
rospy.loginfo("ATF recorder: started!")
class MeasurementsPub:
def __init__(self):
self.measure_dist = rospy.get_param("usv/measurements/dist", 5)
self.width = rospy.get_param("usv/measurements/width", 15)
self.height = rospy.get_param("usv/measurements/height", 15)
self.origin_lat = rospy.get_param("usv/origin/lat", -30.048638)
self.origin_lon = rospy.get_param("usv/origin/lon", -51.23669)
self.start_lat = rospy.get_param("usv/measurements/start/lat",
-30.047311)
self.start_lon = rospy.get_param("usv/measurements/start/lon",
-51.234663)
self.home_lat = rospy.get_param("usv/home/lat", -30.047358)
self.home_lon = rospy.get_param("usv/home/lon", -51.233064)
self.file_name = rospy.get_param("usv/measurements/file_name")
self.move_base = SimpleActionClient("move_base", MoveBaseAction)
self.move_base.wait_for_server()
self.area_pub = rospy.Publisher("/measurement_area",
PolygonStamped,
queue_size=10)
self.point_pub = rospy.Publisher('/measurements',
PointCloud2,
queue_size=10)
self.measurements = []
self.points = []
self.returning_home = False
rospy.Subscriber("/range_depth", Range, self.depth_callback)
rospy.Subscriber("/map", OccupancyGrid, self.map_callback)
rospy.Subscriber("/diffboat/safety", Safety, self.safety_callback)
rospy.Service("next_goal", Empty, self.service_next_goal)
self.has_map = False
self.info = MapMetaData()
self.load()
rospy.on_shutdown(self.save)
def grid_cell(self, wx, wy):
mx = int((wx - self.info.origin.position.x) / self.info.resolution)
my = int((wy - self.info.origin.position.y) / self.info.resolution)
return self.grid[mx, my]
def load(self):
try:
with open(self.file_name, "r") as read_file:
json_data = json.load(read_file)
self.measurements = json_data["measurements"]
self.goal = json_data["goal"]
except Exception as e:
print(e)
def save(self):
print("saving to", self.file_name)
with open(self.file_name, "w") as write_file:
json_data = {"measurements": self.measurements, "goal": self.goal}
json.dump(json_data, write_file)
def send_goal(self):
x, y, d = self.goal
if self.returning_home:
return
if y > self.origin[1] + self.height:
self.return_home()
return
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
heading = 0 if d == 1 else np.pi
qx, qy, qz, qw = quaternion_from_euler(0, 0, heading)
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.orientation.x = qx
goal.target_pose.pose.orientation.y = qy
goal.target_pose.pose.orientation.z = qz
goal.target_pose.pose.orientation.w = qw
self.publish_area()
# self.move_base.cancel_all_goals()
self.move_base.send_goal(goal, self.goal_callback)
def next_goal(self, goal):
x, y, w = goal
if (w == 1 and x + self.measure_dist > self.origin[0] + self.width
) or (w == -1 and x - self.measure_dist < self.origin[0]):
y += self.measure_dist
w *= -1
else:
x += self.measure_dist * w
if self.grid_cell(x, y) > 0:
return self.next_goal((x, y, w))
else:
return (x, y, w)
def goal_callback(self, status, result):
rospy.loginfo("Goal callback %s %s", status, result)
x, y, w = self.goal
if status == GoalStatus.SUCCEEDED:
self.measurements.append({
"depth":
self.depth,
"timestamp":
datetime.now().replace(microsecond=0).isoformat(),
"x":
x,
"y":
y
})
self.points.append([x, y, self.depth])
header = Header(frame_id="map", stamp=rospy.Time.now())
msg = point_cloud2.create_cloud_xyz32(header, self.points)
self.point_pub.publish(msg)
self.goal = self.next_goal(self.goal)
rospy.loginfo("Next goal %s", self.goal)
self.send_goal()
def service_next_goal(self, req):
rospy.logwarn("Skipping goal")
self.goal = self.next_goal(self.goal)
rospy.loginfo("Next goal %s", self.goal)
self.send_goal()
return EmptyResponse()
def return_home(self):
rospy.loginfo("Returning home")
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
x, y = self.gps_to_cell(self.home_lat, self.home_lon)
print("x: ", x, "y: ", y)
heading = 0
qx, qy, qz, qw = quaternion_from_euler(0, 0, heading)
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.orientation.x = qx
goal.target_pose.pose.orientation.y = qy
goal.target_pose.pose.orientation.z = qz
goal.target_pose.pose.orientation.w = qw
self.move_base.send_goal(goal)
self.returning_home = True
self.move_base.wait_for_result()
def safety_callback(self, data):
if data.battery <= 20:
rospy.logerr("Battery low")
self.return_home()
if data.water == 1:
rospy.logerr("Water in boat")
self.return_home()
#calculates cell from gps data
def gps_to_cell(self, lat, lon):
#calculate position in map frame
if self.has_map == True and self.info.resolution != 0:
#distance in metres between current position and bottom left corner of the map
if not (lat < self.origin_lat or lon < self.origin_lon):
dist_lat = vincenty((self.origin_lat, self.origin_lon),
(lat, self.origin_lon)).m
dist_lon = vincenty((self.origin_lat, self.origin_lon),
(self.origin_lat, lon)).m
x = dist_lon
y = dist_lat
return (x + self.info.origin.position.x,
y + self.info.origin.position.y)
else:
return -1
#save depth at current position
def depth_callback(self, data):
self.depth = data.range
def publish_area(self):
msg = PolygonStamped()
msg.header.frame_id = "map"
msg.header.stamp = rospy.Time.now()
origin_x, origin_y = self.origin
msg.polygon.points = [
Point32(x, y, 0)
for (x, y) in [(origin_x,
origin_y), (origin_x + self.width, origin_y),
(origin_x + self.width, origin_y +
self.height), (origin_x, origin_y + self.height)]
]
self.area_pub.publish(msg)
#get map metadata
def map_callback(self, data):
self.info = data.info
width = self.info.width
height = self.info.height
self.grid = np.matrix(np.array(data.data).reshape(
(height, width))).transpose()
self.has_map = True
self.origin = self.gps_to_cell(self.start_lat, self.start_lon)
print(self.origin)
if not self.measurements:
self.goal = (self.origin[0], self.origin[1], 1)
self.send_goal()
class NavTest():
def __init__(self):
rospy.init_node('nav_test', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 10)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
locations = dict()
locations['hall'] = Pose(Point(-0.486, -0.689, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['living_room'] = Pose(Point(1.623, 7.880, 0.000), Quaternion(0.000, 0.000, 0.707, 0.707))
locations['kitchen'] = Pose(Point(-1.577, 6.626, 0.000), Quaternion(0.000, 0.000, 1.000, 0.000))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Subscribe to the move_base action server
self.move_base = SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# Get the initial pose from the user
rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# If we've gone through the current sequence,
# start with a new random sequence
if i == n_locations:
i = 0
sequence = sample(locations, n_locations)
# Skip over first location if it is the same as
# the last location
if sequence[0] == last_location:
i = 1
# Get the next location in the current sequence
location = sequence[i]
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(pow(locations[location].position.x -
locations[last_location].position.x, 2) +
pow(locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(pow(locations[location].position.x -
initial_pose.pose.pose.position.x, 2) +
pow(locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes/n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " + str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def update_initial_pose(self, initial_pose):
self.initial_pose = initial_pose
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.move_base.cancel_goal()
rospy.sleep(2)
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
class InteractiveMarkerPlugin(PluginBase):
"""
Spawns interactive Marker which send cart goals to action server.
Does not interact with god map.
"""
def __init__(self, root_tips, suffix=u''):
"""
:param root_tips: list containing root->tip tuple for each interactive marker.
:type root_tips: list
:param suffix: the marker will be called 'eef_control{}'.format(suffix)
:type suffix: str
"""
if len(root_tips) > 0:
self.roots, self.tips = zip(*root_tips)
else:
self.roots = []
self.tips = []
self.suffix = suffix
self.markers = {}
super(InteractiveMarkerPlugin, self).__init__()
def start_once(self):
# giskard goal client
self.client = SimpleActionClient(u'qp_controller/command', MoveAction)
self.client.wait_for_server()
# marker server
self.server = InteractiveMarkerServer(u'eef_control{}'.format(
self.suffix))
self.menu_handler = MenuHandler()
all_goals = {}
for root, tip in zip(self.roots, self.tips):
int_marker = self.make_6dof_marker(
InteractiveMarkerControl.MOVE_ROTATE_3D, root, tip)
self.server.insert(
int_marker,
self.process_feedback(self.server, int_marker.name,
self.client, root, tip, all_goals))
self.menu_handler.apply(self.server, int_marker.name)
self.server.applyChanges()
def make_sphere(self, msg):
"""
:param msg:
:return:
:rtype: Marker
"""
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = msg.scale * MARKER_SCALE * 2
marker.scale.y = msg.scale * MARKER_SCALE * 2
marker.scale.z = msg.scale * MARKER_SCALE * 2
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 0.5
return marker
def make_sphere_control(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.make_sphere(msg))
msg.controls.append(control)
return control
def make_6dof_marker(self, interaction_mode, root_link, tip_link):
def normed_q(x, y, z, w):
return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w])
int_marker = InteractiveMarker()
int_marker.header.frame_id = tip_link
int_marker.pose.orientation.w = 1
int_marker.scale = MARKER_SCALE
int_marker.name = u'eef_{}_to_{}'.format(root_link, tip_link)
# insert a box
self.make_sphere_control(int_marker)
int_marker.controls[0].interaction_mode = interaction_mode
if interaction_mode != InteractiveMarkerControl.NONE:
control_modes_dict = {
InteractiveMarkerControl.MOVE_3D: u'MOVE_3D',
InteractiveMarkerControl.ROTATE_3D: u'ROTATE_3D',
InteractiveMarkerControl.MOVE_ROTATE_3D: u'MOVE_ROTATE_3D'
}
int_marker.name += u'_' + control_modes_dict[interaction_mode]
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = u'rotate_x'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = u'move_x'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = u'rotate_z'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = u'move_z'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = u'rotate_y'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = u'move_y'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.markers[int_marker.name] = int_marker
return int_marker
class process_feedback(object):
def __init__(self, i_server, marker_name, client, root_link, tip_link,
all_goals):
"""
:param i_server:
:type i_server: InteractiveMarkerServer
:param marker_name:
:type marker_name: str
:param client:
:type client: SimpleActionClient
:param root_link:
:type root_link: str
:param tip_link:
:type tip_link: str
:param all_goals:
:type all_goals: dict
"""
self.i_server = i_server
self.marker_name = marker_name
self.client = client
self.tip_link = tip_link
self.root_link = root_link
self.all_goals = all_goals
self.reset_goal()
self.marker_pub = rospy.Publisher(u'visualization_marker_array',
MarkerArray,
queue_size=10)
def reset_goal(self):
p = Pose()
p.orientation.w = 1
self.all_goals[self.root_link, self.tip_link] = []
self.all_goals[self.root_link, self.tip_link].append(
self.make_translation_controller(self.tip_link, p))
self.all_goals[self.root_link, self.tip_link].append(
self.make_rotation_controller(self.tip_link, p))
def __call__(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
self.all_goals = defaultdict(list)
self.all_goals[self.root_link, self.tip_link] = []
print(u'got interactive goal update')
# translation
controller = self.make_translation_controller(
feedback.header.frame_id, feedback.pose)
self.all_goals[self.root_link,
self.tip_link].append(controller)
# rotation
controller = self.make_rotation_controller(
feedback.header.frame_id, feedback.pose)
self.all_goals[self.root_link,
self.tip_link].append(controller)
self.send_all_goals()
self.pub_goal_marker(feedback.header, feedback.pose)
self.reset_goal()
self.i_server.setPose(self.marker_name, Pose())
self.i_server.applyChanges()
def pub_goal_marker(self, header, pose):
"""
:param header:
:type header: std_msgs.msg._Header.Header
:param pose:
:type pose: Pose
"""
ma = MarkerArray()
m = Marker()
m.action = Marker.ADD
m.type = Marker.CYLINDER
m.header = header
old_q = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
# x
m.pose = deepcopy(pose)
m.scale.x = 0.05 * MARKER_SCALE
m.scale.y = 0.05 * MARKER_SCALE
m.scale.z = MARKER_SCALE
muh = qv_mult(old_q, [m.scale.z / 2, 0, 0])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.pose.orientation = Quaternion(*quaternion_multiply(
old_q, quaternion_about_axis(np.pi / 2, [0, 1, 0])))
m.color.r = 1
m.color.g = 0
m.color.b = 0
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 0
ma.markers.append(m)
# y
m = deepcopy(m)
m.pose = deepcopy(pose)
muh = qv_mult(old_q, [0, m.scale.z / 2, 0])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.pose.orientation = Quaternion(*quaternion_multiply(
old_q, quaternion_about_axis(-np.pi / 2, [1, 0, 0])))
m.color.r = 0
m.color.g = 1
m.color.b = 0
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 1
ma.markers.append(m)
# z
m = deepcopy(m)
m.pose = deepcopy(pose)
muh = qv_mult(old_q, [0, 0, m.scale.z / 2])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.color.r = 0
m.color.g = 0
m.color.b = 1
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 2
ma.markers.append(m)
self.marker_pub.publish(ma)
def make_translation_controller(self, frame_id, pose):
"""
:param frame_id:
:type frame_id: str
:param pose:
:type pose: Pose
:return:
:rtype: giskard_msgs.msg._Controller.Controller
"""
controller = self.make_controller(frame_id, pose)
controller.type = Controller.TRANSLATION_3D
controller.p_gain = 3
controller.max_speed = 0.3
controller.weight = 1.0
return controller
def make_rotation_controller(self, frame_id, pose):
controller = self.make_controller(frame_id, pose)
controller.type = Controller.ROTATION_3D
controller.p_gain = 3
controller.max_speed = 0.5
controller.weight = 1.0
return controller
def make_controller(self, frame_id, pose):
controller = Controller()
controller.tip_link = self.tip_link
controller.root_link = self.root_link
controller.goal_pose.header.frame_id = frame_id
controller.goal_pose.pose = pose
return controller
def send_all_goals(self):
goal = MoveGoal()
goal.type = MoveGoal.PLAN_AND_EXECUTE
move_cmd = MoveCmd()
# move_cmd.max_trajectory_length = 20
for g in self.all_goals.values():
move_cmd.controllers.extend(g)
goal.cmd_seq.append(move_cmd)
self.client.send_goal(goal)
def stop(self):
self.marker_pub.unregister()
def copy(self):
return self
class NavTest():
def __init__(self):
rospy.init_node('nav_test', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 10)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
locations = dict()
locations['hall'] = Pose(Point(-0.486, -0.689, 0.000),
Quaternion(0.000, 0.000, 0.000, 1.000))
locations['living_room'] = Pose(Point(1.623, 7.880, 0.000),
Quaternion(0.000, 0.000, 0.707, 0.707))
locations['kitchen'] = Pose(Point(-1.577, 6.626, 0.000),
Quaternion(0.000, 0.000, 1.000, 0.000))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Subscribe to the move_base action server
self.move_base = SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# Get the initial pose from the user
rospy.loginfo(
"*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..."
)
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped,
self.update_initial_pose)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# If we've gone through the current sequence,
# start with a new random sequence
if i == n_locations:
i = 0
sequence = sample(locations, n_locations)
# Skip over first location if it is the same as
# the last location
if sequence[0] == last_location:
i = 1
# Get the next location in the current sequence
location = sequence[i]
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(
pow(
locations[location].position.x -
locations[last_location].position.x, 2) + pow(
locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(
pow(
locations[location].position.x -
initial_pose.pose.pose.position.x, 2) + pow(
locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(300))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " +
str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes / n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " +
str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def update_initial_pose(self, initial_pose):
self.initial_pose = initial_pose
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.move_base.cancel_goal()
rospy.sleep(2)
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
class Test(object):
def __init__(self, action_server_name):
# action server
self.client = SimpleActionClient(action_server_name,
ControllerListAction)
self.client.wait_for_server()
def send_cart_goal(self, goal_pose):
goal = ControllerListGoal()
goal.type = ControllerListGoal.STANDARD_CONTROLLER
# translaiton
controller = Controller()
controller.type = Controller.TRANSLATION_3D
controller.tip_link = 'gripper_tool_frame'
controller.root_link = 'base_footprint'
controller.goal_pose = goal_pose
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.05
controller.weight = 1.0
goal.controllers.append(controller)
# rotation
controller = Controller()
controller.type = Controller.ROTATION_3D
controller.tip_link = 'gripper_tool_frame'
controller.root_link = 'base_footprint'
controller.goal_pose = goal_pose
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.2
controller.weight = 1.0
goal.controllers.append(controller)
self.client.send_goal(goal)
result = self.client.wait_for_result(rospy.Duration(10))
print('finished in 10s?: {}'.format(result))
def send_joint_goal(self, joint_state):
goal = ControllerListGoal()
goal.type = ControllerListGoal.STANDARD_CONTROLLER
# translation
controller = Controller()
controller.type = Controller.JOINT
controller.tip_link = 'gripper_tool_frame'
controller.root_link = 'base_footprint'
controller.goal_state = joint_state
controller.p_gain = 3
controller.enable_error_threshold = False
controller.threshold_value = 0.05
controller.weight = 1.0
goal.controllers.append(controller)
self.client.send_goal(goal)
result = self.client.wait_for_result(rospy.Duration(10))
print('finished in 10s?: {}'.format(result))
class Interaction(object):
"""
this is the entry point for interaction with outside resources to the state
machine. it contains all the service proxies and servers, meant to be passed
to all the states. gets rid of global variables.
the boolean values are checked in states
"""
def __init__(self):
self.heading = None
"""
Setting default values
"""
self.depth = None
self.forward = None
rospy.loginfo("Initializing mission planner interaction module...")
rospy.Subscriber(name=topicHeader.NAV_POSE_ESTIMATED,data_class= krakenPose , callback=self.positionCallback, queue_size=100)
self.setPointController=SimpleActionClient(topicHeader.CONTROL_SETPOINT_ACTION, controllerAction)
rospy.loginfo("waiting for setPoint controller action Server")
self.setPointController.wait_for_server()
rospy.loginfo("Got controller server ")
self.advancedControllerClient=SimpleActionClient(topicHeader.CONTROL_ADVANCEDCONTROLLER_ACTION, advancedControllerAction)
rospy.loginfo("Waiting for advancedController action server")
self.advancedControllerClient.wait_for_server()
rospy.loginfo("Got advanced Controller Action Server ..")
self.ipControllerPublisher=rospy.Publisher(name=topicHeader.CONTROL_IP_ERROR,
data_class=ipControllererror, queue_size=100)
self.moveAlongService=rospy.ServiceProxy(name=topicHeader.CONTROL_MOVEALONG_SERV,
service_class=moveAlongLine)
rospy.loginfo("waiting for MoveAlongLine Service")
self.moveAlongService.wait_for_service()
rospy.loginfo("Got move along line service !!")
self.resetPoseService=rospy.ServiceProxy(name=topicHeader.RESET_POSITION_SERVICE, service_class=krakenResetPose)
rospy.loginfo("waiting for Reset Position Service")
self.resetPoseService.wait_for_service()
rospy.loginfo("Got move reser pose service !!")
self.premapMarkerLocationService=rospy.ServiceProxy(name=topicHeader.PREMAP_LOCATION_SERVICE, service_class=getLocation)
rospy.loginfo("waiting for premap location Service")
self.premapMarkerLocationService.wait_for_service()
rospy.loginfo("Got move premap location service !!")
#also do on and off camera services--To be implemented
############--------------------------
rospy.loginfo("succesfully got all publishers and subsrcibers to mission planner !! ")
def positionCallback(self,msg):
# self.pose.x=msg.data
# self.pose.header=msg.header
self.state=KrakenState()
self.state.data=msg.data
class Test(object):
def __init__(self, action_server_name):
# action server
self.client = SimpleActionClient(action_server_name, MoveAction)
self.client.wait_for_server()
self.joint_names = rospy.wait_for_message(
'/whole_body_controller/state',
JointTrajectoryControllerState).joint_names
def send_cart_goal(self, goal_pose):
goal = MoveGoal()
goal.type = MoveGoal.PLAN_AND_EXECUTE
# translaiton
controller = Controller()
controller.type = Controller.TRANSLATION_3D
controller.tip_link = 'gripper_tool_frame'
controller.root_link = 'base_footprint'
controller.goal_pose = goal_pose
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.05
goal.cmd_seq.append(MoveCmd())
goal.cmd_seq[-1].controllers.append(controller)
# rotation
controller = Controller()
controller.type = Controller.ROTATION_3D
controller.tip_link = 'gripper_tool_frame'
controller.root_link = 'base_footprint'
controller.goal_pose = goal_pose
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.2
goal.cmd_seq.append(MoveCmd())
goal.cmd_seq[-1].controllers.append(controller)
self.client.send_goal(goal)
result = self.client.wait_for_result(rospy.Duration(10))
print('finished in 10s?: {}'.format(result))
def send_rnd_joint_goal(self):
goal = MoveGoal()
goal.type = MoveGoal.PLAN_AND_EXECUTE
# translation
controller = Controller()
controller.type = Controller.JOINT
controller.tip_link = 'gripper_tool_frame'
controller.root_link = 'base_footprint'
for i, joint_name in enumerate(self.joint_names):
controller.goal_state.name.append(joint_name)
# controller.goal_state.position.append(0)
controller.goal_state.position.append(np.random.random() - 0.5)
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.05
controller.weight = 1
goal.cmd_seq.append(MoveCmd())
goal.cmd_seq[-1].controllers.append(controller)
self.client.send_goal(goal)
result = self.client.wait_for_result()
final_js = rospy.wait_for_message(
'/whole_body_controller/state', JointTrajectoryControllerState
) # type: JointTrajectoryControllerState
asdf = {}
for i, joint_name in enumerate(final_js.joint_names):
asdf[joint_name] = final_js.actual.positions[i]
for i, joint_name in enumerate(controller.goal_state.name):
print('{} real:{} | exp:{}'.format(
joint_name, asdf[joint_name],
controller.goal_state.position[i]))
print('finished in 10s?: {}'.format(result))
class TrajectoryRecorder(object):
"""
Attributes
hebi_group_name (str):
hebi_families (list of str): HEBI actuator families [base,..., tip]
hebi_names (list of str): HEBI actuator names [base,..., tip]
"""
def __init__(self, hebi_group_name, hebi_families, hebi_names):
rospy.loginfo("Creating TrajectoryRecorder instance...")
rospy.loginfo(" hebi_group_name: %s", hebi_group_name)
rospy.loginfo(" hebi_families: %s", hebi_families)
rospy.loginfo(" hebi_names: %s", hebi_names)
self.hebi_group_name = hebi_group_name
self.hebi_families = hebi_families
self.hebi_names = hebi_names
self.hebi_mapping = [
family + '/' + name
for family, name in zip(hebi_families, hebi_names)
]
# jt information populated by self._feedback_cb
self.current_jt_pos = {}
self.current_jt_vel = {}
self.current_jt_eff = {}
self._joint_state_pub = None
# Create a service client to create a group
set_hebi_group = rospy.ServiceProxy('/hebiros/add_group_from_names',
AddGroupFromNamesSrv)
# Create a service client to set the command lifetime
self.set_command_lifetime = rospy.ServiceProxy(
"/hebiros/" + hebi_group_name + "/set_command_lifetime",
SetCommandLifetimeSrv)
# Topic to receive feedback from a group
self.hebi_group_feedback_topic = "/hebiros/" + hebi_group_name + "/feedback/joint_state"
rospy.loginfo(" hebi_group_feedback_topic: %s",
"/hebiros/" + hebi_group_name + "/feedback/joint_state")
# Topic to send commands to a group
self.hebi_group_command_topic = "/hebiros/" + hebi_group_name + "/command/joint_state"
rospy.loginfo(" hebi_group_command_topic: %s",
"/hebiros/" + hebi_group_name + "/command/joint_state")
# Call the /hebiros/add_group_from_names service to create a group
rospy.loginfo(" Waiting for AddGroupFromNamesSrv at %s ...",
'/hebiros/add_group_from_names')
rospy.wait_for_service('/hebiros/add_group_from_names'
) # block until service server starts
rospy.loginfo(" AddGroupFromNamesSrv AVAILABLE.")
set_hebi_group(hebi_group_name, hebi_names, hebi_families)
# Feedback/Command
self.fbk_sub = rospy.Subscriber(self.hebi_group_feedback_topic,
JointState, self._feedback_cb)
self.cmd_pub = rospy.Publisher(self.hebi_group_command_topic,
JointState,
queue_size=1)
self._hold_position = False
self._hold_joint_states = []
# TrajectoryAction client
self.trajectory_action_client = SimpleActionClient(
"/hebiros/" + hebi_group_name + "/trajectory", TrajectoryAction)
rospy.loginfo(" Waiting for TrajectoryActionServer at %s ...",
"/hebiros/" + hebi_group_name + "/trajectory")
self.trajectory_action_client.wait_for_server(
) # block until action server starts
self._executing_trajectory = False
rospy.loginfo(" TrajectoryActionServer AVAILABLE.")
# Check ROS Parameter server for robot_description URDF
urdf_str = ""
urdf_loaded = False
time_end_check = rospy.Time.now().to_sec(
) + 2.0 # Stop looking for URDF after 2 seconds of ROS time
while not rospy.is_shutdown(
) and not urdf_loaded and rospy.Time.now().to_sec() < time_end_check:
if rospy.has_param('/robot_description'):
urdf_str = rospy.get_param('/robot_description')
urdf_loaded = True
rospy.loginfo(
"Pulled /robot_description from parameter server.")
else:
rospy.sleep(0.05) # sleep for 50 ms of ROS time
if urdf_loaded:
# pykdl_utils setup
self.robot_urdf = URDF.from_xml_string(urdf_str)
rospy.loginfo("URDF links: " +
str([link.name
for link in self.robot_urdf.links])[1:-1])
rospy.loginfo("URDF joints: " +
str([joint.name
for joint in self.robot_urdf.joints])[1:-1])
valid_names = False
while not rospy.is_shutdown() and not valid_names:
# self.chain_base_link_name = self.raw_input_ros_safe("Please enter kinematics chain's base link name\n") # FIXME: TEMP
# self.chain_end_link_name = self.raw_input_ros_safe("Please enter kinematics chain's end link name\n") # FIXME: TEMP
self.chain_base_link_name = "a_2043_01_5Z" # FIXME: TEMP
self.chain_end_link_name = "a_2039_02_4Z" # FIXME: TEMP
try:
self.kdl_kin = KDLKinematics(self.robot_urdf,
self.chain_base_link_name,
self.chain_end_link_name)
valid_names = True
except KeyError:
rospy.loginfo(
"Incorrect base or end link name. Please try again...")
# trac-ik setup
ik_solver = IK(self.chain_base_link_name,
self.chain_end_link_name,
urdf_string=urdf_str,
timeout=0.01,
epsilon=1e-4,
solve_type="Distance")
# Determine transformation from global reference frame to base_link
urdf_root = ET.fromstring(urdf_str)
cadassembly_metrics = urdf_root.find(
'link/CyPhy2CAD/CADAssembly_metrics')
if cadassembly_metrics is not None:
robot_base_link_transform = np.zeros((4, 4))
robot_base_link_transform[3, 3] = 1
rotation_matrix_elem = cadassembly_metrics.find(
'RotationMatrix')
for j, row in enumerate(rotation_matrix_elem.iter('Row')):
for i, column in enumerate(row.iter('Column')):
robot_base_link_transform[j, i] = column.get('Value')
translation_elem = cadassembly_metrics.find('Translation')
for j, attribute in enumerate(['X', 'Y', 'Z']):
robot_base_link_transform[j, 3] = translation_elem.get(
attribute)
kdl_kin_robot_base_link_to_chain_base_link = KDLKinematics(
self.robot_urdf, 'base_link', self.chain_base_link_name)
jt_angles = [
0.0
] * kdl_kin_robot_base_link_to_chain_base_link.num_joints
chain_base_link_transform = kdl_kin_robot_base_link_to_chain_base_link.forward(
jt_angles)
self.chain_base_link_abs_transform = np.dot(
robot_base_link_transform, chain_base_link_transform)
else:
self.chain_base_link_abs_transform = None
# Wait for connections to be setup
while not rospy.is_shutdown() and len(self.current_jt_pos) < len(
self.hebi_mapping):
rospy.sleep(0.1)
# Set up joint state publisher
self._joint_state_pub = rospy.Publisher('/joint_states',
JointState,
queue_size=1)
self._active_joints = self.kdl_kin.get_joint_names()
# Set up RViz Interactive Markers
self.int_markers_man = InteractiveMarkerManager(
self,
'trajectory_recording_tool/interactive_markers',
ik_solver=ik_solver)
else:
self.robot_urdf = None
self.waypoints = []
self.waypoints_cnt = 0
self.breakpoints_cnt = 0
self._prev_breakpoint = True
# rospkg
self._rospack = RosPack()
print()
def set_waypoint(self):
waypoint = Waypoint()
joint_state = waypoint.joint_state
joint_state.names = self.hebi_mapping
joint_state.positions = self._get_joint_angles()
if self.robot_urdf is not None:
homogeneous_transform = self.kdl_kin.forward(joint_state.positions)
waypoint.end_effector_pose = self._get_pose_from_homogeneous_matrix(
homogeneous_transform)
print("Store these joint positions as Waypoint #{}?:".format(
self.waypoints_cnt + 1))
for name, pos in zip(self.hebi_mapping, joint_state.positions):
print(" {:20}: {:4f}".format(name, pos))
user_input = self.raw_input_ros_safe(
"[Return] - yes | [Any other key] - no\n")
if user_input != "" and user_input != " ":
self.release_position()
return False
self.hold_position()
valid_input = False
print(
"\nPlease enter velocities (optional) for Waypoint #{} in the following format:"
.format(self.waypoints_cnt + 1))
print("Ex: 0,none,0")
user_input = self.raw_input_ros_safe(
"[Return] - skip | velocity1,velocity2, ...velocityN\n")
while not rospy.is_shutdown() and not valid_input:
if user_input == "" or user_input == " ":
joint_state.velocities = [NAN] * len(self.hebi_mapping)
valid_input = True
elif len(user_input.split(",")) != len(self.hebi_mapping):
print(
" INVALID INPUT: velocity list must be the same size as module list"
)
print(" Please try again.")
user_input = self.raw_input_ros_safe(
"[Return] - skip | velocity1,velocity2, ...velocityN\n")
else:
joint_state.velocities = [
NAN if vel.strip().lower() == 'none' else float(vel)
for vel in user_input.split(",")
]
valid_input = True
valid_input = False
print(
"\nPlease enter accelerations (optional) for Waypoint #{} in the following format:"
.format(self.waypoints_cnt + 1))
print("Ex: 0,1.1,none")
user_input = self.raw_input_ros_safe(
"[Return] - skip | accelerations1,accelerations2, ...accelerationsN\n"
)
while not rospy.is_shutdown() and not valid_input:
if user_input == "" or user_input == " ":
joint_state.accelerations = [NAN] * len(self.hebi_mapping)
valid_input = True
elif len(user_input.split(",")) != len(self.hebi_mapping):
print(
" INVALID INPUT: acceleration list must be the same size as module list"
)
print(" Please try again.")
user_input = self.raw_input_ros_safe(
"[Return] - skip | accelerations1,accelerations2, ... accelerationsN\n"
)
else:
joint_state.accelerations = [
NAN if acc.strip().lower() == "none" else float(acc)
for acc in user_input.split(",")
]
valid_input = True
user_input = self.raw_input_ros_safe(
"\nPlease enter a duration [s] - time to move from the previous waypoint to the current waypoint:\n"
)
while not rospy.is_shutdown() and (user_input == ""
or user_input == " "):
user_input = self.raw_input_ros_safe(
"Please enter a duration [s] - time to move from the previous waypoint to the current waypoint:\n"
)
waypoint.time = float(user_input)
self.append_waypoint(waypoint)
print("\nWaypoint #{} stored!\n".format(self.waypoints_cnt))
self.release_position()
return True
@staticmethod
def raw_input_ros_safe(prompt=None):
sys.stdout.flush()
try:
if prompt is not None:
user_input = raw_input(prompt)
else:
user_input = raw_input()
sys.stdout.flush()
return user_input
except KeyboardInterrupt:
sys.exit()
def append_waypoint(self, waypoint):
self.waypoints.append(waypoint)
self.waypoints_cnt += 1
self.int_markers_man.add_waypoint_marker(waypoint,
str(self.waypoints_cnt))
self._prev_breakpoint = False
def append_breakpoint(self):
if self._prev_breakpoint:
print("Error: Cannot set two consecutive breakpoints!")
else:
self._prev_breakpoint = True
self.waypoints.append(None)
self.breakpoints_cnt += 1
print("\nBreakpoint #{} stored!\n".format(self.breakpoints_cnt))
def record_movement(self):
resolution_xyz = None
if self.robot_urdf is not None:
resolution_xyz = 0.01 * float(
self.raw_input_ros_safe(
"Please enter the desired end-effector Cartesian resolution [cm]:\n"
)) # TODO: Add some user-input checking functions
resolution_jt = (m.pi / 180) * float(
self.raw_input_ros_safe(
"Please enter the desired joint resolution [degrees]:\n"))
duration = float(
self.raw_input_ros_safe(
"Please enter a setup duration for this movement [s]:\n"))
# Set up Thread
user_input = [None]
t = Thread(target=self._wait_for_user_input, args=(user_input, ))
t.start()
# first post
prev_jt_angles = self._get_joint_angles()
prev_end_effector_xyz = None
waypoint = Waypoint()
joint_state = waypoint.joint_state
joint_state.names = self.hebi_mapping
joint_state.positions = prev_jt_angles
joint_state.velocities = self._get_joint_velocities()
joint_state.accelerations = [NAN] * len(
self.hebi_mapping) # TODO: Can we get these from anywhere?
if self.robot_urdf is not None:
homogeneous_transform = self.kdl_kin.forward(joint_state.positions)
waypoint.end_effector_pose = self._get_pose_from_homogeneous_matrix(
homogeneous_transform)
eff_pos = waypoint.end_effector_pose.position
prev_end_effector_xyz = [eff_pos.x, eff_pos.y, eff_pos.z]
waypoint.time = duration
self.append_waypoint(waypoint)
prev_time = rospy.Time.now().to_sec()
# subsequent posts
while not rospy.is_shutdown() and user_input[0] is None:
jt_angles = self._get_joint_angles()
end_effector_pose = None
end_effector_xyz = None
xyz_dist = None
if self.robot_urdf is not None:
homogeneous_transform = self.kdl_kin.forward(
joint_state.positions)
end_effector_pose = self._get_pose_from_homogeneous_matrix(
homogeneous_transform)
eff_pos = waypoint.end_effector_pose.position
end_effector_xyz = [eff_pos.x, eff_pos.y, eff_pos.z]
xyz_dist = self._get_abs_distance(end_effector_xyz,
prev_end_effector_xyz)
jt_dist = self._get_abs_distance(jt_angles, prev_jt_angles)
if (self.robot_urdf is not None
and xyz_dist > resolution_xyz) or jt_dist > resolution_jt:
cur_time = rospy.Time.now().to_sec()
waypoint = Waypoint()
joint_state = waypoint.joint_state
joint_state.positions = jt_angles
joint_state.velocities = self._get_joint_velocities()
joint_state.accelerations = [NAN] * len(
self.hebi_mapping) # TODO: Can we get these from anywhere?
waypoint.end_effector_pose = end_effector_pose
waypoint.time = cur_time - prev_time
self.append_waypoint(waypoint)
prev_jt_angles = jt_angles
prev_end_effector_xyz = end_effector_xyz
prev_time = cur_time
t.join()
@staticmethod
def _get_abs_distance(vector1, vector2):
assert len(vector1) == len(vector2)
sum_of_squares = 0.0
for i, (val1, val2) in enumerate(zip(vector1, vector2)):
sum_of_squares += (val1 - val2)**2
return m.sqrt(sum_of_squares)
@staticmethod
def _wait_for_user_input(user_input):
user_input[0] = raw_input("Press [Return] to stop recording!!!\n")
@staticmethod
def _get_pose_from_homogeneous_matrix(homogeneous_transform_matrix):
pose = Pose()
pose.position.x = round(homogeneous_transform_matrix[0, 3], 6)
pose.position.y = round(homogeneous_transform_matrix[1, 3], 6)
pose.position.z = round(homogeneous_transform_matrix[2, 3], 6)
quaternion = transforms.quaternion_from_matrix(
homogeneous_transform_matrix[:3, :3]) # TODO: Check me
pose.orientation.w = quaternion[0]
pose.orientation.x = quaternion[1]
pose.orientation.y = quaternion[2]
pose.orientation.z = quaternion[3]
return pose
def print_waypoints(self):
breakpoints_passed = 0
time_from_start = 0.0
for i, waypoint in enumerate(self.waypoints):
if waypoint is not None:
time_from_start += waypoint.time
print("\nWaypoint #{} at time {} [s] from trajectory start".
format(i + 1 - breakpoints_passed, time_from_start))
joint_state = waypoint.joint_state
table = [[name, pos, vel, acc] for name, pos, vel, acc in zip(
joint_state.names, joint_state.positions,
joint_state.velocities, joint_state.accelerations)]
print(
tabulate(table,
headers=[
'Names', 'Positions [rad]',
'Velocities [rad/s]',
'Accelerations [rad/s^2]'
]))
eff_position = waypoint.end_effector_pose.position
print("\nEnd effector position x={}, y={}, z={}".format(
eff_position.x, eff_position.y, eff_position.z))
print(
"-" *
(len("\nWaypoint #{} at time {} [s] from trajectory start")
- 4 + len(str(i) + str(time_from_start))))
else:
breakpoints_passed += 1
print("\nBreakpoint #{}".format(breakpoints_passed))
print(("-" * (len("\nBreakpoint #{}") - 4 + len(str(i)))))
def execute_trajectories(self):
trajectory_goals, tmp = self._get_trajectory_and_end_effector_goals_from_waypoints(
)
# execute initial position-to-start trajectory
if len(trajectory_goals) == 0:
print("Error: No trajectory goals to execute!")
else:
# execute initial position to start trajectory
init_goal = TrajectoryGoal()
waypoint_1 = WaypointMsg()
waypoint_1.names = self.hebi_mapping
waypoint_1.positions = self._get_joint_angles()
waypoint_1.velocities = [0] * len(self.hebi_mapping)
waypoint_1.accelerations = [0] * len(self.hebi_mapping)
waypoint_2 = trajectory_goals[0].waypoints[0]
init_goal.waypoints = [waypoint_1, waypoint_2]
init_goal.times = [0, 3]
self._executing_trajectory = True
self.trajectory_action_client.send_goal(init_goal)
self.trajectory_action_client.wait_for_result()
# execute trajectory goals
for goal in trajectory_goals:
self.trajectory_action_client.send_goal(goal)
self.trajectory_action_client.wait_for_result()
self.trajectory_action_client.get_result()
self._executing_trajectory = False
def execute_trajectory(self):
# TODO: Maybe support executing individual trajectories. Project creep... this is just a command line tool...
pass
def save_trajectory_to_file(self):
# Get target package path
rospack = RosPack()
target_package_found = False
package_path = None
while not target_package_found:
target_package_name = raw_input("Please enter target package: ")
try:
package_path = rospack.get_path(target_package_name)
target_package_found = True
print("Target package path: {}".format(package_path))
except ResourceNotFound:
print("Package {} not found!!! Please try again.".format(
target_package_name))
# Create trajectories directory if it does not exist - https://stackoverflow.com/a/14364249
save_dir_path = os.path.join(package_path, "trajectories")
print("Save directory path: {}".format(save_dir_path))
try: # prevents a common race condition - duplicated attempt to create directory
os.makedirs(save_dir_path)
except OSError:
if not os.path.isdir(save_dir_path):
raise
# Get save file name
name_set = False
base_filename = None
while not name_set:
base_filename = raw_input("Please enter the save file name: ")
target_name_path_1 = os.path.join(save_dir_path,
base_filename + ".json")
target_name_path_2 = os.path.join(save_dir_path,
base_filename + "_0.json")
if os.path.isfile(target_name_path_1) or os.path.isfile(
target_name_path_2):
print(
"A file with name {} already exists!!! Please try again.".
format(base_filename))
else:
name_set = True
# Dump trajectory artifacts to file(s)
trajectory_goals, end_effector_goals = self._get_trajectory_and_end_effector_goals_from_waypoints(
)
for i, (trajectory_goal, end_effector_goal) in enumerate(
zip(trajectory_goals, end_effector_goals)):
suffix = "_" + str(i)
path = os.path.join(save_dir_path,
base_filename + suffix + ".json")
print("Saving trajectory {} to: {}\n".format(i, path))
with open(path, 'w') as savefile:
json_data_structure = self._convert_trajectory_goal_to_json_serializable(
trajectory_goal, end_effector_goal)
json_str = json.dumps(json_data_structure,
sort_keys=True,
indent=4,
separators=(',', ': '))
match_re = r'(\n*\s*\[)(\n\s*("*[\w\./\-\d]+"*,*\n\s*)+\])'
reformatted_json_str = re.sub(match_re, self._newlinereplace,
json_str)
savefile.write(reformatted_json_str)
def _convert_trajectory_goal_to_json_serializable(self, trajectory_goal,
end_effector_goal):
waypoints_dict = {}
for i, (waypoint, pose) in enumerate(
zip(trajectory_goal.waypoints, end_effector_goal)):
waypoint_dict = {
'names':
list(waypoint.names),
'positions':
[float(positions) for positions in waypoint.positions],
'velocities':
[float(velocities) for velocities in waypoint.velocities],
'accelerations': [
float(acceleration)
for acceleration in waypoint.accelerations
]
}
if self.robot_urdf is not None:
eff_position = pose.position
waypoint_dict['end-effector xyz'] = [
eff_position.x, eff_position.y, eff_position.z
]
eff_orientation = pose.orientation
waypoint_dict['end-effector wxyz'] = [
eff_orientation.w, eff_orientation.x, eff_orientation.y,
eff_orientation.z
]
waypoints_dict[str(i)] = waypoint_dict
times_list = [float(time) for time in trajectory_goal.times]
json_data_structure = {
'waypoints': waypoints_dict,
'times': times_list
}
if self.chain_base_link_abs_transform is not None:
json_data_structure[
'base link transform'] = self.chain_base_link_abs_transform.tolist(
)
return json_data_structure
@staticmethod
def _newlinereplace(matchobj):
no_newlines = matchobj.group(2).replace(
"\n", "") # eliminate newline characters
no_newlines = no_newlines.split() # eliminate excess whitespace
no_newlines = "".join([" " + segment for segment in no_newlines])
return matchobj.group(1) + no_newlines
def _get_trajectory_and_end_effector_goals_from_waypoints(self):
trajectory_goals = []
end_effector_goals = []
prev_breakpoint = False
prev_time = 0.0
for waypoint in self.waypoints:
if waypoint is not None:
if len(trajectory_goals) == 0 or prev_breakpoint:
trajectory_goals.append(TrajectoryGoal())
end_effector_goals.append([])
# if applicable, 1st append last waypoint from previous trajectory
if len(trajectory_goals) > 1 and prev_breakpoint:
waypoint_msg = WaypointMsg()
waypoint_msg.names = list(
trajectory_goals[-2].waypoints[-1].names)
waypoint_msg.velocities = [0] * len(waypoint_msg.names)
waypoint_msg.accelerations = [0] * len(waypoint_msg.names)
trajectory_goals[-1].waypoints.append(waypoint_msg)
trajectory_goals[-1].times.append(0.0)
end_effector_goals[-1].append(
copy.deepcopy(end_effector_goals[-2]))
# append a new waypoint
trajectory_goals[-1].waypoints.append(
copy.deepcopy(waypoint.joint_state))
prev_time += waypoint.time
trajectory_goals[-1].times.append(prev_time)
end_effector_goals[-1].append(
copy.deepcopy(waypoint.end_effector_pose))
prev_breakpoint = False
else: # breakpoint
prev_time = 0.0
prev_breakpoint = True
return trajectory_goals, end_effector_goals
def delete_waypoint(self, waypoint):
index = self.waypoints.index(waypoint)
# Get index of last waypoint
deleting_last_wp = False
for i, wp in reversed(list(enumerate(self.waypoints))):
if wp is not None:
if i == index:
deleting_last_wp = True
break
self.waypoints.remove(waypoint)
self.waypoints_cnt -= 1
last_wp = None
# Check for and remove adjacent breakpoints. Also record last waypoint
prev_wp = None
for i, wp in enumerate(list(self.waypoints)):
if wp is None and prev_wp is None:
del self.waypoints[i]
self.breakpoints_cnt -= 1
else:
last_wp = wp
prev_wp = wp
# Check if last waypoint is a breakpoint
if len(self.waypoints) == 0 or self.waypoints[-1] is None:
self._prev_breakpoint = True
print("Waypoint deleted...")
if len(self.waypoints) != 0 and index == 0:
self.waypoints[0].joint_state.velocities = [0] * len(
self.hebi_mapping)
self.waypoints[0].joint_state.accelerations = [0] * len(
self.hebi_mapping)
self.waypoints[0].time = 0
# TODO: Fix time. Have to access waypoint marker menus, etc...
elif deleting_last_wp and last_wp is not None:
i = self.waypoints.index(last_wp)
self.waypoints[i].joint_state.velocities = [0] * len(
self.hebi_mapping)
self.waypoints[i].joint_state.accelerations = [0] * len(
self.hebi_mapping)
def insert_waypoint(self, ref_waypoint, before=True):
# create new Waypoint
waypoint = Waypoint()
waypoint.joint_state.names = self.hebi_mapping
waypoint.joint_state.positions = self._get_joint_angles()
waypoint.joint_state.velocities = [NAN] * len(self.hebi_mapping)
waypoint.joint_state.accelerations = [NAN] * len(self.hebi_mapping)
if self.robot_urdf is not None:
homogeneous_transform = self.kdl_kin.forward(
waypoint.joint_state.positions)
waypoint.end_effector_pose = self._get_pose_from_homogeneous_matrix(
homogeneous_transform)
waypoint.time = 2.0 # TODO: Figure out the best way to edit this from GUI
# determine index of reference waypoint
ref_index = None
wp_passed_cnt = 0
for i, wp in enumerate(self.waypoints):
if wp is ref_waypoint:
ref_index = i
break
elif wp is not None:
wp_passed_cnt += 1
# insert new Waypoint
if before:
if ref_index == 0:
# undo previous initial waypoint settings
self.waypoints[0].joint_state.velocities = [NAN] * len(
self.hebi_mapping)
self.waypoints[0].joint_state.accelerations = [NAN] * len(
self.hebi_mapping)
self.waypoints[0].time = 2.0
# TODO: Fix time. Have to access waypoint marker menus, etc...
# apply initial waypoint settings
waypoint.joint_state.velocities = [0] * len(self.hebi_mapping)
waypoint.joint_state.accelerations = [0] * len(
self.hebi_mapping)
waypoint.time = 0
self.waypoints.insert(ref_index, waypoint)
else:
if ref_index < len(self.waypoints) - 1:
self.waypoints.insert(ref_index + 1, waypoint)
else:
# undo previous initial waypoint settings
self.waypoints[-1].joint_state.velocities = [NAN] * len(
self.hebi_mapping)
self.waypoints[-1].joint_state.accelerations = [NAN] * len(
self.hebi_mapping)
# apply final waypoint settings
waypoint.joint_state.velocities = [0] * len(self.hebi_mapping)
waypoint.joint_state.accelerations = [0] * len(
self.hebi_mapping)
self.waypoints.append(waypoint)
wp_passed_cnt += 1
self.waypoints_cnt += 1
self.int_markers_man.add_waypoint_marker(
waypoint, description=str(wp_passed_cnt + 1))
self._prev_breakpoint = False
def restart(self):
self.waypoints = []
self.waypoints_cnt = 0
self.breakpoints_cnt = 0
self._prev_breakpoint = True
self.release_position()
self.int_markers_man.clear_waypoint_markers()
def hold_position(self):
self._hold_joint_states = self._get_joint_angles()
self._hold_position = True
def release_position(self):
self._hold_position = False
def exit(self):
self.release_position()
def _get_joint_angles(self):
return [self.current_jt_pos[motor] for motor in self.hebi_mapping]
def _get_joint_velocities(self):
return [self.current_jt_vel[motor] for motor in self.hebi_mapping]
def _get_joint_efforts(self):
return [self.current_jt_eff[motor] for motor in self.hebi_mapping]
def _feedback_cb(self, msg):
for name, pos, vel, eff in zip(msg.name, msg.position, msg.velocity,
msg.effort):
if name not in self.hebi_mapping:
print("WARNING: arm_callback - unrecognized name!!!")
else:
self.current_jt_pos[name] = pos
self.current_jt_vel[name] = vel
self.current_jt_eff[name] = eff
if not rospy.is_shutdown(
) and self._joint_state_pub is not None: # Publish JointState() for RViz
jointstate = JointState()
jointstate.header.stamp = rospy.Time.now()
jointstate.name = self._active_joints
jointstate.position = self._get_joint_angles()
jointstate.velocity = [0.0] * len(jointstate.name)
jointstate.effort = [0.0] * len(jointstate.name)
self._joint_state_pub.publish(jointstate)
# TODO: Make this less hacky
if not rospy.is_shutdown() and self._joint_state_pub is not None:
if not self._executing_trajectory:
joint_grav_torques = self.kdl_kin.get_joint_torques_from_gravity(
self._get_joint_angles(),
grav=[0, 0, -9.81]) # TODO: FIXME: Hardcoded
jointstate = JointState()
jointstate.header.stamp = rospy.Time.now()
jointstate.name = self.hebi_mapping
if self._hold_position:
# jointstate.position = self.waypoints[-1].positions # jerky
jointstate.position = self._hold_joint_states
else:
jointstate.position = []
jointstate.velocity = []
jointstate.effort = joint_grav_torques
self.cmd_pub.publish(jointstate)
class InteractiveMarkerGoal(object):
def __init__(self, root_link, tip_links):
# tf
self.tfBuffer = Buffer(rospy.Duration(1))
self.tf_listener = TransformListener(self.tfBuffer)
# giskard goal client
# self.client = SimpleActionClient('move', ControllerListAction)
self.client = SimpleActionClient('qp_controller/command',
ControllerListAction)
self.client.wait_for_server()
# marker server
self.server = InteractiveMarkerServer("eef_control")
self.menu_handler = MenuHandler()
for tip_link in tip_links:
int_marker = self.make6DofMarker(
InteractiveMarkerControl.MOVE_ROTATE_3D, root_link, tip_link)
self.server.insert(
int_marker,
self.process_feedback(self.server, self.client, root_link,
tip_link))
self.menu_handler.apply(self.server, int_marker.name)
self.server.applyChanges()
def transformPose(self, target_frame, pose, time=None):
transform = self.tfBuffer.lookup_transform(
target_frame, pose.header.frame_id,
pose.header.stamp if time is not None else rospy.Time(0),
rospy.Duration(1.0))
new_pose = do_transform_pose(pose, transform)
return new_pose
def makeBox(self, msg):
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = msg.scale * 0.2
marker.scale.y = msg.scale * 0.2
marker.scale.z = msg.scale * 0.2
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 0.5
return marker
def makeBoxControl(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.makeBox(msg))
msg.controls.append(control)
return control
def make6DofMarker(self, interaction_mode, root_link, tip_link):
def normed_q(x, y, z, w):
return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w])
int_marker = InteractiveMarker()
p = PoseStamped()
p.header.frame_id = tip_link
p.pose.orientation.w = 1
int_marker.header.frame_id = tip_link
int_marker.pose.orientation.w = 1
int_marker.pose = self.transformPose(root_link, p).pose
int_marker.header.frame_id = root_link
int_marker.scale = .2
int_marker.name = "eef_{}_to_{}".format(root_link, tip_link)
# insert a box
self.makeBoxControl(int_marker)
int_marker.controls[0].interaction_mode = interaction_mode
if interaction_mode != InteractiveMarkerControl.NONE:
control_modes_dict = {
InteractiveMarkerControl.MOVE_3D: "MOVE_3D",
InteractiveMarkerControl.ROTATE_3D: "ROTATE_3D",
InteractiveMarkerControl.MOVE_ROTATE_3D: "MOVE_ROTATE_3D"
}
int_marker.name += "_" + control_modes_dict[interaction_mode]
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
return int_marker
class process_feedback(object):
def __init__(self, i_server, client, root_link, tip_link):
self.i_server = i_server
self.client = client
self.tip_link = tip_link
self.root_link = root_link
def __call__(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
print('interactive goal update')
goal = ControllerListGoal()
goal.type = ControllerListGoal.STANDARD_CONTROLLER
# asd = 'asd'
# translation
controller = Controller()
controller.type = Controller.TRANSLATION_3D
controller.tip_link = self.tip_link
controller.root_link = self.root_link
controller.goal_pose.header = feedback.header
controller.goal_pose.pose = feedback.pose
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.05
controller.weight = 1.0
goal.controllers.append(controller)
# rotation
controller = Controller()
controller.type = Controller.ROTATION_3D
controller.tip_link = self.tip_link
controller.root_link = self.root_link
controller.goal_pose.header = feedback.header
controller.goal_pose.pose = feedback.pose
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.2
controller.weight = 1.0
goal.controllers.append(controller)
self.client.send_goal(goal)
self.i_server.applyChanges()
class NavStall():
def __init__(self):
rospy.init_node('nav_stall', anonymous=True)
rospy.on_shutdown(self.shutdown)
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Subscribe to the move_base action server
self.move_base = SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server()
rospy.loginfo("Connected to move base server")
#initiate bucket counter variables
self.stallCounterBucket = 0
self.STOP_MAX_STALL_BUCKET_COUNT = 10
self.MAX_STALL_BUCKET_COUNT = 3
self.hasGivenUp = 0
self.current_topic = rospy.get_param("~current_topic")
self.stall_current = rospy.get_param("~stall_current", 0.1)
self.recovery_speed = rospy.get_param("~recovery_speed", 0.1)
self.recovery_time = rospy.get_param("~recovery_time", 2)
rospy.loginfo("Time: " + str(self.recovery_time))
# Wait for motor current topics to become available
rospy.loginfo("Waiting for motor current topic to become available...")
rospy.wait_for_message(self.current_topic, AnalogFloat)
#subscribe to motor current topics
rospy.Subscriber(self.current_topic, AnalogFloat, self.detect_stall)
rospy.loginfo("Stall detection started on " + self.current_topic)
def shutdown(self):
self.cancelAndStop()
'''
Implement leaky bucket for stall detection
'''
def detect_stall(self, msg):
now = rospy.Time.now()
if msg.header.stamp.secs < (now.secs-1):
#skip messages that are older than 1 sec (stale)
return
'''
if (self.hasGivenUp):
self.givenUp()
return
#if we've been stalled for too long time, just give up
if (self.stallCounterBucket > self.STOP_MAX_STALL_BUCKET_COUNT):
self.hasGivenUp = 1
self.givenUp()
return
'''
if (msg.value > self.stall_current):
self.stallCounterBucket+=2;
rospy.loginfo("Potential stall condition detected at current: " + str(msg.value) + " (stall current: " + str(self.stall_current) + ") - incremented Stall Counter to " + str(self.stallCounterBucket))
else:
if (self.stallCounterBucket > 0):
self.stallCounterBucket-=1; #decrement bucket
rospy.loginfo("Decremented Stall Counter to " + str(self.stallCounterBucket))
if (self.stallCounterBucket > self.MAX_STALL_BUCKET_COUNT):
rospy.logwarn("Stall conditition detected! Trying to recover...")
self.cancelAndStop()
self.recover()
rospy.logwarn("Stall recovery completed.")
def recover(self):
rospy.loginfo("Initiating recovery... Speed: " + str(self.recovery_speed) + " Time: " + str(self.recovery_time))
cmd_vel = Twist()
#move back for one second at low speed
cmd_vel.linear.x = -self.recovery_speed
#need to repeat this multiple times, as base_controller will timeout after 0.5 sec
for x in range(0, self.recovery_time*4):
self.cmd_vel_pub.publish(cmd_vel)
rospy.sleep(0.25)
#stop
rospy.loginfo("Stopping the robot after recovery move...")
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
def cancelAndStop(self):
rospy.loginfo("Canceling all goals...")
self.move_base.cancel_all_goals()
rospy.sleep(1)
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
'''
class SimpleActionState(State):
"""Simple action client state.
Use this class to represent an actionlib as a state in a state machine.
"""
# Meta-states for this action
WAITING_FOR_SERVER = 0
INACTIVE = 1
ACTIVE = 2
PREEMPTING = 3
COMPLETED = 4
def __init__(
self,
# Action info
action_name,
action_spec,
# Default goal
goal=None,
goal_key=None,
goal_slots=[],
goal_cb=None,
goal_cb_args=[],
goal_cb_kwargs={},
# Result modes
result_key=None,
result_slots=[],
result_cb=None,
result_cb_args=[],
result_cb_kwargs={},
# Keys
input_keys=[],
output_keys=[],
outcomes=[],
# Timeouts
exec_timeout=None,
cancel_timeout=rospy.Duration(15.0),
server_wait_timeout=rospy.Duration(60.0),
):
"""Constructor for SimpleActionState action client wrapper.
@type action_name: string
@param action_name: The name of the action as it will be broadcast over ros.
@type action_spec: actionlib action msg
@param action_spec: The type of action to which this client will connect.
@type goal: actionlib goal msg
@param goal: If the goal for this action does not need to be generated at
runtime, it can be passed to this state on construction.
@type goal_key: string
@param goal_key: Pull the goal message from a key in the userdata.
This will be done before calling the goal_cb if goal_cb is defined.
@type goal_slots: list of string
@param goal_slots: Pull the goal fields (__slots__) from like-named
keys in userdata. This will be done before calling the goal_cb if
goal_cb is defined.
@type goal_cb: callable
@param goal_cb: If the goal for this action needs to be generated at
runtime, a callback can be stored which modifies the default goal
object. The callback is passed two parameters:
- userdata
- current stored goal
The callback returns a goal message.
@type result_key: string
@param result_key: Put the result message into the userdata with
the given key. This will be done after calling the result_cb
if result_cb is defined.
@type result_slots: list of strings
@param result_slots: Put the result message fields (__slots__)
into the userdata with keys like the field names. This will be done
after calling the result_cb if result_cb is defined.
@type result_cb: callable
@param result_cb: If result information from this action needs to be
stored or manipulated on reception of a result from this action, a
callback can be stored which is passed this information. The callback
is passed three parameters:
- userdata (L{UserData<smach.user_data.UserData>})
- result status (C{actionlib.GoalStatus})
- result (actionlib result msg)
@type exec_timeout: C{rospy.Duration}
@param exec_timeout: This is the timeout used for sending a preempt message
to the delegate action. This is C{None} by default, which implies no
timeout.
@type cancel_timeout: C{rospy.Duration}
@param cancel_timeout: This is the timeout used for aborting the state after a
preempt has been requested or the execution timeout occured and no result
from the action has been received. This timeout begins counting after cancel
to the action server has been sent.
@type server_wait_timeout: C{rospy.Duration}
@param server_wait_timeout: This is the timeout used for aborting while
waiting for an action server to become active.
"""
# Initialize base class
State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'])
# Set action properties
self._action_name = action_name
self._action_spec = action_spec
# Set timeouts
self._goal_status = 0
self._goal_result = None
self._exec_timeout = exec_timeout
self._cancel_timeout = cancel_timeout
self._server_wait_timeout = server_wait_timeout
# Set goal generation policy
if goal and hasattr(goal, '__call__'):
raise smach.InvalidStateError(
"Goal object given to SimpleActionState that IS a function object"
)
sl = action_spec().action_goal.goal.__slots__
if not all([s in sl for s in goal_slots]):
raise smach.InvalidStateError(
"Goal slots specified are not valid slots. Available slots: %s; specified slots: %s"
% (sl, goal_slots))
if goal_cb and not hasattr(goal_cb, '__call__'):
raise smach.InvalidStateError(
"Goal callback object given to SimpleActionState that IS NOT a function object"
)
# Static goal
if goal is None:
self._goal = copy.copy(action_spec().action_goal.goal)
else:
self._goal = goal
# Goal from userdata key
self._goal_key = goal_key
if goal_key is not None:
self.register_input_keys([goal_key])
# Goal slots from userdata keys
self._goal_slots = goal_slots
self.register_input_keys(goal_slots)
# Goal generation callback
self._goal_cb = goal_cb
self._goal_cb_args = goal_cb_args
self._goal_cb_kwargs = goal_cb_kwargs
if smach.has_smach_interface(goal_cb):
self._goal_cb_input_keys = goal_cb.get_registered_input_keys()
self._goal_cb_output_keys = goal_cb.get_registered_output_keys()
self.register_input_keys(self._goal_cb_input_keys)
self.register_output_keys(self._goal_cb_output_keys)
else:
self._goal_cb_input_keys = input_keys
self._goal_cb_output_keys = output_keys
# Set result processing policy
if result_cb and not hasattr(result_cb, '__call__'):
raise smach.InvalidStateError(
"Result callback object given to SimpleActionState that IS NOT a function object"
)
if not all([
s in action_spec().action_result.result.__slots__
for s in result_slots
]):
raise smach.InvalidStateError(
"Result slots specified are not valid slots.")
# Result callback
self._result_cb = result_cb
self._result_cb_args = result_cb_args
self._result_cb_kwargs = result_cb_kwargs
if smach.has_smach_interface(result_cb):
self._result_cb_input_keys = result_cb.get_registered_input_keys()
self._result_cb_output_keys = result_cb.get_registered_output_keys(
)
self._result_cb_outcomes = result_cb.get_registered_outcomes()
self.register_input_keys(self._result_cb_input_keys)
self.register_output_keys(self._result_cb_output_keys)
self.register_outcomes(self._result_cb_outcomes)
else:
self._result_cb_input_keys = input_keys
self._result_cb_output_keys = output_keys
self._result_cb_outcomes = outcomes
# Result to userdata key
self._result_key = result_key
if result_key is not None:
self.register_output_keys([result_key])
# Result slots to userdata keys
self._result_slots = result_slots
self.register_output_keys(result_slots)
# Register additional input and output keys
self.register_input_keys(input_keys)
self.register_output_keys(output_keys)
self.register_outcomes(outcomes)
# Declare some status variables
self._activate_time = rospy.Time.now()
self._cancel_time = rospy.Time.now()
self._duration = rospy.Duration(0.0)
self._status = SimpleActionState.WAITING_FOR_SERVER
# Construct action client, and wait for it to come active
self._action_client = SimpleActionClient(action_name, action_spec)
self._action_wait_thread = threading.Thread(
name=self._action_name + '/wait_for_server',
target=self._wait_for_server)
self._action_wait_thread.start()
self._execution_timer_thread = None
self._cancelation_timer_thread = None
# Condition variables for threading synchronization
self._done_cond = threading.Condition()
def _wait_for_server(self):
"""Internal method for waiting for the action server
This is run in a separate thread and allows construction of this state
to not block the construction of other states.
"""
timeout_time = rospy.get_rostime() + self._server_wait_timeout
while self._status == SimpleActionState.WAITING_FOR_SERVER and not rospy.is_shutdown(
) and not rospy.get_rostime() >= timeout_time:
try:
if self._action_client.wait_for_server(
rospy.Duration(1.0)): #self._server_wait_timeout):
self._status = SimpleActionState.INACTIVE
if self.preempt_requested():
return
except:
if not rospy.core._in_shutdown: # This is a hack, wait_for_server should not throw an exception just because shutdown was called
rospy.logerr("Failed to wait for action server '%s'" %
(self._action_name))
def _execution_timer(self):
"""Internal method for cancelling a timed out goal after a timeout."""
while self._status == SimpleActionState.ACTIVE and not rospy.is_shutdown(
):
try:
rospy.sleep(0.1)
except:
if not rospy.is_shutdown():
rospy.logerr("Failed to sleep while running '%s'" %
self._action_name)
if rospy.Time.now() - self._activate_time > self._exec_timeout:
goal = None
if isinstance(self._goal, unicode):
goal = self._goal.encode('utf8')
else:
goal = self._goal
rospy.logwarn(
"Action %s timed out after %d seconds. Cancelling goal: \n%s"
% (self._action_name, self._exec_timeout.to_sec(), goal))
# Cancel the goal
self.cancel_goal()
break
### smach State API
def request_preempt(self):
rospy.loginfo("Preempt requested on action '%s'" % (self._action_name))
smach.State.request_preempt(self)
if self._status == SimpleActionState.ACTIVE:
goal = None
if isinstance(self._goal, unicode):
goal = self._goal.encode('utf8')
else:
goal = self._goal
rospy.loginfo("Preempt on action '%s' cancelling goal: \n%s" %
(self._action_name, goal))
# Cancel the goal
self.cancel_goal()
def cancel_goal(self):
self._action_client.cancel_goal()
self._cancel_time = rospy.Time.now()
self._cancelation_timer_thread = threading.Thread(
name=self._action_name + '/cancel_watchdog',
target=self._cancelation_timer)
self._cancelation_timer_thread.start()
def _cancelation_timer(self):
while self._status == SimpleActionState.ACTIVE and not rospy.is_shutdown(
):
try:
rospy.sleep(0.1)
except:
if not rospy.is_shutdown():
rospy.logerr("Failed to sleep while running '%s'" %
self._action_name)
if rospy.Time.now() - self._cancel_time > self._cancel_timeout:
rospy.logerr(
"Action %s could not be canceled for more than %d seconds. Force state transition!"
% (self._action_name, self._cancel_timeout.to_sec()))
self._status = SimpleActionState.INACTIVE
self._done_cond.acquire()
self._done_cond.notify()
self._done_cond.release()
def execute(self, ud):
"""Called when executing a state.
This calls the goal_cb if it is defined, and then dispatches the
goal with a non-blocking call to the action client.
"""
# Make sure we're connected to the action server
if self._status is SimpleActionState.WAITING_FOR_SERVER:
rospy.logwarn(
"Still waiting for action server '%s' to start... is it running?"
% self._action_name)
if self._action_wait_thread.is_alive():
# Block on joining the server wait thread (This can be preempted)
self._action_wait_thread.join()
else:
# Wait for the server in this thread (This can also be preempted)
self._wait_for_server()
if not self.preempt_requested():
# In case of preemption we probably didn't connect
rospy.loginfo("Connected to action server '%s'." %
self._action_name)
# Check if server is still available
if self._status is SimpleActionState.INACTIVE:
try:
if not self._action_client.wait_for_server(
rospy.Duration(1.0)):
rospy.logerr("Failed to wait for action server '%s'" %
(self._action_name))
return 'aborted'
except:
if not rospy.core._in_shutdown: # This is a hack, wait_for_server should not throw an exception just because shutdown was called
rospy.logerr("Failed to wait for action server '%s'" %
(self._action_name))
return 'aborted'
# Check for preemption before executing
if self.preempt_requested():
rospy.loginfo("Preempting %s before sending goal." %
self._action_name)
self.service_preempt()
return 'preempted'
# We should only get here if we have connected to the server
if self._status is SimpleActionState.WAITING_FOR_SERVER:
rospy.logfatal("Action server for " + self._action_name +
" is not running.")
return 'aborted'
else:
self._status = SimpleActionState.INACTIVE
# Grab goal key, if set
if self._goal_key is not None:
self._goal = ud[self._goal_key]
# Write goal fields from userdata if set
for key in self._goal_slots:
setattr(self._goal, key, ud[key])
# Call user-supplied callback, if set, to get a goal
if self._goal_cb is not None:
try:
goal_update = self._goal_cb(
smach.Remapper(ud, self._goal_cb_input_keys,
self._goal_cb_output_keys, []), self._goal,
*self._goal_cb_args, **self._goal_cb_kwargs)
if goal_update is not None:
self._goal = goal_update
except:
rospy.logerr("Could not execute goal callback: " +
traceback.format_exc())
return 'aborted'
# Make sure the necessary paramters have been set
if self._goal is None and self._goal_cb is None:
rospy.logerr(
"Attempting to activate action " + self._action_name +
" with no goal or goal callback set. Did you construct the SimpleActionState properly?"
)
return 'aborted'
# Dispatch goal via non-blocking call to action client
self._activate_time = rospy.Time.now()
self._status = SimpleActionState.ACTIVE
# Wait on done condition
self._done_cond.acquire()
self._action_client.send_goal(self._goal, self._goal_done_cb,
self._goal_active_cb,
self._goal_feedback_cb)
# Preempt timeout watch thread
if self._exec_timeout:
self._execution_timer_thread = threading.Thread(
name=self._action_name + '/preempt_watchdog',
target=self._execution_timer)
self._execution_timer_thread.start()
# Wait for action to finish
self._done_cond.wait()
# Call user result callback if defined
result_cb_outcome = None
if self._result_cb is not None:
try:
result_cb_outcome = self._result_cb(
smach.Remapper(ud, self._result_cb_input_keys,
self._result_cb_output_keys, []),
self._goal_status, self._goal_result)
if result_cb_outcome is not None and result_cb_outcome not in self.get_registered_outcomes(
):
rospy.logerr(
"Result callback for action " + self._action_name +
", " + str(self._result_cb) +
" was not registered with the result_cb_outcomes argument. The result callback returned '"
+ str(result_cb_outcome) +
"' but the only registered outcomes are: " +
str(self.get_registered_outcomes()))
return 'aborted'
except:
rospy.logerr("Could not execute result callback: " +
traceback.format_exc())
return 'aborted'
if self._result_key is not None:
ud[self._result_key] = self._goal_result
for key in self._result_slots:
ud[key] = getattr(self._goal_result, key)
# Check status
if self._status == SimpleActionState.INACTIVE:
# Set the outcome on the result state
if self._goal_status == GoalStatus.SUCCEEDED:
outcome = 'succeeded'
elif self._goal_status == GoalStatus.PREEMPTED and self.preempt_requested(
):
outcome = 'preempted'
self.service_preempt()
else:
# All failures at this level are captured by aborting, even if we timed out
# This is an important distinction between local preemption, and preemption
# from above.
outcome = 'aborted'
else:
# We terminated without going inactive
rospy.logwarn(
"Action state terminated without going inactive first.")
outcome = 'aborted'
# Check custom result cb outcome
if result_cb_outcome is not None:
outcome = result_cb_outcome
# Set status inactive
self._status = SimpleActionState.INACTIVE
self._done_cond.release()
if self.preempt_requested():
outcome = 'preempted'
self.service_preempt()
return outcome
### Action client callbacks
def _goal_active_cb(self):
"""Goal Active Callback
This callback starts the timer that watches for the timeout specified for this action.
"""
rospy.logdebug("Action " + self._action_name + " has gone active.")
def _goal_feedback_cb(self, feedback):
"""Goal Feedback Callback"""
rospy.logdebug("Action " + self._action_name + " sent feedback.")
def _goal_done_cb(self, result_state, result):
"""Goal Done Callback
This callback resets the active flags and reports the duration of the action.
Also, if the user has defined a result_cb, it is called here before the
method returns.
"""
def get_result_str(i):
strs = ('PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'LOST')
if i < len(strs):
return strs[i]
else:
return 'UNKNOWN (' + str(i) + ')'
# Calculate duration
self._duration = rospy.Time.now() - self._activate_time
rospy.logdebug("Action "+self._action_name+" terminated after "\
+str(self._duration.to_sec())+" seconds with result "\
+get_result_str(self._action_client.get_state())+".")
# Store goal state
self._goal_status = result_state
self._goal_result = result
# Rest status
self._status = SimpleActionState.INACTIVE
# Notify done
self._done_cond.acquire()
self._done_cond.notify()
self._done_cond.release()
def main():
# Initialize ROS node
rospy.init_node("example_04_trajectory_node", disable_signals=True)
rate = rospy.Rate(200)
group_name = "my_group"
num_joints = 3
num_waypoints = 5
# Create a client which uses the service to create a group
add_group_client = rospy.ServiceProxy("hebiros/add_group_from_names", AddGroupFromNamesSrv)
# Create a subscriber to receive feedback from a group
# Register feedback callback which runs when feedback is received
feedback_subscriber = rospy.Subscriber("/hebiros/"+group_name+"/feedback/joint_state", JointState, feedback_callback, queue_size=100)
# Construct a group using 3 known modules
rospy.wait_for_service("hebiros/add_group_from_names")
req_group_name = group_name
req_names = ["base", "shoulder", "elbow"]
req_families = ["HEBI"]
add_group_client(req_group_name, req_names, req_families)
# Create an action client for executing a trajectory
client = SimpleActionClient("/hebiros/"+group_name+"/trajectory", TrajectoryAction)
# Wait for the action server corresponding to the action client
client.wait_for_server()
user_input = raw_input("Press Enter/Return to execute Trajectory")
# Construct a trajectory to be sent as an action goal
goal = TrajectoryGoal()
# Set the times to reach each waypoint in seconds
times = [0, 5, 10, 15, 20]
names = ["HEBI/base", "HEBI/shoulder", "HEBI/elbow"]
# Wait for feedback from actuators
while not rospy.is_shutdown() and feedback.position is not None and len(feedback.position) < len(names):
rate.sleep()
# Set positions, velocities, and accelerations for each waypoint and each joint
# The following vectors have one joint per row and one waypoint per column
positions = [[feedback.position[0], 0, M_PI_2, 0, 0],
[feedback.position[1], 0, M_PI_2, M_PI_2, 0],
[feedback.position[2], 0, 0, M_PI_2, 0]]
velocities = [[0, NAN, NAN, NAN, 0],
[0, NAN, NAN, NAN, 0],
[0, NAN, NAN, NAN, 0]]
accelerations = [[0, NAN, NAN, NAN, 0],
[0, NAN, NAN, NAN, 0],
[0, NAN, NAN, NAN, 0]]
# Construct the goal using the TrajectoryGoal format
for i in range(num_waypoints):
waypoint = WaypointMsg()
waypoint.names = names
waypoint.positions = [joint[i] for joint in positions]
waypoint.velocities = [joint[i] for joint in velocities]
waypoint.accelerations = [joint[i] for joint in accelerations]
goal.waypoints.append(waypoint)
goal.times.append(times[i])
# Send the goal, executing the trajectory
client.send_goal(goal, trajectory_done, trajectory_active, trajectory_feedback)
while not rospy.is_shutdown():
rate.sleep()
def main():
## Parse args
parser = parse_args(sys.argv[1:])
hebi_group_name = parser.hebi_group_name
hebi_families = [
parser.hebi_base_family, parser.hebi_shoulder_family,
parser.hebi_elbow_family
]
hebi_names = [
parser.hebi_base_name, parser.hebi_shoulder_name,
parser.hebi_elbow_name
]
from_master_topic = parser.from_master_topic
to_master_topic = parser.to_master_topic
# ROS stuff
rospy.init_node('smach_fsm_using_execute_joint_trajectory_state',
anonymous=True)
rate = rospy.Rate(200)
# HEBI setup - NOTE: hebiros_node must be running
rospy.loginfo("HEBI Group name: " + str(hebi_group_name))
rospy.loginfo("HEBI families: " + str(hebi_families))
rospy.loginfo("HEBI names: " + str(hebi_names))
hebi_mapping = [
family + '/' + name for family, name in zip(hebi_families, hebi_names)
]
# Create a service client to create a group
set_hebi_group = rospy.ServiceProxy('/hebiros/add_group_from_names',
AddGroupFromNamesSrv)
# Create a service client to set the command lifetime
set_command_lifetime = rospy.ServiceProxy(
"/hebiros/" + hebi_group_name + "/set_command_lifetime",
SetCommandLifetimeSrv)
# Topic to receive feedback from a group
hebi_group_fbk_topic = "/hebiros/" + hebi_group_name + "/feedback/joint_state"
rospy.loginfo(" hebi_group_feedback_topic: %s",
"/hebiros/" + hebi_group_name + "/feedback/joint_state")
# Topic to send commands to a group
hebi_group_cmd_topic = "/hebiros/" + hebi_group_name + "/command/joint_state"
rospy.loginfo(" hebi_group_command_topic: %s",
"/hebiros/" + hebi_group_name + "/command/joint_state")
# Call the /hebiros/add_group_from_names service to create a group
rospy.loginfo(" Waiting for AddGroupFromNamesSrv at %s ...",
'/hebiros/add_group_from_names')
rospy.wait_for_service('/hebiros/add_group_from_names')
rospy.loginfo(" AddGroupFromNamesSrv AVAILABLE.")
set_hebi_group(hebi_group_name, hebi_names, hebi_families)
# TrajectoryAction client
trajectory_action_client = SimpleActionClient(
"/hebiros/" + hebi_group_name + "/trajectory", TrajectoryAction)
rospy.loginfo(" Waiting for TrajectoryActionServer at %s ...",
"/hebiros/" + hebi_group_name + "/trajectory")
trajectory_action_client.wait_for_server(
) # block until action server starts
rospy.loginfo(" TrajectoryActionServer AVAILABLE.")
### CREATE ARM STATE INSTANCES ###
# trace line
trace_line = ExecuteJointTrajectoryFromFile("hebi_3dof_arm_description",
"trace_line_0.json",
hebi_mapping,
trajectory_action_client,
set_command_lifetime,
hebi_group_fbk_topic,
hebi_group_cmd_topic,
setup_time=3.0)
# stir pot
stir_pot_setup = ExecuteJointTrajectoryFromFile(
"hebi_3dof_arm_description",
"stir_pot_0.json",
hebi_mapping,
trajectory_action_client,
set_command_lifetime,
hebi_group_fbk_topic,
hebi_group_cmd_topic,
setup_time=2.0)
stir_pot_loop_1 = ExecuteJointTrajectoryFromFile(
"hebi_3dof_arm_description",
"stir_pot_1.json",
hebi_mapping,
trajectory_action_client,
set_command_lifetime,
hebi_group_fbk_topic,
hebi_group_cmd_topic,
setup_time=1.0)
stir_pot_loop_n = ExecuteJointTrajectoryFromFile(
"hebi_3dof_arm_description",
"stir_pot_1.json",
hebi_mapping,
trajectory_action_client,
set_command_lifetime,
hebi_group_fbk_topic,
hebi_group_cmd_topic,
setup_time=0.0)
break_stir_pot_loop = BreakOnMsg("break_topic_1")
stir_pot_exit = ExecuteJointTrajectoryFromFile("hebi_3dof_arm_description",
"stir_pot_2.json",
hebi_mapping,
trajectory_action_client,
set_command_lifetime,
hebi_group_fbk_topic,
hebi_group_cmd_topic,
setup_time=1.0)
# stab bag of veggies
stab_bag_setup = ExecuteJointTrajectoryFromFile(
"hebi_3dof_arm_description",
"stab_bag_0.json",
hebi_mapping,
trajectory_action_client,
set_command_lifetime,
hebi_group_fbk_topic,
hebi_group_cmd_topic,
setup_time=3.0)
stab_bag_loop_n = ExecuteJointTrajectoryFromFile(
"hebi_3dof_arm_description",
"stab_bag_1.json",
hebi_mapping,
trajectory_action_client,
set_command_lifetime,
hebi_group_fbk_topic,
hebi_group_cmd_topic,
setup_time=0.5)
break_stab_bag_loop = BreakOnMsg("break_topic_2")
stab_bag_exit = ExecuteJointTrajectoryFromFile("hebi_3dof_arm_description",
"stab_bag_2.json",
hebi_mapping,
trajectory_action_client,
set_command_lifetime,
hebi_group_fbk_topic,
hebi_group_cmd_topic,
setup_time=1.5)
### CREATE TOP SM ###
top = StateMachine(outcomes=['exit', 'success'])
### INITIALIZE USERDATA ###
# Updated by ExecuteJointTrajectoryFromFile State instances
top.userdata.final_joint_positions = [None, None, None]
# Keeps things a tad neater
remapping_dict = {'final_joint_positions': 'final_joint_positions'}
with top:
### ADD LEG STATES TO THE TOP SM ###
StateMachine.add('TRACE_LINE',
trace_line,
transitions={
'exit': 'exit',
'failure': 'STIR_POT_SETUP',
'success': 'STIR_POT_SETUP'
},
remapping=remapping_dict)
StateMachine.add('STIR_POT_SETUP',
stir_pot_setup,
transitions={
'exit': 'exit',
'failure': 'STIR_POT_SETUP',
'success': 'STIR_POT_LOOP_1'
},
remapping=remapping_dict)
StateMachine.add('STIR_POT_LOOP_1',
stir_pot_loop_1,
transitions={
'exit': 'exit',
'failure': 'STIR_POT_LOOP_N',
'success': 'STIR_POT_LOOP_N'
},
remapping=remapping_dict)
StateMachine.add('STIR_POT_LOOP_N',
stir_pot_loop_n,
transitions={
'exit': 'exit',
'failure': 'STIR_POT_LOOP_N',
'success': 'BREAK_STIR_POT_LOOP'
},
remapping=remapping_dict)
StateMachine.add('BREAK_STIR_POT_LOOP',
break_stir_pot_loop,
transitions={
'exit': 'exit',
'true': 'STIR_POT_EXIT',
'false': 'STIR_POT_LOOP_N'
})
StateMachine.add('STIR_POT_EXIT',
stir_pot_exit,
transitions={
'exit': 'exit',
'failure': 'exit',
'success': 'STAB_BAG_SETUP'
},
remapping=remapping_dict)
StateMachine.add('STAB_BAG_SETUP',
stab_bag_setup,
transitions={
'exit': 'exit',
'failure': 'STAB_BAG_SETUP',
'success': 'STAB_BAG_LOOP_N'
},
remapping=remapping_dict)
StateMachine.add('STAB_BAG_LOOP_N',
stab_bag_loop_n,
transitions={
'exit': 'exit',
'failure': 'STAB_BAG_LOOP_N',
'success': 'BREAK_STAB_BAG_LOOP'
},
remapping=remapping_dict)
StateMachine.add('BREAK_STAB_BAG_LOOP',
break_stab_bag_loop,
transitions={
'exit': 'exit',
'true': 'STAB_BAG_EXIT',
'false': 'STAB_BAG_LOOP_N'
})
StateMachine.add('STAB_BAG_EXIT',
stir_pot_exit,
transitions={
'exit': 'exit',
'failure': 'exit',
'success': 'success'
},
remapping=remapping_dict)
sis = smach_ros.IntrospectionServer(str(rospy.get_name()), top,
'/SM_ROOT' + str(rospy.get_name()))
sis.start()
user_input = raw_input(
"Please press the 'Return/Enter' key to start executing \
- pkg: smach_fsm_using_execute_joint_trajectory_state.py | node: " +
str(rospy.get_name()) + "\n")
print("Input received. Executing "
"- pkg: smach_fsm_using_execute_joint_trajectory_state | node: " +
str(rospy.get_name()) + "\n")
top.execute()
rospy.spin()
sis.stop()
print("\nExiting " + str(rospy.get_name()))
class Hexapod(object):
"""
Attributes
hebi_group_name (str):
hebi_mapping (list of list of str):
leg_base_links (list of str):
leg_end_links (list of str):
"""
def __init__(self, hebi_group_name, hebi_mapping, leg_base_links,
leg_end_links):
rospy.loginfo("Creating Hexapod instance...")
hebi_families = []
hebi_names = []
for leg in hebi_mapping:
for actuator in leg:
family, name = actuator.split('/')
hebi_families.append(family)
hebi_names.append(name)
rospy.loginfo(" hebi_group_name: %s", hebi_group_name)
rospy.loginfo(" hebi_families: %s", hebi_families)
rospy.loginfo(" hebi_names: %s", hebi_names)
self.hebi_mapping = hebi_mapping
self.hebi_mapping_flat = self._flatten(self.hebi_mapping)
# jt information populated by self._feedback_cb
self._current_jt_pos = {}
self._current_jt_vel = {}
self._current_jt_eff = {}
self._joint_state_pub = None
self._hold_leg_list = [False, False, False, False, False, False]
self._hold_leg_positions = self._get_list_of_lists()
# Create a service client to create a group
set_hebi_group = rospy.ServiceProxy('/hebiros/add_group_from_names',
AddGroupFromNamesSrv)
# Topic to receive feedback from a group
self.hebi_group_feedback_topic = "/hebiros/" + hebi_group_name + "/feedback/joint_state"
rospy.loginfo(" hebi_group_feedback_topic: %s",
"/hebiros/" + hebi_group_name + "/feedback/joint_state")
# Topic to send commands to a group
self.hebi_group_command_topic = "/hebiros/" + hebi_group_name + "/command/joint_state"
rospy.loginfo(" hebi_group_command_topic: %s",
"/hebiros/" + hebi_group_name + "/command/joint_state")
# Call the /hebiros/add_group_from_names service to create a group
rospy.loginfo(" Waiting for AddGroupFromNamesSrv at %s ...",
'/hebiros/add_group_from_names')
rospy.wait_for_service('/hebiros/add_group_from_names'
) # block until service server starts
rospy.loginfo(" AddGroupFromNamesSrv AVAILABLE.")
set_hebi_group(hebi_group_name, hebi_names, hebi_families)
# Create a service client to set group settings
change_group_settings = rospy.ServiceProxy(
"/hebiros/" + hebi_group_name +
"/send_command_with_acknowledgement",
SendCommandWithAcknowledgementSrv)
rospy.loginfo(
" Waiting for SendCommandWithAcknowledgementSrv at %s ...",
"/hebiros/" + hebi_group_name +
"/send_command_with_acknowledgement")
rospy.wait_for_service("/hebiros/" + hebi_group_name +
"/send_command_with_acknowledgement"
) # block until service server starts
cmd_msg = CommandMsg()
cmd_msg.name = self.hebi_mapping_flat
cmd_msg.settings.name = self.hebi_mapping_flat
cmd_msg.settings.position_gains.name = self.hebi_mapping_flat
cmd_msg.settings.position_gains.kp = [5, 8, 2] * LEGS
cmd_msg.settings.position_gains.ki = [0.001] * LEGS * ACTUATORS_PER_LEG
cmd_msg.settings.position_gains.kd = [0] * LEGS * ACTUATORS_PER_LEG
cmd_msg.settings.position_gains.i_clamp = [
0.25
] * LEGS * ACTUATORS_PER_LEG # TODO: Tune this. Setting it low for testing w/o restarting Gazebo
change_group_settings(cmd_msg)
# Feedback/Command
self.fbk_sub = rospy.Subscriber(self.hebi_group_feedback_topic,
JointState, self._feedback_cb)
self.cmd_pub = rospy.Publisher(self.hebi_group_command_topic,
JointState,
queue_size=1)
self._hold_position = False
self._hold_joint_states = {}
# TrajectoryAction client
self.trajectory_action_client = SimpleActionClient(
"/hebiros/" + hebi_group_name + "/trajectory", TrajectoryAction)
rospy.loginfo(" Waiting for TrajectoryActionServer at %s ...",
"/hebiros/" + hebi_group_name + "/trajectory")
self.trajectory_action_client.wait_for_server(
) # block until action server starts
rospy.loginfo(" TrajectoryActionServer AVAILABLE.")
# Twist Subscriber
self._cmd_vel_sub = rospy.Subscriber("/hexapod/cmd_vel/", Twist,
self._cmd_vel_cb)
self.last_vel_cmd = None
self.linear_displacement_limit = 0.075 # m
self.angular_displacement_limit = 0.65 # rad
# Check ROS Parameter server for robot_description URDF
urdf_str = ""
urdf_loaded = False
while not rospy.is_shutdown() and not urdf_loaded:
if rospy.has_param('/robot_description'):
urdf_str = rospy.get_param('/robot_description')
urdf_loaded = True
rospy.loginfo(
"Pulled /robot_description from parameter server.")
else:
rospy.sleep(0.01) # sleep for 10 ms of ROS time
# pykdl_utils setup
self.robot_urdf = URDF.from_xml_string(urdf_str)
self.kdl_fk_base_to_leg_base = [
KDLKinematics(self.robot_urdf, 'base_link', base_link)
for base_link in leg_base_links
]
self.kdl_fk_leg_base_to_eff = [
KDLKinematics(self.robot_urdf, base_link, end_link)
for base_link, end_link in zip(leg_base_links, leg_end_links)
]
# trac-ik setup
self.trac_ik_leg_base_to_end = [
IK(
base_link,
end_link,
urdf_string=urdf_str,
timeout=0.01, # TODO: Tune me
epsilon=1e-4,
solve_type="Distance")
for base_link, end_link in zip(leg_base_links, leg_end_links)
]
self.ik_pos_xyz_bounds = [0.01, 0.01, 0.01]
self.ik_pos_wxyz_bounds = [31416.0, 31416.0, 31416.0
] # NOTE: This implements position-only IK
# Wait for connections to be set up
rospy.loginfo("Wait for ROS connections to be set up...")
while not rospy.is_shutdown() and len(self._current_jt_pos) < len(
self.hebi_mapping_flat):
rospy.sleep(0.1)
# Set up joint state publisher
self._joint_state_pub = rospy.Publisher('/joint_states',
JointState,
queue_size=1)
self._loop_rate = rospy.Rate(20)
# leg joint home pos Hip, Knee, Ankle
self.leg_jt_home_pos = [
[0.0, +0.26, -1.57], # Leg 1
[0.0, -0.26, +1.57], # Leg 2
[0.0, +0.26, -1.57], # Leg 3
[0.0, -0.26, +1.57], # Leg 4
[0.0, +0.26, -1.57], # Leg 5
[0.0, -0.26, +1.57]
] # Leg 6
# leg end-effector home position
self.leg_eff_home_pos = self._get_leg_base_to_eff_fk(
self.leg_jt_home_pos)
# leg step height relative to leg base link
self.leg_eff_step_height = [[]] * LEGS # relative to leg base
for i, fk_solver in enumerate(self.kdl_fk_base_to_leg_base):
base_to_leg_base_rot = fk_solver.forward([])[:3, :3]
step_ht_chassis = np.array([0, 0, STEP_HEIGHT])
step_ht_leg_base = np.dot(base_to_leg_base_rot, step_ht_chassis)
self.leg_eff_step_height[i] = step_ht_leg_base.tolist()[0]
self._odd_starts = True
rospy.loginfo("Done creating Hexapod instance...")
def stand_up(self):
rospy.loginfo("Hexapod standing up...")
current_leg_positions = self._get_joint_angles()
goal = TrajectoryGoal()
start_wp = WaypointMsg()
start_wp.names = self.hebi_mapping_flat
start_wp.positions = self._flatten(current_leg_positions)
start_wp.velocities = [0.0] * ACTUATORS_TOTAL
start_wp.accelerations = [0.0] * ACTUATORS_TOTAL
goal.waypoints.append(start_wp)
goal.times.append(0.0)
end_wp = WaypointMsg()
end_wp.names = self.hebi_mapping_flat
end_wp.positions = self._flatten(self.leg_jt_home_pos)
end_wp.velocities = [0.0] * ACTUATORS_TOTAL
end_wp.accelerations = [0.0] * ACTUATORS_TOTAL
goal.waypoints.append(end_wp)
goal.times.append(4.0)
self.trajectory_action_client.send_goal(
goal) # TODO: Add the various callbacks
self.trajectory_action_client.wait_for_result()
self.hold_pos([1, 2, 3, 4, 5, 6])
def loop(self):
"""Main Hexapod loop (distant - somewhat less accomplished - relative of HEBI algorithm)
- Get chassis translation
- Get leg end-effector translations (relative to leg base link)
- side_alpha:odd, side_beta:even or side_alpha:even, side_beta:odd
- side_alpha legs lift, plant to +transformation
- side_alpha legs push to new home positions; side_beta legs push to -transformation
- swap side_alpha and side_beta
"""
rospy.loginfo("Hexapod entering main loop...")
rospy.loginfo(
" Waiting for initial velocity command on /hexapod/cmd_vel/ ...")
while self.last_vel_cmd is None:
self._loop_rate.sleep()
# start main loop
while not rospy.is_shutdown():
chassis_pos_delta = None
if self.last_vel_cmd is not None:
dt = 1 # FIXME: Temporary for debugging
lin_disp_lmt = self.linear_displacement_limit
ang_disp_lmt = self.angular_displacement_limit
chassis_pos_delta = Twist()
chassis_pos_delta.linear.x = clamp(
self.last_vel_cmd.linear.x * dt, -lin_disp_lmt,
lin_disp_lmt)
chassis_pos_delta.linear.y = clamp(
self.last_vel_cmd.linear.y * dt, -lin_disp_lmt,
lin_disp_lmt)
chassis_pos_delta.linear.z = clamp(
self.last_vel_cmd.linear.z * dt, -lin_disp_lmt,
lin_disp_lmt)
chassis_pos_delta.angular.x = clamp(
self.last_vel_cmd.angular.x * dt, -ang_disp_lmt,
ang_disp_lmt)
chassis_pos_delta.angular.y = clamp(
self.last_vel_cmd.angular.y * dt, -ang_disp_lmt,
ang_disp_lmt)
chassis_pos_delta.angular.z = clamp(
self.last_vel_cmd.angular.z * dt, -ang_disp_lmt,
ang_disp_lmt)
self.last_vel_cmd = None
if chassis_pos_delta is not None \
and not self._check_if_twist_msg_is_zero(chassis_pos_delta, linear_threshold=0.005, angular_threshold=0.01):
# Get chassis position transformation
chassis_pos_rot = transforms.euler_matrix(
chassis_pos_delta.angular.x, chassis_pos_delta.angular.y,
chassis_pos_delta.angular.z)[:3, :3]
rospy.loginfo("chassis_pos_rot: %s", chassis_pos_rot)
chassis_pos_trans = np.zeros([3])
chassis_pos_trans[0] = chassis_pos_delta.linear.x
chassis_pos_trans[1] = chassis_pos_delta.linear.y
chassis_pos_trans[2] = chassis_pos_delta.linear.z
chassis_translation = np.dot(chassis_pos_trans,
chassis_pos_rot)
rospy.loginfo("chassis_translation: %s", chassis_translation)
leg_target_eff_translation = [[]] * LEGS
# Get leg base positions relative to chassis
leg_base_positions = self._get_base_to_leg_base_fk()
for i, leg_base_pos in enumerate(leg_base_positions):
leg_base_pos_arr = np.array(leg_base_pos).reshape(3, 1)
leg_base_pos_arr_new = np.dot(chassis_pos_rot,
leg_base_pos_arr)
leg_base_pos_trans_4 = np.ones(4).reshape(4, 1)
leg_base_pos_trans_4[:3, :] = leg_base_pos_arr_new
# get leg base translations relative to leg_base coordinate frame
relative_trans = np.dot(
np.linalg.inv(self.kdl_fk_base_to_leg_base[i].forward(
[])), leg_base_pos_trans_4)
relative_trans = relative_trans.reshape(1,
4).tolist()[0][:3]
leg_target_eff_translation[i] = relative_trans
# Get leg target end-effector translations
for i, q in enumerate(self.leg_jt_home_pos):
base_to_leg_base_rot = self.kdl_fk_base_to_leg_base[
i].forward([])[:3, :3]
leg_target_eff_trans = np.dot(
np.linalg.inv(base_to_leg_base_rot),
chassis_translation).tolist()[0]
leg_target_eff_translation[i] = [
x + y for x, y in zip(leg_target_eff_translation[i],
leg_target_eff_trans)
] # TODO: FIXME: Technically incorrect
# 1: side_alpha legs lift, plant to +transformation
rospy.loginfo(
"1: side_alpha legs lift, plant to +transformation")
if self._odd_starts:
active_legs = [1, 2, 5]
else: # even starts
active_legs = [0, 3, 4]
init_wp = WaypointMsg()
lift_wp = WaypointMsg()
end_wp = WaypointMsg()
legs_jt_init_pos = self._get_joint_angles()
leg_eff_cur_pos = self._get_leg_base_to_eff_fk(
legs_jt_init_pos)
for i in range(LEGS):
motor_names = [name for name in self.hebi_mapping[i]]
# INITIAL POSITION
init_wp.names.extend(motor_names)
init_wp.positions.extend(legs_jt_init_pos[i])
init_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG)
init_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG)
# LIFT
lift_wp.names.extend(motor_names)
if i in active_legs:
# apply translation
leg_lift_eff_target_pos = [
(x + y + z) / 2.0 for x, y, z in zip(
leg_eff_cur_pos[i], self.leg_eff_home_pos[i],
leg_target_eff_translation[i])
]
leg_lift_eff_target_pos = [
x + y for x, y in zip(leg_lift_eff_target_pos,
self.leg_eff_step_height[i])
]
# get ik
leg_lift_jt_target_pos = self._get_pos_ik(
self.trac_ik_leg_base_to_end[i],
legs_jt_init_pos[i],
leg_lift_eff_target_pos,
seed_xyz=self.leg_eff_home_pos[i])
lift_wp.positions.extend(leg_lift_jt_target_pos)
lift_wp.velocities.extend([NAN] * ACTUATORS_PER_LEG)
lift_wp.accelerations.extend([NAN] * ACTUATORS_PER_LEG)
else:
lift_wp.positions.extend(legs_jt_init_pos[i])
lift_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG)
lift_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG)
# PLANT
end_wp.names.extend(motor_names)
if i in active_legs:
# apply translation
leg_plant_eff_target_pos = [
x + y
for x, y in zip(self.leg_eff_home_pos[i],
leg_target_eff_translation[i])
]
leg_plant_eff_target_pos[2] = self.leg_eff_home_pos[i][
2] # end eff z-position should match home z-position
# get ik
leg_plant_jt_target_pos = self._get_pos_ik(
self.trac_ik_leg_base_to_end[i],
leg_lift_jt_target_pos,
leg_plant_eff_target_pos,
seed_xyz=leg_lift_eff_target_pos)
end_wp.positions.extend(leg_plant_jt_target_pos)
end_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG)
end_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG)
else:
end_wp.positions.extend(legs_jt_init_pos[i])
end_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG)
end_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG)
goal = TrajectoryGoal()
goal.waypoints.append(init_wp)
goal.waypoints.append(lift_wp)
goal.waypoints.append(end_wp)
goal.times.extend([0.0, 0.4, 0.8])
self.release_pos([1, 2, 3, 4, 5, 6])
self.trajectory_action_client.send_goal(goal)
self.trajectory_action_client.wait_for_result()
self.hold_pos([1, 2, 3, 4, 5, 6])
# 2: side_alpha legs push to new home positions; side_beta legs push to -transformation
rospy.loginfo(
"2: side_alpha legs push to new home positions; side_beta legs push to -transformation"
)
if self._odd_starts:
active_legs = [0, 3, 4]
else: # even starts
active_legs = [1, 2, 5]
init_wp = WaypointMsg()
end_wp = WaypointMsg()
legs_jt_init_pos = self._get_joint_angles()
for i in range(LEGS):
motor_names = [name for name in self.hebi_mapping[i]]
# INITIAL POSITION
init_wp.names.extend(motor_names)
init_wp.positions.extend(legs_jt_init_pos[i])
init_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG)
init_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG)
# PUSH
end_wp.names.extend(motor_names)
if i in active_legs:
# apply -translation
leg_plant_eff_target_pos = [
x + y for x, y in zip(self.leg_eff_home_pos[i], [
-val for val in leg_target_eff_translation[i]
])
]
leg_plant_eff_target_pos[2] = self.leg_eff_home_pos[i][
2] # end eff z-position should match home z-position
# get ik
leg_plant_jt_target_pos = self._get_pos_ik(
self.trac_ik_leg_base_to_end[i],
legs_jt_init_pos[i],
leg_plant_eff_target_pos,
seed_xyz=self.leg_eff_home_pos[i])
end_wp.positions.extend(leg_plant_jt_target_pos)
end_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG)
end_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG)
else:
end_wp.positions.extend(self.leg_jt_home_pos[i])
end_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG)
end_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG)
goal = TrajectoryGoal()
goal.waypoints.append(init_wp)
goal.waypoints.append(end_wp)
goal.times.extend([0.0, 0.4])
self.release_pos([1, 2, 3, 4, 5, 6])
self.trajectory_action_client.send_goal(goal)
self.trajectory_action_client.wait_for_result()
self.hold_pos([1, 2, 3, 4, 5, 6])
self._odd_starts = not self._odd_starts
self._loop_rate.sleep(
) # FIXME: Doesn't make sense to use this unless re-planning trajectories
# end main loop
def _get_pos_ik(self,
ik_solver,
seed_angles,
target_xyz,
target_wxyz=None,
seed_xyz=None,
recursion_depth_cnt=100):
if recursion_depth_cnt < 0:
rospy.logdebug("%s FAILURE. Maximum recursion depth reached",
self._get_pos_ik.__name__)
return None
rospy.logdebug("recursion depth = %s", recursion_depth_cnt)
if target_wxyz is None:
target_wxyz = [
1, 0, 0, 0
] # trak-ik seems a little more stable when given initial pose for pos-only ik
target_jt_angles = ik_solver.get_ik(
seed_angles, target_xyz[0], target_xyz[1], target_xyz[2],
target_wxyz[0], target_wxyz[1], target_wxyz[2], target_wxyz[3],
self.ik_pos_xyz_bounds[0], self.ik_pos_xyz_bounds[1],
self.ik_pos_xyz_bounds[2], self.ik_pos_wxyz_bounds[0],
self.ik_pos_wxyz_bounds[1], self.ik_pos_wxyz_bounds[2])
if target_jt_angles is not None: # ik_solver succeeded
rospy.logdebug(
"%s SUCCESS. Solution: %s to target xyz: %s from seed angles: %s",
self._get_pos_ik.__name__, round_list(target_jt_angles, 4),
round_list(target_xyz, 4), round_list(seed_angles, 4))
return target_jt_angles
else: # ik_solver failed
if seed_xyz is None:
rospy.logdebug(
"%s FAILURE. Solution: %s to target_xyz: %s from seed_angles: %s",
self._get_pos_ik.__name__, ['NA', 'NA', 'NA'], target_xyz,
seed_angles)
return target_jt_angles
else:
# binary recursive search
target_xyz_new = [(x + y) / 2.0
for x, y in zip(target_xyz, seed_xyz)]
recursive_jt_angles = self._get_pos_ik(ik_solver, seed_angles,
target_xyz_new,
target_wxyz, seed_xyz,
recursion_depth_cnt - 1)
if recursive_jt_angles is None:
rospy.logdebug(
"%s FAILURE. Solution: %s to target_xyz: %s from seed_angles: %s",
self._get_pos_ik.__name__, ['NA', 'NA', 'NA'],
round_list(target_xyz, 4), round_list(seed_angles, 4))
else:
return self._get_pos_ik(ik_solver, recursive_jt_angles,
target_xyz, target_wxyz,
target_xyz_new,
recursion_depth_cnt - 1)
def _get_base_to_leg_base_fk(self):
leg_base_pos = [[]] * LEGS
for i, fk_solver in enumerate(self.kdl_fk_base_to_leg_base):
base_to_leg_base_tf = fk_solver.forward([])
leg_base_pos[i] = base_to_leg_base_tf[:3, 3].reshape(1,
3).tolist()[0]
return leg_base_pos
def _get_leg_base_to_eff_fk(self, jt_angles):
leg_eff_cur_pos = [[]] * LEGS # relative to leg base
for i, (fk_solver, angles) \
in enumerate(zip(self.kdl_fk_leg_base_to_eff, jt_angles)):
leg_base_to_eff_tf = fk_solver.forward(angles)
leg_eff_cur_pos[i] = leg_base_to_eff_tf[:3,
3].reshape(1,
3).tolist()[0]
return leg_eff_cur_pos
def _get_base_to_leg_eff_fk(self, jt_angles):
leg_eff_cur_pos = [[]] * LEGS # relative to base
for i, (fk_solver_1, fk_solver_2, angles) \
in enumerate(zip(self.kdl_fk_base_to_leg_base, self.kdl_fk_leg_base_to_eff, jt_angles)):
base_to_leg_base_tf = fk_solver_1.forward([])
leg_base_to_eff_tf = fk_solver_2.forward(angles)
base_to_eff_tf = np.dot(base_to_leg_base_tf, leg_base_to_eff_tf)
leg_eff_cur_pos[i] = base_to_eff_tf[:3, 3].reshape(1,
3).tolist()[0]
return leg_eff_cur_pos
def hold_pos(self, leg_nums):
released_leg = False
for num in leg_nums:
num -= 1 # Convert from 1-based leg numbering to 0-based indexing
if not self._hold_leg_list[num]:
released_leg = True
break
if released_leg:
self._hold_leg_positions = self._get_joint_angles()
for num in leg_nums:
num -= 1 # Convert from 1-based leg numbering to 0-based indexing
self._hold_leg_list[num] = True
def release_pos(self, leg_nums):
for num in leg_nums:
num -= 1 # Convert from 1-based leg numbering to 0-based indexing
self._hold_leg_list[num] = False
def _get_joint_angles(self):
return [[self._current_jt_pos[motor] for motor in leg]
for leg in self.hebi_mapping]
def _get_joint_velocities(self):
return [[self._current_jt_vel[motor] for motor in leg]
for leg in self.hebi_mapping]
def _get_joint_efforts(self):
return [[self._current_jt_eff[motor] for motor in leg]
for leg in self.hebi_mapping]
@staticmethod
def _get_list_of_lists(item=None):
return [[item] * ACTUATORS_PER_LEG] * LEGS
@staticmethod
def _flatten(listoflists):
return [item for lst in listoflists for item in lst]
def _feedback_cb(self, msg):
for name, pos, vel, eff in zip(msg.name, msg.position, msg.velocity,
msg.effort):
if name not in self.hebi_mapping_flat:
print("WARNING: arm_callback - unrecognized name!!!")
else:
self._current_jt_pos[name] = pos
self._current_jt_vel[name] = vel
self._current_jt_eff[name] = eff
# Publish JointState() for RViz
if not rospy.is_shutdown(
) and self._joint_state_pub is not None:
jointstate = JointState()
jointstate.header.stamp = rospy.Time.now()
jointstate.name = self.hebi_mapping_flat
jointstate.position = self._flatten(
self._get_joint_angles())
jointstate.velocity = [0.0] * len(jointstate.name)
jointstate.effort = [0.0] * len(jointstate.name)
self._joint_state_pub.publish(jointstate)
# Publish JointState() for held legs
# TODO: Probably better to put in its own callback
# TODO: Trigger at regular intervals and when self._hold_leg_list changes
# TODO: Smooth transition from trajectory to cmd
if not rospy.is_shutdown(
) and self._joint_state_pub is not None and any(self._hold_leg_list):
jointstate = JointState()
jointstate.header.stamp = rospy.Time.now()
for i, leg in enumerate(self.hebi_mapping):
if self._hold_leg_list[i]:
jointstate.name.extend(leg)
jointstate.position.extend(self._hold_leg_positions[i])
jointstate.velocity = []
jointstate.effort = [] # TODO: Gravity compensation?
self.cmd_pub.publish(jointstate)
def _cmd_vel_cb(self, msg):
if isinstance(msg, Twist):
if self.last_vel_cmd is None:
self.last_vel_cmd = Twist()
self.last_vel_cmd.linear.x = msg.linear.x
self.last_vel_cmd.linear.y = msg.linear.y
self.last_vel_cmd.linear.z = msg.linear.z
self.last_vel_cmd.angular.x = msg.angular.x
self.last_vel_cmd.angular.y = msg.angular.y
self.last_vel_cmd.angular.z = msg.angular.z
@staticmethod
def _check_if_twist_msg_is_zero(twist_msg, linear_threshold,
angular_threshold):
assert isinstance(twist_msg, Twist)
if abs(twist_msg.linear.x) > linear_threshold:
return False
elif abs(twist_msg.linear.y) > linear_threshold:
return False
elif abs(twist_msg.linear.z) > linear_threshold:
return False
elif abs(twist_msg.angular.x) > angular_threshold:
return False
elif abs(twist_msg.angular.y) > angular_threshold:
return False
elif abs(twist_msg.angular.z) > angular_threshold:
return False
else:
return True