def process_feedback(feedback):
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.logdebug(s + ": button click" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) +
" clicked" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
rospy.logdebug(s + ": pose changed to " + pose3d2str(feedback.pose))
state_pub.publish(
ModelState(feedback.marker_name, feedback.pose, Twist(),
feedback.header.frame_id))
pose_pub.publish(PoseStamped(feedback.header, feedback.pose))
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.logdebug(s + ": mouse down" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.logdebug(s + ": mouse up" + mp + ".")
server.applyChanges()
def link_states_cb(msg):
try:
index = msg.name.index('thorp::' + sys.argv[1])
print(pose3d2str(msg.pose[index]))
print(msg.twist[index])
except ValueError:
pass
def model_states_cb(self, msg):
if self.killed_cats and not self.alive_cats:
# all cats toppled; we can stop listening for model states
rospy.loginfo("All cats toppled; stop listening for model states")
self.model_states_sub.unregister()
return
# TODO maybe pause/resume myself (so once)? looks like not needed with current throttling
for index, model_name in enumerate(msg.name):
if model_name in self.alive_cats:
if abs(roll(msg.pose[index])) > self.hit_roll_threshold:
# we consider a cat toppled after it rolls by more than hit_roll_threshold
# TODO: maybe better, use body/head contacts with ground_plane
rospy.loginfo("Cat %s toppled at %s! (|roll| > %g)",
model_name, pose3d2str(msg.pose[index]),
self.hit_roll_threshold)
# send toppled cats out of the house and stop tracking them
pose = create_2d_pose(4.0 + len(self.killed_cats), 1.0,
math.pi / 2.0)
self.set_model_state_srv(
ModelState(model_name, pose, Twist(), 'map'))
self.killed_cats.append(model_name)
del self.alive_cats[model_name]
elif rospy.get_time() - self.alive_cats[model_name][
'last_hit'] < self.hit_sock_duration:
# hit, so stop moving; admittedly, not the most realistic for a cat
pass
elif self.prowling_step > 0.0:
# hanging around: perform small steps but changing directions whenever we touch anything
new_pose = copy.deepcopy(msg.pose[index])
contact = self.alive_cats[model_name]['contact']
if contact:
# use inv contact direction to limit the possible orientations to those away from the contact
contact_inv_dir = math.atan2(
contact.contact_normals[0].y,
contact.contact_normals[0].x)
new_yaw_bound1 = norm_angle(contact_inv_dir -
math.pi / 3.0)
new_yaw_bound2 = norm_angle(contact_inv_dir +
math.pi / 3.0)
new_yaw = random.uniform(new_yaw_bound1,
new_yaw_bound2)
new_pose.orientation = quaternion_msg_from_yaw(new_yaw)
rospy.loginfo(
"%s: contact from %f; new direction to %f",
model_name, contact_inv_dir, new_yaw)
self.alive_cats[model_name]['contact'] = None
translate_pose(new_pose, self.prowling_step, 'x')
self.set_model_state_srv(
ModelState(model_name, new_pose, Twist(), 'map'))
elif model_name not in self.killed_cats:
# first time to see this model; keep track of it if listed under target models
# (individual instances are numbered _0, _1 and so by the model spawner)
if model_name in self.target_models or '_'.join(
model_name.split('_')[:-1]) in self.target_models:
self.alive_cats[model_name] = {
'last_hit': -1.0,
'contact': None
}
def model_states_cb(msg):
for index, model_name in enumerate(msg.name):
if model_name in target_models or '_'.join(
model_name.split('_')[:-1]) in target_models:
if server.get(model_name) is None:
make_interactive_marker(msg.pose[index], model_name)
rospy.loginfo("Interactive marker added at %s for model %s",
pose3d2str(msg.pose[index]), model_name)
else:
server.setPose(model_name, msg.pose[index])
server.applyChanges()
state_pub = rospy.Publisher("gazebo/set_model_state", ModelState, queue_size=1)
pose_pub = rospy.Publisher("target_object_pose", PoseStamped, queue_size=1)
target_models = sys.argv[1:]
if not target_models:
rospy.logwarn("No target models provided; nothing to do")
print("Usage: model_markers.py <list of interactive models>")
sys.exit(0)
try:
model_states = rospy.wait_for_message("gazebo/model_states", ModelStates, 10.0)
except rospy.ROSInterruptException:
sys.exit(0)
except rospy.ROSException as err:
rospy.logwarn(err)
rospy.logwarn("Creating markers for models %s on random poses", str(target_models))
model_states = ModelStates()
for model_name in target_models:
model_states.name.append(model_name)
model_states.pose.append(create_2d_pose(randint(1, 10), randint(1, 10), uniform(-pi, pi)))
for index, model_name in enumerate(model_states.name):
if model_name in target_models:
make_interactive_marker(model_states.pose[index], model_name)
rospy.loginfo("Interactive marker added at %s for model %s",
pose3d2str(model_states.pose[index]), model_name)
server.applyChanges()
rospy.spin()