class AligningVehicle(smach.State):
def __init__(self,resources):
smach.State.__init__(self, outcomes=['aligned','timed_out'],input_keys=['time_out','e_x','e_y'])
self.resources=resources
def execute(self, ud):
self.resources=Interaction()
#do this job publish data to sensor controller
rospy.loginfo("marker detected .....setting vehicle point to marker calling %s",header.RESET_POSE_SERVICE)
resetClient=rospy.ServiceProxy(header.RESET_POSE_SERVICE, krakenResetPose)
msg=krakenResetPoseRequest()
resp=resetClient(msg)
##help look here --- http://wiki.ros.org/rospy_tutorials/Tutorials/WritingServiceClient
##################--within this it is done
rospy.loginfo("aligning vehicle to set the marker")
self.ipClient=SimpleActionClient(header.MARKER_DETECT_ACTION_SERVER,markerAction)
self.ipClient.cancel_all_goals()
goal=markerGoal()
goal.order=header.ALLIGN_MARKER
self.ipClient.send_goal(goal,done_cb= self.done_cb, feedback_cb= self.feedback_cb)
self.ipClient.wait_for_result(rospy.Duration(ud.time_out))
if self.result:
rospy.loginfo("successfully aligned the vehicle with the marker ")
#again use the service to reset the vehicle's position
msg=krakenResetPoseRequest()
resp=resetClient(msg)
########################
self.ipClient.cancel_goal()
return 'aligned'
else:
self.ipClient.cancel_goal()
rospy.logerr("marker unaligned and timed out!!!")
return 'timed_out'
def feedback_cb(self,feed):
##publish this error data to the sensor and move the vehicle
msg=ipControllererror()
msg.dy=feed.errorx
msg.dx=msg.dy/tan(feed.errorangle)
self.resources.ipControllerPublisher.publish(msg)
##done moving
def done_cb(self,result):
if result.sequence==header.MARKER_ALLIGNED:
self.result=True
else:
self.result=False
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'
def execute(self, ud):
#comment this at last
self.resources=Interaction()
#-------------
ipClient=SimpleActionClient(header.MARKER_DETECT_ACTION_SERVER, markerAction)
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 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 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 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
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 AligningVehicle(smach.State):
def __init__(self, resources):
smach.State.__init__(self,
outcomes=['aligned', 'timed_out'],
input_keys=['time_out', 'e_x', 'e_y'])
self.resources = resources
def execute(self, ud):
self.resources = Interaction()
#do this job publish data to sensor controller
rospy.loginfo(
"marker detected .....setting vehicle point to marker calling %s",
header.RESET_POSE_SERVICE)
resetClient = rospy.ServiceProxy(header.RESET_POSE_SERVICE,
krakenResetPose)
msg = krakenResetPoseRequest()
resp = resetClient(msg)
##help look here --- http://wiki.ros.org/rospy_tutorials/Tutorials/WritingServiceClient
##################--within this it is done
rospy.loginfo("aligning vehicle to set the marker")
self.ipClient = SimpleActionClient(header.MARKER_DETECT_ACTION_SERVER,
markerAction)
self.ipClient.cancel_all_goals()
goal = markerGoal()
goal.order = header.ALLIGN_MARKER
self.ipClient.send_goal(goal,
done_cb=self.done_cb,
feedback_cb=self.feedback_cb)
self.ipClient.wait_for_result(rospy.Duration(ud.time_out))
if self.result:
rospy.loginfo("successfully aligned the vehicle with the marker ")
#again use the service to reset the vehicle's position
msg = krakenResetPoseRequest()
resp = resetClient(msg)
########################
self.ipClient.cancel_goal()
return 'aligned'
else:
self.ipClient.cancel_goal()
rospy.logerr("marker unaligned and timed out!!!")
return 'timed_out'
def feedback_cb(self, feed):
##publish this error data to the sensor and move the vehicle
msg = ipControllererror()
msg.dy = feed.errorx
msg.dx = msg.dy / tan(feed.errorangle)
self.resources.ipControllerPublisher.publish(msg)
##done moving
def done_cb(self, result):
if result.sequence == header.MARKER_ALLIGNED:
self.result = True
else:
self.result = False
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 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)
