look_for_diffs.py

Go to the documentation of this file.

#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of the Willow Garage nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

# Author: Bhaskara Marthi
#
# Given a database of stored objects, revisit the object locations
# and rescan.  

import roslib; roslib.load_manifest('semanticmodel')
import rospy
import optparse as op
import threading
import logging
import mongo_ros as mr
import actionlib as al
from actionlib_msgs.msg import *
from pr2_controllers_msgs.msg import *
import move_base_msgs.msg as mbm
import sensor_msgs.msg as sm
import semanticmodel.srv as srv
import nav_msgs.msg as nm
from semanticmodel.msg import * 
import geometry_msgs.msg as gm
from std_msgs.msg import Header 
from math import sin, cos
import semanticmodel.db as sdb

log = logging.getLogger('world_model.diffs')


class Node:
    "Holds the node state and ros interaction logic"

    def __init__(self, run):
        "Initialize variables, set up ros comms"
        self.lock = threading.Lock()
        rospy.init_node('look_for_diffs')
        self.run1 = run
        self.db = rospy.get_param('semantic_db_name')
        self.path_pub = rospy.Publisher('plan', nm.Path)
        self.nav_client = al.SimpleActionClient('move_base', 
                                                mbm.MoveBaseAction)
        log.info("Waiting for move_base action server")
        self.nav_client.wait_for_server()
        log.info("Got move_base action server.")

        self.headclient = al.SimpleActionClient(
            '/head_traj_controller/point_head_action', PointHeadAction)
        log.info("Waiting for point_head action server.")
        self.headclient.wait_for_server()
        log.info("Got point_head server.")

    def run(self):
        "Top level"
        # This used to switch to a second db and generate diffs, but no more
        self.navigate_viewpoints()

    def find_blob(self, blobs, image):
      """
      Our image messages don't contain the (x, y, z) coordinate of the blob,
      but we use them to figure out where to rescan from. Given a
      MessageCollection of blobs, and a particular image dictionary, return
      the (x, y, z) coordinate of the corresponding blob (where
      "corresponding" means "same id"), or None if it's all gone wrong.
      """
      for blob in blobs.query({}, True):
        if blob['id'] == image['cluster_id']:
          return (blob['x'], blob['y'], blob['z'])
      return None

    def navigate_viewpoints(self):
        images = mr.MessageCollection(self.db, 
                                      sdb.collection_name(self.run1,
                                                          'images'),
                                      sm.Image)
        blobs = mr.MessageCollection(self.db, 
                                     sdb.collection_name(self.run1, 'blobs'),
                                     BlobMessage)
        seen_clusters = set()
        log.info("The locations are {0}".
            format([(m['x'], 
                     m['y'], 
                     m['cluster_id']) for m in images.query({}, True)]))
        clusters = list(images.query({}, True))
        mb_goals = map(mb_goal, clusters)
        for i, cluster in enumerate(clusters):
            cluster_id = cluster['cluster_id']
            if cluster_id in seen_clusters:
              log.info("Cluster {0} already seen.  Skipping.".format(cluster_id))
              continue
            seen_clusters.add(cluster_id)
            if rospy.is_shutdown():
              break

            # Visualize remaining path
            path = nm.Path()
            path.header.frame_id="/map"
            path.header.stamp = rospy.Time.now()
            path.poses = mb_goals[i:]
            self.path_pub.publish(path)
            log.info("Sending goal ({0[x]}, {0[y]}, {0[theta]} "
                     "for cluster {0[cluster_id]}".format(cluster))

            # Send goal
            res = self.nav_client.send_goal_and_wait(\
                mbm.MoveBaseGoal(mb_goals[i]), rospy.Duration(300.0))
            log.info("Nav result was {0}".format(res))
            XYZ = self.find_blob(blobs, cluster)
            if not XYZ:
              log.fatal("Couldn't find matching id! What the heck?")
              continue



            log.info("Pointing the head at {0}.".format(XYZ))
            g = PointHeadGoal()
            g.target.header.frame_id = "/map"
            g.target.header.stamp = rospy.Time.now()
            g.target.point.x = XYZ[0]
            g.target.point.y = XYZ[1]
            g.target.point.z = XYZ[2]
            g.min_duration = rospy.Duration(1.0)
            self.headclient.send_goal(g)
            # wait_for_result seems to be hanging.
            #self.headclient.wait_for_result()
            rospy.sleep(2.0)
            if self.headclient.get_state() != GoalStatus.SUCCEEDED:
              log.fatal("Head pointing failed! What?  {0}".\
                  format(self.headclient.get_state()))
            log.info("Scanning...")
            rospy.sleep(3.0)
            g2 = PointHeadGoal()
            g.target.header.stamp = rospy.Time.now()
            g2.target.header.frame_id = '/base_link'
            g2.target.point.x = 3.0
            g2.min_duration = rospy.Duration(1.0)
            log.info("Pointing head forward")
            self.headclient.send_goal(g2)
            self.headclient.wait_for_result()
        self.nav_client.send_goal_and_wait(mbm.MoveBaseGoal(mb_goals[0]),
            rospy.Duration(300.0))


############################################################
# Helper functions
############################################################

def mb_goal(m):
    theta = m['theta']
    q = gm.Quaternion(z=sin(theta/2), w=cos(theta/2))
    p = gm.Point(m['x'], m['y'], 0)
    return gm.PoseStamped(pose=gm.Pose(position=p, orientation=q),
                          header=Header(frame_id='/map', 
                                        stamp=rospy.Time.now()))


############################################################
# Arg parsing, startup
############################################################

if __name__ == "__main__":
    # Parse arguments
    parser = op.OptionParser()
    parser.add_option('--run', dest='run', 
                      help="The original run id")
    (options, _) = parser.parse_args(rospy.myargv())

    # Start node
    node = Node(options.run)
    node.run()