def execute(self, userdata):
        sss.move("gripper", "open", blocking=False)
       # sss.move("arm", "zeroposition")
        
        for object in userdata.object_list:         
            
            # ToDo: need to be adjusted to correct stuff           
            if object.pose.pose.position.z <= 0.0 or object.pose.pose.position.z >= 0.10:
                continue
    
            sss.move("arm", "zeroposition", mode="planned")                             

            #object.pose.pose.position.z = object.pose.pose.position.z + 0.02
            object.pose.pose.position.x = object.pose.pose.position.x + 0.01
            object.pose.pose.position.y = object.pose.pose.position.y - 0.005

            handle_arm = sss.move("arm", [object.pose.pose.position.x, object.pose.pose.position.y, object.pose.pose.position.z, "/base_link"], mode="planned")

            if handle_arm.get_state() == 3:
                sss.move("gripper", "close", blocking=False)
                rospy.sleep(3.0)
                sss.move("arm", "zeroposition", mode="planned")        
                return 'succeeded'    
            else:
                rospy.logerr('could not find IK for current object')

        return 'failed'
Esempio n. 2
0
    def __init__(self):
        NaoNode.__init__(self)
        rospy.init_node('nao_camera')
        self.camProxy = self.getProxy("ALVideoDevice")
        if self.camProxy is None:
            exit(1)

        # ROS pub/sub
        self.pub_img_ = rospy.Publisher('image_raw', Image)
        self.pub_info_ = rospy.Publisher('camera_info', CameraInfo)
        self.set_camera_info_service_ = rospy.Service('set_camera_info', SetCameraInfo, self.set_camera_info)
        # Messages
        self.info_ = CameraInfo()
        self.set_default_params_qvga(self.info_) #default params should be overwritten by service call
        # parameters
        self.camera_switch = rospy.get_param('~camera_switch', 0)
        if self.camera_switch == 0:
            self.frame_id = "/CameraTop_frame"
        elif self.camera_switch == 1:
            self.frame_id = "/CameraBottom_frame"
        else:
            rospy.logerr('Invalid camera_switch. Must be 0 or 1')
            exit(1)
        print "Using namespace ", rospy.get_namespace()
        print "using camera: ", self.camera_switch
        #TODO: parameters
        self.resolution = kQVGA
        self.colorSpace = kBGRColorSpace
        self.fps = 30
        # init
        self.nameId = ''
        self.subscribe()
Esempio n. 3
0
    def servo_to_joints_cb(self,req):
        if self.driver_status == 'SERVO':

            # Check acceleration and velocity limits
            (acceleration, velocity) = self.check_req_speed_params(req)

            stamp = self.acquire()

            # inverse kinematics
            traj = self.planner.getJointMove(req.target.position,
                self.q0,
                self.base_steps,
                self.steps_per_meter,
                self.steps_per_radians,
                time_multiplier = (1./velocity),
                percent_acc = acceleration,
                use_joint_move = True,
                table_frame = self.table_pose)

            # Send command
            if len(traj.points) > 0:
                if stamp is not None:
                    rospy.logwarn("Robot moving to " + str(traj.points[-1].positions))
                    res = self.send_trajectory(traj,stamp,acceleration,velocity,cartesian=False)
                    self.release()
                else:
                    res = 'FAILURE -- could not preempt current arm control.'
                return res
            else:
                rospy.logerr('SIMPLE DRIVER -- IK failed')
                return 'FAILURE -- no trajectory points'
        else:
            rospy.logerr('SIMPLE DRIVER -- Not in servo mode')
            return 'FAILURE -- not in servo mode'
    def solve(self, ptarget, target_rot): #ptarget should be a Point(); target_rot 'FRONT' or 'DOWN'
        # self.solution_found = False
        ikreq = SolvePositionIKRequest() #service request object
        hdr = Header(
            stamp=rospy.Time.now(), frame_id='base')
        if target_rot == 'FRONT':
            pose = PoseStamped(
                header=hdr,
                pose=Pose(
                    position=ptarget,
                    orientation=self.qtarget[0]
                ),
            )
        elif target_rot == 'DOWN':
            pose = PoseStamped(
                header=hdr,
                pose=Pose(
                    position=ptarget,
                    orientation=self.qtarget[1]
                ),
            )

        ikreq.pose_stamp.append(pose)

        try:
            resp = self.iksvc(ikreq)           
        except (rospy.ServiceException, rospy.ROSException), e:
            rospy.logerr("Service call failed: %s" % (e,))
            return False
    def loadMap(self, pointset):

        #pointset=str(sys.argv[1])
        host = rospy.get_param("datacentre_host")
        port = rospy.get_param("datacentre_port")
        client = pymongo.MongoClient(host, port)
        db=client.autonomous_patrolling
        points_db=db["waypoints"]
        available = points_db.find().distinct("meta.pointset")
        
        #print available
        if pointset not in available :
            rospy.logerr("Desired pointset '"+pointset+"' not in datacentre")
            rospy.logerr("Available pointsets: "+str(available))
            raise Exception("Can't find waypoints.")      

        #points = self._get_points(waypoints_name) 
        points = []
        search =  {"meta.pointset": pointset}
        for point in points_db.find(search) :
            a= point["meta"]["name"]
            b = topological_node(a)
            b.edges = point["meta"]["edges"]
            b.waypoint = point["meta"]["waypoint"]
            b._get_coords()
            b._insert_vertices(point["meta"]["vertices"])
            points.append(b)
        return points
Esempio n. 6
0
def handle_start_srv(req):
    try:
        p=Popen(['roslaunch', 'jsk_pepper_startup', 'play_audio_stream.launch']) 
        return EmptyResponse()
    except RuntimeError, e:
        rospy.logerr("Exception caught:\n%s", e)
        return res
Esempio n. 7
0
    def smart_move_cb(self,req):
        '''
        Find any valid object that meets the requirements.
        Find a cartesian path or possibly longer path through joint space.
        '''

        if not self.driver_status == 'SERVO':
            msg = 'FAILURE -- not in servo mode'
            rospy.logerr('DRIVER -- Not in servo mode!')
            return msg

        # Check acceleration and velocity limits
        (acceleration, velocity) = self.check_req_speed_params(req)
        possible_goals = self.query(req)
        stamp = self.acquire()

        if len(possible_goals) == 0:
            return 'FAILURE -- no valid poses found. see costar_arm.py'

        for (dist,T,obj,name) in possible_goals:
            rospy.logwarn("Trying to move to frame at distance %f"%(dist))

            # plan to T
            if not self.valid_verify(stamp):
                msg = 'FAILURE - Stopping smart move action because robot has been preempted by another process. see costar_arm.py'
                rospy.logwarn(msg)
                return msg
            (code,res) = self.planner.getPlan(T,self.q0,obj=obj)
            msg = self.send_and_publish_planning_result(res,stamp,acceleration,velocity)

            if msg[0:7] == 'SUCCESS':
                break

        return msg
Esempio n. 8
0
def main():
    rospy.init_node("find_fiducial_pose_test")

    # Construct action ac
    rospy.loginfo("Starting action client...")
    action_client = actionlib.SimpleActionClient("find_fiducial_pose", FindFiducialAction)
    action_client.wait_for_server()
    rospy.loginfo("Action client connected to action server.")

    # Call the action
    rospy.loginfo("Calling the action server...")
    action_goal = FindFiducialGoal()
    action_goal.camera_name = "/camera/rgb"
    action_goal.pattern_width = 7
    action_goal.pattern_height = 6
    action_goal.pattern_size = 0.027
    action_goal.pattern_type = 1  # CHESSBOARD

    if (
        action_client.send_goal_and_wait(action_goal, rospy.Duration(50.0), rospy.Duration(50.0))
        == GoalStatus.SUCCEEDED
    ):
        rospy.loginfo("Call to action server succeeded")
    else:
        rospy.logerr("Call to action server failed")
Esempio n. 9
0
def serWrite(myStr):
    try:
        for i in range(len(myStr)):
            ser.write(myStr[i])
            rospy.sleep(0.0001)   # Sometimes, a delay seems to help. Maybe?
    except:
        rospy.logerr("[GS] Unable to send data. Check connection.")
 def execute(self, userdata):
     rospy.loginfo(OKGREEN+"i'm looking what tags are"+ENDC)
     rospy.loginfo(OKGREEN+str(userdata.asr_userSaid)+ENDC)
     #rospy.loginfo(OKGREEN+"TAGS: "+str(self.tags)+ENDC)
     #rospy.loginfo(OKGREEN+str(userdata.asr_userSaid_tags)+ENDC)
     #userdata.object=userdata.asr_userSaid # it means that in this place it have a coke
     
     if userdata.asr_userSaid=="finish" :
         rospy.logwarn("-------------------------------------i'm have a finish order")
         return 'finish'
     
     listTags = userdata.asr_userSaid_tags
      
     #Process tags
     userdata.object_array = []
     locationValue = [tag for tag in listTags if tag.key == 'direction']
     objectValue = [tag for tag in listTags if tag.key == 'class']
  
     if objectValue and locationValue:
         userdata.objectOrientation = locationValue[0].value
         userdata.objectName = objectValue[0].value
         return 'new_position'  
     else:
         rospy.logerr("Object or Location not set")
         return 'aborted'  
Esempio n. 11
0
 def run_spawn_hook(hook):
     try:
         hook()
     except:
         rospy.logerr("Caught an Exception while running a spawn hook!")
         # Log the traceback.
         rospy.logerr(sys.exc_info()[2])
    def verify_boards(self, check):
        if self.has_finished:
            return False

        try:
            rospy.wait_for_service('mcb_conf_results', 15)
        except:
            msg = "MCB conf results service not available. Unable to configure MCB's"
            rospy.logerr(msg)
            self.finished(False, msg)
            return False

        if not self.count_boards():
            return False

        if not self.check_link():
            return False

        if not self.get_serials():
            return False
        
        if check:
            return self.check_boards()

        return True
Esempio n. 13
0
def main():
    try:
        opts, args = getopt.getopt(sys.argv[1:], 'hc:',
            ['help', 'calibrate=',])
    except getopt.GetoptError as err:
        print str(err)
        usage()
        sys.exit(2)

    arm = None
    for o, a in opts:
        if o in ('-h', '--help'):
            usage()
            sys.exit(0)
        elif o in ('-c', '--calibrate'):
            arm = a

    if not arm:
        usage()
        rospy.logerr("No arm specified")
        sys.exit(1)

    rospy.init_node('calibrate_arm_sdk', anonymous=True)
    rs = baxter_interface.RobotEnable()
    rs.enable()
    cat = CalibrateArm(arm)
    rospy.loginfo("Running calibrate on %s arm" % (arm,))

    error = None
    try:
        cat.run()
    except Exception, e:
        error = str(e)
Esempio n. 14
0
def talker():
    rospy.init_node('random_motors', anonymous=False)
    r = rospy.Rate(0.5) # 10hz
    joints = [rospy.get_param("~pan_name", 'pan_joint'), rospy.get_param("~tilt_name", 'tilt_joint')]
    torque_enable = dict()
    servo_position = dict()

    #Configure joints
    for controller in sorted(joints):
        try:
            rospy.loginfo("controller is "+controller)

            # Torque enable/disable control for each servo
            torque_service = '/' + controller + '/torque_enable'
            rospy.wait_for_service(torque_service)
            torque_enable[controller] = rospy.ServiceProxy(torque_service, TorqueEnable)

            # Start each servo in the disabled state so we can move them by hand
            torque_enable[controller](False)

            # The position controllers
            servo_position[controller] = rospy.Publisher('/' + controller + '/command', Float64)
        except:
            rospy.logerr("Can't contact servo services!")

    while not rospy.is_shutdown():
        for controller in sorted(joints):
            rand_val = uniform(-2,2)
            #Send random float to each joint
            servo_position[controller].publish(rand_val)
        r.sleep()
    """
Esempio n. 15
0
    def __init__(self):

        node_name = rospy.get_name()
        #print "node_name: " + str(node_name)

        rate = rospy.get_param("~rate", 14.429)
        self.history_size = rospy.get_param("~history_size", 5)
        self.thresh = rospy.get_param("~thresh", 0.05)

        try:
            self.input_topic = rospy.get_param("~input_topic")
        except:
            err = 'Please specify an input topic'
            rospy.logerr('Please specify an input topic')
            raise Exception(err)

        try:
            self.output_topic = rospy.get_param("~output_topic")
        except:
            err = 'Please specify an output topic'
            rospy.logerr(err)
            raise Exception(err)

        self.tf = tf.TransformListener()
        self.br = tf.TransformBroadcaster()
        self.rate = rospy.Rate(rate)
        self.history = RingBuffer(self.history_size)

        self.parent = self.get_parent(self.input_topic)
    def __init__(self, port=None, baud=57600, timeout=5.0):
        """ Initialize node, connect to bus, attempt to negotiate topics. """
        self.mutex = thread.allocate_lock()

        self.lastsync = rospy.Time(0)
        self.lastsync_lost = rospy.Time(0)
        self.timeout = timeout
        self.synced = False

        self.pub_diagnostics = rospy.Publisher('/diagnostics', diagnostic_msgs.msg.DiagnosticArray)

        if port== None:
            # no port specified, listen for any new port?
            pass
        elif hasattr(port, 'read'):
            #assume its a filelike object
            self.port=port
        else:
            # open a specific port
            try:
                self.port = Serial(port, baud, timeout=self.timeout*0.5)
            except SerialException as e:
                rospy.logerr("Error opening serial: %s", e)
                rospy.signal_shutdown("Error opening serial: %s" % e)
                raise SystemExit

        self.port.timeout = 0.01  # Edit the port timeout

        time.sleep(0.1)           # Wait for ready (patch for Uno)

        # hydro introduces protocol ver2 which must match node_handle.h
        # The protocol version is sent as the 2nd sync byte emitted by each end
        self.protocol_ver1 = '\xff'
        self.protocol_ver2 = '\xfe'
        self.protocol_ver = self.protocol_ver2

        self.publishers = dict()  # id:Publishers
        self.subscribers = dict() # topic:Subscriber
        self.services = dict()    # topic:Service

        self.buffer_out = -1
        self.buffer_in = -1

        self.callbacks = dict()
        # endpoints for creating new pubs/subs
        self.callbacks[TopicInfo.ID_PUBLISHER] = self.setupPublisher
        self.callbacks[TopicInfo.ID_SUBSCRIBER] = self.setupSubscriber
        # service client/servers have 2 creation endpoints (a publisher and a subscriber)
        self.callbacks[TopicInfo.ID_SERVICE_SERVER+TopicInfo.ID_PUBLISHER] = self.setupServiceServerPublisher
        self.callbacks[TopicInfo.ID_SERVICE_SERVER+TopicInfo.ID_SUBSCRIBER] = self.setupServiceServerSubscriber
        self.callbacks[TopicInfo.ID_SERVICE_CLIENT+TopicInfo.ID_PUBLISHER] = self.setupServiceClientPublisher
        self.callbacks[TopicInfo.ID_SERVICE_CLIENT+TopicInfo.ID_SUBSCRIBER] = self.setupServiceClientSubscriber
        # custom endpoints
        self.callbacks[TopicInfo.ID_PARAMETER_REQUEST] = self.handleParameterRequest
        self.callbacks[TopicInfo.ID_LOG] = self.handleLoggingRequest
        self.callbacks[TopicInfo.ID_TIME] = self.handleTimeRequest

        rospy.sleep(2.0) # TODO
        self.requestTopics()
        self.lastsync = rospy.Time.now()
Esempio n. 17
0
    def free_capability(self, capability_interface, timeout=None):
        """Free's a previously used capability.

        Calls the ``~free_capability`` service, which effectively decrements
        the internal reference count for that capability in the remote
        capability server.

        If that results in a reference count of zero,
        then the capability server will shutdown that capability automatically.

        :param capability_interface: Name of the capability interface to free up
        :type capability_interface: str
        :param timeout: time to wait on service to be available (optional)
        :type timeout: float
        :returns: :py:obj:`True` if successful, otherwise :py:obj:`False`
        :rtype: :py:obj:`bool`
        :raises: CapabilityNotInUseException if the capability has not been previously used
        :raises: CapabilityNotRunningException if the capability has already been stopped
            which can be caused by a capability it depends on stopping
        :raises: ServiceNotAvailableException if the required service is not available
            after the timeout has expired
        """
        if capability_interface not in self._used_capabilities:
            rospy.logerr("Cannot free capability interface '{0}', because it was not first used."
                         .format(capability_interface))
            CapabilityNotInUseException(capability_interface)
        if self.__wait_for_service(self.__free_capability, timeout) is False:
            raise ServiceNotAvailableException(self.__free_capability.resolved_name)
        try:
            self.__free_capability.call(capability_interface, self._bond_id)
        except rospy.ServiceException as exc:
            exc_str = "{0}".format(exc)
            if 'Cannot free Capability' in exc_str and 'because it is not running' in exc_str:
                raise CapabilityNotRunningException(capability_interface)
        return True
Esempio n. 18
0
def graspit_grasp_pose_to_moveit_grasp_pose(move_group_commander, listener, graspit_grasp_msg,
                                            grasp_frame='/approach_tran'):
    """
    :param move_group_commander: A move_group command from which to get the end effector link.
    :type move_group_commander: moveit_commander.MoveGroupCommander
    :param listener: A transformer for looking up the transformation
    :type tf.TransformListener
    :param graspit_grasp_msg: A graspit grasp message
    :type graspit_grasp_msg: graspit_msgs.msg.Grasp

    """

    try:
        listener.waitForTransform(grasp_frame, move_group_commander.get_end_effector_link(),
                                     rospy.Time(0), timeout=rospy.Duration(1))
        at_to_ee_tran, at_to_ee_rot = listener.lookupTransform(grasp_frame, move_group_commander.get_end_effector_link(),rospy.Time())
    except:
        rospy.logerr("graspit_grasp_pose_to_moveit_grasp_pose::\n " +
                    "Failed to find transform from %s to %s"%(grasp_frame, move_group_commander.get_end_effector_link()))
        ipdb.set_trace()



    graspit_grasp_msg_final_grasp_tran_matrix = tf_conversions.toMatrix(tf_conversions.fromMsg(graspit_grasp_msg.final_grasp_pose))
    approach_tran_to_end_effector_tran_matrix = tf.TransformerROS().fromTranslationRotation(at_to_ee_tran, at_to_ee_rot)
    if move_group_commander.get_end_effector_link() == 'l_wrist_roll_link':
        rospy.logerr('This is a PR2\'s left arm so we have to rotate things.')
        pr2_is_weird_mat = tf.TransformerROS().fromTranslationRotation([0,0,0], tf.transformations.quaternion_from_euler(0,math.pi/2,0))
    else:
        pr2_is_weird_mat = tf.TransformerROS().fromTranslationRotation([0,0,0], [0,0,0])
    actual_ee_pose_matrix = np.dot( graspit_grasp_msg_final_grasp_tran_matrix, approach_tran_to_end_effector_tran_matrix)
    actual_ee_pose_matrix = np.dot(actual_ee_pose_matrix, pr2_is_weird_mat)
    actual_ee_pose = tf_conversions.toMsg(tf_conversions.fromMatrix(actual_ee_pose_matrix))
    rospy.loginfo("actual_ee_pose: " + str(actual_ee_pose))
    return actual_ee_pose
 def _load_interface(self):
     interface_file = resolve_url(rospy.get_param('~interface_url', ''))
     if interface_file:
         rospy.loginfo("interface_url: %s", interface_file)
     try:
         data = read_interface(interface_file) if interface_file else {}
         # set the ignore hosts list
         self._re_ignore_hosts = create_pattern('ignore_hosts', data, interface_file, [])
         # set the sync hosts list
         self._re_sync_hosts = create_pattern('sync_hosts', data, interface_file, [])
         self.__sync_topics_on_demand = False
         if interface_file:
             if 'sync_topics_on_demand' in data:
                 self.__sync_topics_on_demand = data['sync_topics_on_demand']
         elif rospy.has_param('~sync_topics_on_demand'):
             self.__sync_topics_on_demand = rospy.get_param('~sync_topics_on_demand')
         rospy.loginfo("sync_topics_on_demand: %s", self.__sync_topics_on_demand)
         self.__resync_on_reconnect = rospy.get_param('~resync_on_reconnect', True)
         rospy.loginfo("resync_on_reconnect: %s", self.__resync_on_reconnect)
         self.__resync_on_reconnect_timeout = rospy.get_param('~resync_on_reconnect_timeout', 0)
         rospy.loginfo("resync_on_reconnect_timeout: %s", self.__resync_on_reconnect_timeout)
     except:
         import traceback
         # kill the ros node, to notify the user about the error
         rospy.logerr("Error on load interface: %s", traceback.format_exc())
         import os
         import signal
         os.kill(os.getpid(), signal.SIGKILL)
Esempio n. 20
0
 def getPointDist(self, pt):
     
     assert(self.face is not None)
     
     # fist, get my position
     p = PoseStamped()
     
     p.header.frame_id = "base_link"
     p.header.stamp = rospy.Time.now() - rospy.Duration(0.5)
     
     p.pose.position.x = 0
     p.pose.position.y = 0
     p.pose.position.z = 0
     p.pose.orientation.x = 0
     p.pose.orientation.y = 0
     p.pose.orientation.z = 0
     p.pose.orientation.w = 1
     
     try:
         
         self._tf.waitForTransform(p.header.frame_id, self._robot_frame, p.header.stamp, rospy.Duration(2))
         p = self._tf.transformPose(self._robot_frame, p)
         
     except:
         
         rospy.logerr("TF error!")
         return None
     
     return sqrt(pow(p.pose.position.x - pt.point.x, 2) + pow(p.pose.position.y - pt.point.y, 2) + pow(p.pose.position.z - pt.point.z, 2))
Esempio n. 21
0
 def getPoseStamped(self, group, c):
     
     assert(len(c) == 6)
     
     p = PoseStamped()
     
     p.header.frame_id = "base_link"
     p.header.stamp = rospy.Time.now() - rospy.Duration(0.5)
     
     p.pose.position.x = c[0]
     p.pose.position.y = c[1]
     p.pose.position.z = c[2]
     
     quat = tf.transformations.quaternion_from_euler(c[3], c[4], c[5])
     
     p.pose.orientation.x = quat[0]
     p.pose.orientation.y = quat[1]
     p.pose.orientation.z = quat[2]
     p.pose.orientation.w = quat[3]
     
     try:
         
         self._tf.waitForTransform(p.header.frame_id, group.get_pose_reference_frame(), p.header.stamp, rospy.Duration(2))
         p = self._tf.transformPose(group.get_pose_reference_frame(), p)
         
     except:
         
         rospy.logerr("TF error!")
         return None
     
     return p
Esempio n. 22
0
 def scan_callback(self, msg):
     angle = msg.angle_min
     d_angle = msg.angle_increment
     sum_x = 0
     sum_y = 0
     sum_xx = 0
     sum_xy = 0
     num = 0
     for r in msg.ranges:
         if angle > self.min_angle and angle < self.max_angle and r < msg.range_max:
             x = math.sin(angle) * r
             y = math.cos(angle) * r
             sum_x += x
             sum_y += y
             sum_xx += x*x
             sum_xy += x*y
             num += 1
         angle += d_angle
     if num > 0:
         denominator = num*sum_xx-sum_x*sum_x
         if denominator != 0:
             angle=math.atan2((-sum_x*sum_y+num*sum_xy)/(denominator), 1)
             #print("Scan Angle: %s"%str(angle))
             relay = ScanAngle()
             relay.header = msg.header 
             relay.scan_angle = angle
             with self.lock:
                 if self._laser_scan_angle_publisher:
                     self._laser_scan_angle_publisher.publish(relay)
     else:
         rospy.logerr("Please point me at a wall.")
Esempio n. 23
0
def main():
    rospy.init_node("sonarFilter", anonymous=False)
    filterChain = SonarFilter()
    try:
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.logerr("sonarFilter interrupted")
Esempio n. 24
0
 def __update_graph(self):
     # collect all of the waiting capabilities
     waiting = [x
                for x in self.__capability_instances.values()
                if x.state == 'waiting']
     # If any of the waiting have no blocking dependencies start them
     for instance in waiting:
         blocking_dependencies = []
         for dependency_name in instance.depends_on:
             if dependency_name not in self.__capability_instances:  # pragma: no cover
                 rospy.logerr(
                     "Inconsistent capability run graph, '{0}' depends on "
                     .format(instance.name) + "'{0}', ".format(dependency_name) +
                     "which is not in the list of capability instances.")
                 return
             dependency = self.__capability_instances[dependency_name]
             if dependency.state != 'running':
                 blocking_dependencies.append(dependency)
         if not blocking_dependencies:
             instance.launch()
             self.__launch_manager.run_capability_provider(
                 instance.provider, instance.provider_path
             )
     # Remove any terminated capabilities
     self.__remove_terminated_capabilities()
Esempio n. 25
0
 def rosImgToCVMono(self, rosImg):
     try:
         cvImg = self.bridge.imgmsg_to_cv2(rosImg, desired_encoding="mono8")
         cvImg = cv2.cvtColor(cvImg, cv2.COLOR_GRAY2BGR)
     except CvBridgeError as e:
         rospy.logerr(e)
     return cvImg
    def parse_usbllong(self, tokens):
        """
        method which will generate a ros message from USBL data
        :param tokens: list of message fields
        """
        if len(tokens) != 17:
            rospy.logerr("%s: USBLLONG message fields count doesn't match" % self.name)
            return
        usblmsg = AcousticModemUSBLLONG()
        usblmsg.header.stamp = rospy.Time.now()

        usblmsg.measurement_time = float(tokens[2])
        usblmsg.remote_address = int(tokens[3])
        usblmsg.X = float(tokens[4])
        usblmsg.Y = float(tokens[5])
        usblmsg.Z = float(tokens[6])
        usblmsg.E = float(tokens[7])
        usblmsg.N = float(tokens[8])
        usblmsg.U = float(tokens[9])
        usblmsg.roll = float(tokens[10])    # RPY of local device
        usblmsg.pitch = float(tokens[11])
        usblmsg.yaw = float(tokens[12])
        usblmsg.propagation_time = float(tokens[13])
        usblmsg.accuracy = float(tokens[-1])

        rospy.loginfo('%s: Received USBLLONG data' %(self.name))
        self.received_cnt += 1
        self.pub_usbllong.publish(usblmsg)
    def parse_usblangles(self, tokens):
        """
        method which will generate a ros message from USBL data
        :param tokens: list of message fields
        """
        if len(tokens) != 14:
            rospy.logerr("%s: USBLANGLES message fields count doesn't match" % self.name)
            return
        usblmsg = AcousticModemUSBLANGLES()
        usblmsg.header.stamp = rospy.Time.now()

        usblmsg.measurement_time = float(tokens[2])
        usblmsg.remote_address = int(tokens[3])
        usblmsg.lbearing = float(tokens[4])
        usblmsg.lelevation = float(tokens[5])
        usblmsg.bearing = float(tokens[6])
        usblmsg.elevation = float(tokens[7])
        usblmsg.roll = float(tokens[8])    # RPY of local device
        usblmsg.pitch = float(tokens[9])
        usblmsg.yaw = float(tokens[10])
        usblmsg.accuracy = float(tokens[13])

        rospy.loginfo('%s: Received USBLANGLES data' %(self.name))
        self.received_cnt += 1
        self.pub_usblangles.publish(usblmsg)
Esempio n. 28
0
    def start_recognizer(self):
        rospy.loginfo("Starting recognizer... ")

        self.pipeline = gst.parse_launch(self.launch_config)
        self.asr = self.pipeline.get_by_name('asr')
        self.asr.connect('partial_result', self.asr_partial_result)
        self.asr.connect('result', self.asr_result)
        self.asr.set_property('configured', True)
        self.asr.set_property('dsratio', 1)


        # Configure language model
        if rospy.has_param(self._lm_param):
            lm = rospy.get_param(self._lm_param)
        else:
            rospy.logerr('Recognizer not started. Please specify a language model file.')
            return

        if rospy.has_param(self._dic_param):
            dic = rospy.get_param(self._dic_param)
        else:
            rospy.logerr('Recognizer not started. Please specify a dictionary.')
            return

        self.asr.set_property('lm', lm)
        self.asr.set_property('dict', dic)
        self.bus = self.pipeline.get_bus()
        self.bus.add_signal_watch()
        self.bus_id = self.bus.connect('message::application', self.application_message)
        self.pipeline.set_state(gst.STATE_PLAYING)
        self.started = True
Esempio n. 29
0
def get_param_duration(param):
    """Calls rospy.get_param and logs errors.

    Logs if the param does not exist or is not parsable to rospy.Durotation.
    And calls rospy.signal_shutdown if the value is invalid/not existing.

    :return:    The Param param from the parameter server.
    :rtype: rospy.Duration

    """

    # dummy value
    value = rospy.Duration(1)

    try:
        # only a default value in case the param gets fuzzed.
        value = rospy.Duration(get_param_num(param))
    except ValueError:
        err_msg = (
            "Param %s has the invalid value '%s'."
            % (param, rospy.get_param(param)))
        rospy.logerr(err_msg)
        rospy.signal_shutdown(err_msg)
        value = rospy.Duration(1)
    return value
Esempio n. 30
0
 def stretchingAction(self):
     
     self.say("I'm bit tired. Let's do some stretching.")
     
     self._torso_action_cl.wait_for_server()
     
     goal = pr2_controllers_msgs.msg.SingleJointPositionGoal()
     
     goal.position = 0.195
     goal.max_velocity = 0.5
     
     try:
     
       self._torso_action_cl.send_goal(goal)
       self._torso_action_cl.wait_for_result(rospy.Duration.from_sec(10.0))
       
     except:
         
         rospy.logerr("torso action error (up)")
     
     # TODO move arms
     
     self.say("I feel much better now!")
     
     goal.position = 0.0
     
     try:
     
       self._torso_action_cl.send_goal(goal)
       self._torso_action_cl.wait_for_result(rospy.Duration.from_sec(10.0))
       
     except:
         
         rospy.logerr("torso action error (down)")
Esempio n. 31
0
def change_motor_config_file(hw_version, can_id_list, dxl_id_list):
    rospack = rospkg.RosPack()
    folder_path = rospack.get_path('niryo_one_bringup')
    file_path = folder_path + '/config/v' + str(int(hw_version)) + '/niryo_one_motors.yaml'
    lines = []
    can_bus_enabled = len(can_id_list) > 0
    dxl_bus_enabled = len(dxl_id_list) > 0
    rospy.loginfo("Change motor config file (for debug purposes): " + str(file_path))
    rospy.loginfo("Hw version: " + str(hw_version))
    rospy.loginfo("CAN ID list: " + str(can_id_list))
    rospy.loginfo("DXL ID list: " + str(dxl_id_list))

    if hw_version != 1 and hw_version != 2:
        return CHANGE_MOTOR_CONFIG_WRONG_VERSION

    # Open file, get text and split lines
    try:
        with open(file_path, 'r') as f:
            lines = f.read().splitlines()
    except EnvironmentError as e:
        rospy.logerr(e)
        return CHANGE_MOTOR_CONFIG_READ_FAIL
  
    text_begin_lines = [] 
    text_can_lines = []
    text_dxl_lines = []
    text_end_lines = []

    # Get beginning of file (not modified)
    for line in lines:
        if not line.startswith('#'):
            break
        text_begin_lines.append(line)

    # Get end of file (not modified)
    for i, line in enumerate(lines):
        if line.startswith('dxl_authorized_motors'):
            text_end_lines = lines[i:]
            break

    # Fill CAN motor IDs depending on given ID array
    # If CAN bus is already disabled, then don't disable motors here
    if hw_version == 1:
        text_can_lines.append('can_required_motors: # Axis 1-4 of Niryo One (stepper 1 -> id 1, stepper 2 -> id 2, ...)')
    else:
        text_can_lines.append('can_required_motors: # Axis 1-3 of Niryo One (stepper 1 -> id 1, stepper 2 -> id 2, ...)')
    text_can_lines.append('    # Edit only for debug purposes (ex : you want to test some motors separately and detached from the robot)')
    text_can_lines.append('    # --> Commented ids will make associated motor disable (and thus not trigger an error if not connected)')
    text_can_lines.append('    # Before editing, please be sure that you know what you\'re doing')
    if 1 in can_id_list or not can_bus_enabled:
        text_can_lines.append('    - 1 # Axis 1 enabled if not commented')
    else:
        text_can_lines.append('    #- 1 # Axis 1 enabled if not commented')
    if 2 in can_id_list or not can_bus_enabled:
        text_can_lines.append('    - 2 # Axis 2 enabled if not commented')
    else:
        text_can_lines.append('    #- 2 # Axis 2 enabled if not commented')
    if 3 in can_id_list or not can_bus_enabled:
        text_can_lines.append('    - 3 # Axis 3 enabled if not commented')
    else:
        text_can_lines.append('    #- 3 # Axis 3 enabled if not commented')
    if hw_version == 1:
        if 4 in can_id_list or not can_bus_enabled:
            text_can_lines.append('    - 4 # Axis 4 enabled if not commented')
        else:
            text_can_lines.append('    #- 4 # Axis 4 enabled if not commented')

    # Fill DXL motor IDs depending on given ID array
    # If DXL bus is already disabled, then don't disable motors here
    if hw_version == 1:
        text_dxl_lines.append('dxl_required_motors: # axis 5 and 6 of Niryo One.')
    else:
        text_dxl_lines.append('dxl_required_motors: # axis 4, 5 and 6 of Niryo One.')
    text_dxl_lines.append('    # Edit only for debug purposes (ex : you want to test some motors separately and detached from the robot)')
    text_dxl_lines.append('    # --> Commented ids will make associated motor disable (and thus not trigger an error if not connected)')
    text_dxl_lines.append('    # Before editing, please be sure that you know what you\'re doing')
    if hw_version == 1:
        text_dxl_lines.append('    # - Note : Axis 5 has 2 motors, but you can use only one motor for this axis when debugging')
    if hw_version == 1:
        if 4 in dxl_id_list or not dxl_bus_enabled:
            text_dxl_lines.append('    - 4 # -> id of Axis 5_1')
        else:
            text_dxl_lines.append('    #- 4 # -> id of Axis 5_1')
        if 5 in dxl_id_list or not dxl_bus_enabled:
            text_dxl_lines.append('    - 5 # -> id of Axis 5_2')
        else:
            text_dxl_lines.append('    #- 5 # -> id of Axis 5_2')
        if 6 in dxl_id_list or not dxl_bus_enabled:
            text_dxl_lines.append('    - 6 # -> id of Axis 6')
        else:
            text_dxl_lines.append('    #- 6 # -> id of Axis 6')
    else:
        if 2 in dxl_id_list or not dxl_bus_enabled:
            text_dxl_lines.append('    - 2 # -> id of Axis 4')
        else:
            text_dxl_lines.append('    #- 2 # -> id of Axis 4')
        if 3 in dxl_id_list or not dxl_bus_enabled:
            text_dxl_lines.append('    - 3 # -> id of Axis 5')
        else:
            text_dxl_lines.append('    #- 3 # -> id of Axis 5')
        if 6 in dxl_id_list or not dxl_bus_enabled:
            text_dxl_lines.append('    - 6 # -> id of Axis 6')
        else:
            text_dxl_lines.append('    #- 6 # -> id of Axis 6')

    new_text = '\n'.join(text_begin_lines + [''] + text_can_lines + [''] + text_dxl_lines + [''] + text_end_lines + [''])

    # Rewrite file with new text
    try:
        with open(file_path, 'w') as f:
            f.write(new_text)
    except EnvironmentError as e:
        rospy.logerr(e)
        return CHANGE_MOTOR_CONFIG_WRITE_FAIL
    return CHANGE_MOTOR_CONFIG_SUCCESS
Esempio n. 32
0
        rospy.Subscriber("/clicked_point", PointStamped, self.insert_marker_callback)
        rospy.Subscriber("/clicked_pose", PoseStamped, self.insert_terminal_marker_callback)
        rospy.on_shutdown(self.clear_all_markers)
        # rospy.logwarn("The waypoint server is waiting for RViz to run and to be subscribed to {0}.".format(rospy.resolve_name("~edges")))
        # while self.edge_line_publisher.get_num_connections() == 0:
        #     rospy.sleep(1.0)

        self.clear_all_markers()

        self.waypoint_file = rospy.get_param("~waypoint_file", "") # yaml file with waypoints to load

        if len(self.waypoint_file) != 0:
            rospy.loginfo("Waypoint_Server is loading initial waypoint file {0}.".format(self.waypoint_file))
            error_message = self.load_waypoints_from_file(self.waypoint_file)
            if error_message:
                rospy.logerr(error_message)

    def get_waypoint_data_service(self, req):

        response = GetWaypointDataResponse()

        response.waypoint_found = True

<<<<<<< HEAD
    def get_valid_locations(self):

        valid_locations = {}

        for uuid, waypoint_data in self.waypoint_graph.nodes(data=True):
            if waypoint_data['waypoint_type'] == WAYPOINT_TERMINATING:
                name = self.uuid_name_map[uuid]
Esempio n. 33
0
        pt.pose.orientation.x = q[0]
        pt.pose.orientation.y = q[1]
        pt.pose.orientation.z = q[2]
        pt.pose.orientation.w = q[3]
        moveit_test.create_plan_to_target("left_arm", pt)

        q = (kdl.Rotation.RPY(1.57,0,-1.57)).GetQuaternion()
        pt = geometry_msgs.msg.PoseStamped()
        pt.header.frame_id = "world"
        pt.header.seq = 0
        pt.header.stamp = rospy.Time.now()
        pt.pose.position.x = 0.3
        pt.pose.position.y = -0.5
        pt.pose.position.z = 1.2
        pt.pose.orientation.x = q[0]
        pt.pose.orientation.y = q[1]
        pt.pose.orientation.z = q[2]
        pt.pose.orientation.w = q[3]
        moveit_test.create_plan_to_target("right_arm", pt)

        r1 = moveit_test.execute_plan("left_arm")
        r2 = moveit_test.execute_plan("right_arm")
        if not r1 : rospy.logerr("moveit_test(left_arm) -- couldn't execute plan")
        if not r2 : rospy.logerr("moveit_test(right_arm) -- couldn't execute plan")

        moveit_commander.roscpp_shutdown()

    except rospy.ROSInterruptException:
        pass

Esempio n. 34
0
        if(stop_line_idx != stop_forward_idx):
            #likely car is away from stop line still ?
            if(self.debug):
                rospy.loginfo('traffic light upcoming but there\s no stop line position found')
            return -1, TrafficLight.UNKNOWN

        # 3. Find the car waypoint closest to the stop line
        stop_waypoint_idx = self.get_closest_waypoint( 
                self.stop_line_positions_poses[stop_line_idx].pose.pose, 
                self.waypoints.waypoints )  

        if( stop_waypoint_idx == None ):
            rospy.loginfo('Couldnt find waypoint in process_traffic_lights()')
            return -1, TrafficLight.UNKNOWN

        use_detector_flag = True
   
        if use_detector_flag:
            state = self.get_light_state( self.lights[light_idx] )
        else:
            state = self.lights[light_idx].state  #this is only valid within simulator

        rospy.loginfo(self.tlclasses_d[ state ] )
        return stop_waypoint_idx, state
if __name__ == '__main__':
    try:
        TLDetector()
    except rospy.ROSInterruptException:
        rospy.logerr('Could not start traffic node.')
Esempio n. 35
0
    global found_button
    # Button pressed position gathered from Gazebo analysis
    buttonPos = Vector3(3.35, -0.73, 1.25)
    offset = Vector3(center.point.x - buttonPos.x,
                     center.point.y - buttonPos.y,
                     center.point.z - buttonPos.z)
    zarj_eyes.remove_detection_request(name)
    zarj_tf.set_world_transform(offset)
    found_button = True

if __name__ == '__main__':
    try:
        rospy.init_node('team_zarj_qual2')

        if not rospy.has_param('/ihmc_ros/robot_name'):
            rospy.logerr("Cannot run team_zarj_qual2.py, missing parameters!")
            rospy.logerr("Missing parameter '/ihmc_ros/robot_name'")

        else:
            robot_name = rospy.get_param('/ihmc_ros/robot_name')

            right_foot_frame_parameter_name = "/ihmc_ros/{0}/right_foot_frame_name".format(robot_name)
            left_foot_frame_parameter_name = "/ihmc_ros/{0}/left_foot_frame_name".format(robot_name)

            if rospy.has_param(right_foot_frame_parameter_name) and rospy.has_param(left_foot_frame_parameter_name):
                control = "/ihmc_ros/{0}/control/".format(robot_name)
                output = "/ihmc_ros/{0}/output/".format(robot_name)

                BodyPublisher = rospy.Publisher(control + "whole_body_trajectory", WholeBodyTrajectoryRosMessage, queue_size=10)
                footStepListPublisher = rospy.Publisher(control + "footstep_list", FootstepDataListRosMessage, queue_size=10)
                handTrajectoryPublisher = rospy.Publisher(control + "hand_trajectory", HandTrajectoryRosMessage, queue_size=10)
Esempio n. 36
0
 def CamRgbImageCallback(self, rgb_image_data):
     try:
         rgb_image = self.cv_bridge.imgmsg_to_cv2(rgb_image_data, 'passthrough')
     except CvBridgeError, e:
         rospy.logerr(e)
Esempio n. 37
0
 def CamDepthImageCallback(self, depth_image_data):
     try:
         self.m_depth_image = self.cv_bridge.imgmsg_to_cv2(depth_image_data, 'passthrough')
     except CvBridgeError, e:
         rospy.logerr(e)
Esempio n. 38
0
    def pick(self,pickup_goal):
        #prepare result
        pickresult = PickupResult()

        #get grasps for the object
        # fill up a grasp planning request
        grasp_planning_req = GraspPlanningRequest()
        grasp_planning_req.arm_name = pickup_goal.arm_name
        grasp_planning_req.target = pickup_goal.target
        object_to_attach = pickup_goal.collision_object_name
        # call grasp planning service
        grasp_planning_res = self.grasp_planning_service_.call(grasp_planning_req)
        #print grasp_planning_res
        # process grasp planning result
        if (grasp_planning_res.error_code.value != grasp_planning_res.error_code.SUCCESS):
            rospy.logerr("No grasp found for this object, we will generate some, but only when the node is ready for that !")
            pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
            return pickresult
        else:
            rospy.loginfo("Got "+ str(len(grasp_planning_res.grasps)) +" grasps for this object")

        # for each grasp, generate rotational symmetric grasps around the object (this is already in the DB for the CokeCan but should be removed and done online)

        #for each grasp, check path from pre-grasp pose to grasp pose first and then check motion to pre-grasp pose
        motion_plan_res=GetMotionPlanResponse()
        grasp_to_execute_=Grasp()
        for index, grasp in enumerate(grasp_planning_res.grasps):
            # extract grasp_pose
            grasp_pose_ = PoseStamped()
            grasp_pose_.header.frame_id = "/world";
            grasp_pose_.pose = grasp.grasp_pose

            grasp_pose_.pose.position.y=grasp_pose_.pose.position.y-0.01 #-0.01 in sim, +0.03 in real #cheating here

            # copy the grasp_pose as a pre-grasp_pose
            pre_grasp_pose_ = copy.deepcopy(grasp_pose_)

            # add desired_approach_distance along the approach vector. above the object to plan pre-grasp pose

            # currently add this to Z because approach vector needs to be computed somehow first (TODO)
            pre_grasp_pose_.pose.position.z = pre_grasp_pose_.pose.position.z + 0.05

            # for distance from 0 (grasp_pose) to desired_approach distance (pre_grasp_pose) test IK/Collision and save result
            # decompose this in X steps depending on distance to do and max speed
            interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(pre_grasp_pose_, grasp_pose_, False)

            # check the result (depending on number of steps etc...)
            if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS):
                number_of_interpolated_steps=0
                # check if one approach trajectory is feasible
                for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes):
                    if traj_error_code.val!=1:
                        rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val))
                        break
                    else:
                        number_of_interpolated_steps=interpolation_index

                # if trajectory is feasible then plan reach motion to pre-grasp pose
                if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points):
                    rospy.loginfo("Grasp number "+str(index)+" approach is possible, checking motion plan to pre-grasp")
                    #print interpolated_motion_plan_res

                    # check and plan motion to this pre_grasp_pose
                    motion_plan_res = self.plan.plan_arm_motion( pickup_goal.arm_name, "jointspace", pre_grasp_pose_ )

                    #if this pre-grasp pose is successful do not test others
                    if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
                        rospy.loginfo("Grasp number "+str(index)+" is possible, executing it")
                        # copy the grasp to execute for the following steps
                        grasp_to_execute_ = copy.deepcopy(grasp)
                        break
                else:
                    rospy.logerr("Grasp number "+str(index)+" approach is impossible")
                    #print interpolated_motion_plan_res
            else:
                rospy.logerr("Grasp number "+str(index)+" approach is impossible")
                #print interpolated_motion_plan_res
        # execution part
        if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
            #put hand in pre-grasp posture
            if self.pre_grasp_exec(grasp_to_execute_)<0:
                #QMessageBox.warning(self, "Warning",
                #    "Pre-grasp action failed: ")
                pickresult.manipulation_result.value = ManipulationResult.FAILED
                return pickresult

            #go there
            # filter the trajectory
            filtered_traj = self.filter_traj_(motion_plan_res)

            self.display_traj_( filtered_traj )

            # reach pregrasp pose
            if self.send_traj_( filtered_traj )<0:
                #QMessageBox.warning(self, "Warning",
                #    "Reach trajectory execution failed: ")
                pickresult.manipulation_result.value = ManipulationResult.FAILED
                return pickresult
            #time.sleep(20) # TODO use actionlib here
            time.sleep(self.simdelay) # TODO use actionlib here

            # approach
            if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0:
                #QMessageBox.warning(self, "Warning",
                #    "Approach trajectory execution failed: ")
                pickresult.manipulation_result.value = ManipulationResult.FAILED
                return pickresult
            time.sleep(self.simdelay) # TODO use actionlib here

            #grasp
            if self.grasp_exec(grasp_to_execute_)<0:
                #QMessageBox.warning(self, "Warning",
                #    "Grasp action failed: ")
                pickresult.manipulation_result.value = ManipulationResult.FAILED
                return pickresult
            time.sleep(self.simdelay) # TODO use actionlib here

            #attach the collision object to the hand (should be cleaned-up)
            rospy.loginfo("Now we attach the object")

            att_object = AttachedCollisionObject()
            att_object.link_name = "palm"
            att_object.object.id = object_to_attach
            att_object.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
            att_object.object.header.frame_id = "palm"
            att_object.object.header.stamp = rospy.Time.now()
            object = Shape()
            object.type = Shape.CYLINDER
            object.dimensions.append(.03)
            object.dimensions.append(0.1)
            pose = Pose()
            pose.position.x = 0.0
            pose.position.y = -0.06
            pose.position.z = 0.06
            pose.orientation.x = 0
            pose.orientation.y = 0
            pose.orientation.z = 0
            pose.orientation.w = 1
            att_object.object.shapes.append(object)
            att_object.object.poses.append(pose);
            att_object.touch_links= ["ffdistal","mfdistal","rfdistal","lfdistal","thdistal","ffmiddle","mfmiddle","rfmiddle","lfmiddle","thmiddle","ffproximal","mfproximal","rfproximal","lfproximal","thproximal","palm","lfmetacarpal","thbase"]
            self.att_object_in_map_pub_.publish(att_object)
            rospy.loginfo("Attach object published")
        else:
            rospy.logerr("None of the grasps tested is possible")
            pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
            return pickresult
        pickresult.manipulation_result.value = ManipulationResult.SUCCESS
        pickresult.grasp= grasp_to_execute_
        return pickresult
Esempio n. 39
0
    def __init__(self):
        self.odom_freq = float(rospy.get_param('~odom_freq',
                                               '50'))  # hz of odom pub
        self.odom_topic = rospy.get_param('~odom_topic',
                                          '/odom2base_footprint')  # topic name
        self.baseId = rospy.get_param('~base_id',
                                      'base_footprint')  # base link
        self.odomId = rospy.get_param('~odom_id', 'odom')  # odom link
        self.VxCov = float(rospy.get_param(
            '~vx_cov', '1.0'))  # covariance for Vx measurement
        self.VyawCov = float(rospy.get_param(
            '~vyaw_cov', '1.0'))  # covariance for Vyaw measurement
        self.pub_tf = bool(rospy.get_param(
            '~pub_tf', True))  # whether publishes TF or not
        self.wheelRad = float(0.072 / 2.0)  #m
        self.wheelSep = float(0.17)  #m
        self.scale = rospy.get_param('~scale', '1.0')

        try:
            self.serial = serial.Serial('/dev/due_controller',
                                        115200,
                                        timeout=0.5)
            rospy.loginfo("Connect success ...")

            try:
                print("Flusing first 50 data readings ...")
                for x in range(0, 50):
                    data = self.serial.read()
                    time.sleep(0.01)
            except:
                print("Flusing faile ")
                sys.exit(0)

        except serial.serialutil.SerialException:
            rospy.logerr(
                "Can not receive data from the port: "  #+ self.device_port + 
                ". Did you specify the correct port ?")
            self.serial.close
            sys.exit(0)
        rospy.loginfo("Communication success !")

        # rospy.loginfo("Flusing first 50 data readings ...")
        #     for x in range(0, 50):
        #         self.serial.readline().strip()
        #         time.sleep(0.01)

        # ROS handler
        self.sub = rospy.Subscriber('/car/cmd_vel',
                                    Twist,
                                    self.cmdCB,
                                    queue_size=10)
        self.pub = rospy.Publisher(self.odom_topic, Odometry, queue_size=10)

        self.timer_odom = rospy.Timer(rospy.Duration(0.1), self.timerOdomCB)
        self.timer_cmd = rospy.Timer(rospy.Duration(0.1),
                                     self.timerCmdCB)  # 10Hz
        self.tf_broadcaster = tf.TransformBroadcaster()

        # variable
        self.trans_x = 0.0  # cmd
        self.rotat_z = 0.0
        self.WL_send = 0.0
        self.WR_send = 0.0
        self.current_time = rospy.Time.now()
        self.previous_time = rospy.Time.now()
        self.pose_x = 0.0  # SI
        self.pose_y = 0.0
        self.pose_yaw = 0.0
    def traffic_cb(self, msg):
        self.stopline_wp_idx = msg.data

    def obstacle_cb(self, msg):
        # TODO: Callback for /obstacle_waypoint message. We will implement it later
        pass

    def get_waypoint_velocity(self, waypoint):
        return waypoint.twist.twist.linear.x

    def set_waypoint_velocity(self, waypoints, waypoint, velocity):
        waypoints[waypoint].twist.twist.linear.x = velocity

    def distance(self, waypoints, wp1, wp2):
        dist = 0
        dl = lambda a, b: math.sqrt((a.x - b.x)**2 + (a.y - b.y)**2 +
                                    (a.z - b.z)**2)
        for i in range(wp1, wp2 + 1):
            dist += dl(waypoints[wp1].pose.pose.position,
                       waypoints[i].pose.pose.position)
            wp1 = i
        return dist


if __name__ == '__main__':
    try:
        WaypointUpdater()
    except rospy.ROSInterruptException:
        rospy.logerr('Could not start waypoint updater node.')
    def __init__(self):
        rospy.init_node('arduino', log_level=rospy.INFO)

        # Get the actual node name in case it is set in the launch file
        self.name = rospy.get_name()

        # Cleanup when termniating the node
        rospy.on_shutdown(self.shutdown)

        self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.5)
        self.base_frame = rospy.get_param("~base_frame", 'base_link')
        self.motors_reversed = rospy.get_param("~motors_reversed", False)
        # Overall loop rate: should be faster than fastest sensor rate
        self.rate = int(rospy.get_param("~rate", 50))
        r = rospy.Rate(self.rate)

        # Rate at which summary SensorState message is published. Individual sensors publish
        # at their own rates.
        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))

        self.use_base_controller = rospy.get_param("~use_base_controller",
                                                   False)

        # Set up the time for publishing the next SensorState message
        now = rospy.Time.now()
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
        self.t_next_sensors = now + self.t_delta_sensors

        # Initialize a Twist message
        self.cmd_vel = Twist()

        # A cmd_vel publisher so we can stop the robot when shutting down
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)

        # The SensorState publisher periodically publishes the values of all sensors on
        # a single topic.
        self.sensorStatePub = rospy.Publisher('~sensor_state',
                                              SensorState,
                                              queue_size=5)

        # A service to position a PWM servo
        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)

        # A service to read the position of a PWM servo
        rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)

        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
        rospy.Service('~digital_set_direction', DigitalSetDirection,
                      self.DigitalSetDirectionHandler)

        # A service to turn a digital sensor on or off
        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)

        # A service to read the value of a digital sensor
        rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler)

        # A service to set pwm values for the pins
        rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)

        # A service to read the value of an analog sensor
        rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)

        # Initialize the controlller
        self.controller = Arduino(self.port, self.baud, self.timeout,
                                  self.motors_reversed)

        # Make the connection
        self.controller.connect()

        rospy.loginfo("Connected to Arduino on port " + self.port + " at " +
                      str(self.baud) + " baud")

        # Reserve a thread lock
        mutex = _thread.allocate_lock()

        # Initialize any sensors
        self.mySensors = list()

        sensor_params = rospy.get_param("~sensors", dict({}))

        for name, params in sensor_params.items():
            # Set the direction to input if not specified
            try:
                params['direction']
            except:
                params['direction'] = 'input'

            if params['type'] == "Ping":
                sensor = Ping(self.controller, name, params['pin'],
                              params['rate'], self.base_frame)
            elif params['type'] == "GP2D12":
                sensor = GP2D12(self.controller, name, params['pin'],
                                params['rate'], self.base_frame)
            elif params['type'] == 'Digital':
                sensor = DigitalSensor(self.controller,
                                       name,
                                       params['pin'],
                                       params['rate'],
                                       self.base_frame,
                                       direction=params['direction'])
            elif params['type'] == 'Analog':
                sensor = AnalogSensor(self.controller,
                                      name,
                                      params['pin'],
                                      params['rate'],
                                      self.base_frame,
                                      direction=params['direction'])
            elif params['type'] == 'PololuMotorCurrent':
                sensor = PololuMotorCurrent(self.controller, name,
                                            params['pin'], params['rate'],
                                            self.base_frame)
            elif params['type'] == 'PhidgetsVoltage':
                sensor = PhidgetsVoltage(self.controller, name, params['pin'],
                                         params['rate'], self.base_frame)
            elif params['type'] == 'PhidgetsCurrent':
                sensor = PhidgetsCurrent(self.controller, name, params['pin'],
                                         params['rate'], self.base_frame)


#                if params['type'] == "MaxEZ1":
#                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
#                    self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
            try:
                self.mySensors.append(sensor)
                rospy.loginfo(name + " " + str(params) +
                              " published on topic " + rospy.get_name() +
                              "/sensor/" + name)
            except:
                rospy.logerr("Sensor type " + str(params['type']) +
                             " not recognized.")

        # Initialize the base controller if used
        if self.use_base_controller:
            self.myBaseController = BaseController(
                self.controller, self.base_frame,
                self.name + "_base_controller")

        # Start polling the sensors and base controller
        while not rospy.is_shutdown():
            for sensor in self.mySensors:
                mutex.acquire()
                sensor.poll()
                mutex.release()

            if self.use_base_controller:
                mutex.acquire()
                self.myBaseController.poll()
                mutex.release()

            # Publish all sensor values on a single topic for convenience
            now = rospy.Time.now()

            if now > self.t_next_sensors:
                msg = SensorState()
                msg.header.frame_id = self.base_frame
                msg.header.stamp = now
                for i in range(len(self.mySensors)):
                    msg.name.append(self.mySensors[i].name)
                    msg.value.append(self.mySensors[i].value)
                try:
                    self.sensorStatePub.publish(msg)
                except:
                    pass

                self.t_next_sensors = now + self.t_delta_sensors

            r.sleep()
Esempio n. 42
0
    def place(self,place_goal):

        placeresult = PlaceResult()
        target_pose_to_execute_ = PoseStamped()
        #for location, check path from approach pose to release pose first and then check motion to approach pose
        motion_plan_res=GetMotionPlanResponse()
        object_to_attach = place_goal.collision_object_name
        # get current hand pose
        self.listener.waitForTransform('/world', '/palm', rospy.Time(), rospy.Duration(1.0))
        try:
            (trans,rot) = self.listener.lookupTransform('/world', '/palm', rospy.Time())
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            rospy.logerr("Cannot get current palm pose")
            placeresult.manipulation_result.value = ManipulationResult.ERROR
            return placeresult

        current_pose_= PoseStamped()
        current_pose_.header.frame_id = "/world"
        current_pose_.pose.position = Point(trans[0],trans[1],trans[2])
        current_pose_.pose.orientation = Quaternion(rot[0],rot[1],rot[2],rot[3])

        # for each place location
        for index, target_pose_ in enumerate(place_goal.place_locations):

            #compute straight trajectory to approach distance
            target_approach_pose_= PoseStamped()
            target_approach_pose_.header.frame_id = "/world"
            target_approach_pose_.pose.position = Point(target_pose_.pose.position.x,target_pose_.pose.position.y,target_pose_.pose.position.z+place_goal.approach.desired_distance)
            target_approach_pose_.pose.orientation = Quaternion(target_pose_.pose.orientation.x,target_pose_.pose.orientation.y,target_pose_.pose.orientation.z,target_pose_.pose.orientation.w) #keep same orientation for the first test

            # for distance from 0 (release_pose) to desired_approach distance (approach_pose) test IK/Collision and save result
            # decompose this in X steps depending on distance to do and max speed
            interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(target_approach_pose_, target_pose_, False)

            # check the result (depending on number of steps etc...)
            if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS):
                number_of_interpolated_steps=0
                # check if one approach trajectory is feasible
                for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes):
                    if traj_error_code.val!=1:
                        rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val))
                        break
                    else:
                        number_of_interpolated_steps=interpolation_index

                # if trajectory is feasible then plan reach motion to approach pose
                if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points):
                    rospy.loginfo("Place pose number "+str(index)+" approach is possible, checking motion plan to approach pose")
                    #print interpolated_motion_plan_res

                    # check and plan motion to this approach pose
                    motion_plan_res = self.plan.plan_arm_motion( place_goal.arm_name, "jointspace" ,target_approach_pose_)#,object_to_attach)

                    #if this approach pose is successful do not test others
                    if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
                        rospy.loginfo("Place pose number "+str(index)+" is possible, executing it")
                        # copy the pose to execute for the following steps
                        target_pose_to_execute_ = copy.deepcopy(target_pose_)
                        break
                else:
                    rospy.logerr("Place pose number "+str(index)+" approach is impossible")
                    #print interpolated_motion_plan_res
            else:
                rospy.logerr("Place pose number "+str(index)+" approach is impossible")
                #print interpolated_motion_plan_res

        # execution part
        if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
            #go there
            # filter the trajectory
            filtered_traj = self.filter_traj_(motion_plan_res)

            self.display_traj_( filtered_traj )

            # reach approach pose
            if self.send_traj_( filtered_traj )<0:
                #QMessageBox.warning(self, "Warning",
                #    "Reach trajectory execution failed: ")
                placeresult.manipulation_result.value = ManipulationResult.FAILED
                return placeresult
            time.sleep(self.simdelay) # TODO use actionlib here

            # approach
            if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0:
                #QMessageBox.warning(self, "Warning",
                #    "Approach trajectory execution failed: ")
                placeresult.manipulation_result.value = ManipulationResult.FAILED
                return placeresult
            time.sleep(self.simdelay) # TODO use actionlib here

            #put hand in pre-grasp posture (to gently release)
            if self.pre_grasp_exec(place_goal.grasp)<0:
                #QMessageBox.warning(self, "Warning",
                #    "Release action failed: ")
                placeresult.manipulation_result.value = ManipulationResult.FAILED
                return placeresult
            time.sleep(self.simdelay) # TODO use actionlib here
            #detach the object from the hand
            rospy.loginfo("Now we detach the attached object")
            att_object = AttachedCollisionObject()
            att_object.link_name = "palm"
            att_object.object.id = object_to_attach
            att_object.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
            att_object.object.header.frame_id = "palm"
            att_object.object.header.stamp = rospy.Time.now()
            object = Shape()
            object.type = Shape.CYLINDER
            object.dimensions.append(.03)
            object.dimensions.append(0.1)
            pose = Pose()
            pose.position.x = 0.0
            pose.position.y = -0.06
            pose.position.z = 0.06
            pose.orientation.x = 0
            pose.orientation.y = 0
            pose.orientation.z = 0
            pose.orientation.w = 1
            att_object.object.shapes.append(object)
            att_object.object.poses.append(pose);
            self.att_object_in_map_pub_.publish(att_object)
            rospy.loginfo("Attached object to be detached published")


        else:
            rospy.logerr("None of the place pose tested is possible")
            placeresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
            return placeresult
        placeresult.manipulation_result.value = ManipulationResult.SUCCESS
        placeresult.place_location= target_pose_to_execute_
        return placeresult
Esempio n. 43
0
    def poll(self):
        self.send_debug_pid()

        time_now = rospy.Time.now()
        if time_now > self.t_next:
            #rospy.logwarn("Voltage: %f, Current: %f", float(vol), float(current))
            #vol = self.arduino.detect_voltage()
            #current = self.arduino.detect_current()
            #rospy.logwarn("Voltage: %f, Current: %f", vol, current)
            # Read the encoders
            try:
                aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.get_encoder_counts(
                )
            except:
                self.bad_encoder_count += 1
                rospy.logerr("Encoder exception count: " +
                             str(self.bad_encoder_count))
                return
            #rospy.loginfo("Encoder A:"+str(aWheel_enc)+",B:"+str(bWheel_enc)+",C:" + str(cWheel_enc))

            # Calculate odometry
            dist_A = (aWheel_enc - self.enc_A) / self.ticks_per_meter
            dist_B = (bWheel_enc - self.enc_B) / self.ticks_per_meter
            dist_C = (cWheel_enc - self.enc_C) / self.ticks_per_meter

            # save current encoder value for next calculate
            self.enc_A = aWheel_enc
            self.enc_B = bWheel_enc
            self.enc_C = cWheel_enc

            dt = time_now - self.then
            self.then = time_now
            dt = dt.to_sec()

            va = dist_A / dt
            vb = dist_B / dt
            vc = dist_C / dt

            #forward kinemation
            vx = sqrt(3) * (va - vb) / 3.0
            vy = (va + vb - 2 * vc) / 3.0
            vth = (va + vb + vc) / (3 * self.wheel_track)

            delta_x = (vx * cos(self.th) - vy * sin(self.th)) * dt
            delta_y = (vx * sin(self.th) + vy * cos(self.th)) * dt
            delta_th = vth * dt

            self.x += delta_x
            self.y += delta_y
            self.th += delta_th

            quaternion = Quaternion()
            quaternion.x = 0.0
            quaternion.y = 0.0
            quaternion.z = sin(self.th / 2.0)
            quaternion.w = cos(self.th / 2.0)

            # create the odometry transform frame broadcaster.
            #self.odomBroadcaster.sendTransform(
            #    (self.x, self.y, 0),
            #    (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            #    rospy.Time.now(),
            #    self.base_frame,
            #    self.odom_name
            #)

            odom = Odometry()
            odom.header.frame_id = self.odom_name
            odom.child_frame_id = self.base_frame
            odom.header.stamp = time_now
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.pose.covariance = [
                1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0, 0, 0, 1e6, 0, 0,
                0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e-9
            ]
            odom.twist.twist.linear.x = vx
            odom.twist.twist.linear.y = vy
            odom.twist.twist.angular.z = vth
            odom.twist.covariance = [
                1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0, 0, 0, 1e6, 0, 0,
                0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e-9
            ]
            self.odomPub.publish(odom)

            # send motor cmd
            if time_now > (self.last_cmd_time + rospy.Duration(self.timeout)):
                self.stop()
            else:
                self.send_motor_speed()

            # set next compare time
            self.t_next = time_now + self.t_delta
Esempio n. 44
0
            #self.battery_monitor_status_msg.dc_voltage = float('nan')
            #self.battery_monitor_status_msg.load = float('nan')
            #self.battery_monitor_status_msg.percentage = float('nan')
            #self.battery_monitor_status_msg.temperature = float('nan')
            # self.battery_monitor_status_msg.on = False # as we cannot process the response, we can assume that it is on an error state, so it cannot be considered to be on
        '''

    def writeToSerialDevice(self, data):
        bytes_written = 0
        try:
            bytes_written = self.serial_device.write(data)
        except serial.serialutil.SerialException, e:
            rospy.logerr('%s:writeToSerialDevice: Error writing port %s'%(self.node_name, e))
            return -1
        except Exception, e:
            rospy.logerr('%s:writeToSerialDevice: Error writing port %s'%(self.node_name, e))
            return -1

        return bytes_written

    def readFromSerialDevice(self, size):
        data_read = ''
        try:
            data_read = self.serial_device.read(size)
        except serial.serialutil.SerialException, e:
            rospy.logerr('%s:readFromSerialDevice: Error reading port %s'%(self.node_name, e))
            return -1,''
        except Exception, e:
            rospy.logerr('%s:readFromSerialDevice: Error reading port %s'%(self.node_name, e))
            return -1,''
            rospy.logwarn("# episode cumulated_reward=>" + str(cumulated_reward))
            rospy.logwarn("# State in which we will start next step=>" + str(nextState))
            qlearn.learn(state, action, reward, nextState)

            if not (done):
                rospy.logwarn("NOT DONE")
                state = nextState
            else:
                rospy.logwarn("DONE")
                last_time_steps = numpy.append(last_time_steps, [int(i + 1)])
                break
            rospy.logwarn("############### END Step=>" + str(i))
            #raw_input("Next Step...PRESS KEY")
            # rospy.sleep(2.0)
        m, s = divmod(int(time.time() - start_time), 60)
        h, m = divmod(m, 60)
        rospy.logerr(("EP: " + str(x + 1) + " - [alpha: " + str(round(qlearn.alpha, 2)) + " - gamma: " + str(
            round(qlearn.gamma, 2)) + " - epsilon: " + str(round(qlearn.epsilon, 2)) + "] - Reward: " + str(
            cumulated_reward) + "     Time: %d:%02d:%02d" % (h, m, s)))

    rospy.loginfo(("\n|" + str(nepisodes) + "|" + str(qlearn.alpha) + "|" + str(qlearn.gamma) + "|" + str(
        initial_epsilon) + "*" + str(epsilon_discount) + "|" + str(highest_reward) + "| PICTURE |"))

    l = last_time_steps.tolist()
    l.sort()

    # print("Parameters: a="+str)
    rospy.loginfo("Overall score: {:0.2f}".format(last_time_steps.mean()))
    rospy.loginfo("Best 100 score: {:0.2f}".format(reduce(lambda x, y: x + y, l[-100:]) / len(l[-100:])))

    env.close()
    def AnalogReadHandler(self, req):
        value = self.controller.analog_read(req.pin)
        return AnalogReadResponse(value)

    def shutdown(self):
        rospy.loginfo("Shutting down Arduino Node...")

        # Stop the robot
        try:
            rospy.loginfo("Stopping the robot...")
            self.cmd_vel_pub.Publish(Twist())
            rospy.sleep(2)
        except:
            pass

        # Close the serial port
        try:
            self.controller.close()
        except:
            pass
        finally:
            rospy.loginfo("Serial port closed.")
            os._exit(0)

if __name__ == '__main__':
    try:
        myArduino = ArduinoROS()
    except SerialException:
        rospy.logerr("Serial exception trying to open port.")
        os._exit(0)
 def messageCallback(self, msg, topic):
     try:
         self.publishers[self.remap(topic)].publish(msg)
     except Exception as exc:
         rospy.logerr('Error on publishing to {}: {}'.format(topic, exc))
Esempio n. 48
0
        self.activate_all_output()
        config = deepcopy(self.component_config_)
        data = deepcopy(self.component_data_)
        self.set_all_input_read()
        self.component_implementation_.update(data, config)

        {ifpublisher}
        try:
            {forallpublisher}
            self.component_data_.out_{name}_active = data.out_{name}_active
            self.component_data_.out_{name} = data.out_{name}
            if self.component_data_.out_{name}_active:
                self.{name}_.publish(self.component_data_.out_{name})
            {endforallpublisher}
        except rospy.ROSException as error:
            rospy.logerr("Exception: {}".format(error))
        {endifpublisher}


def main():
    """
    @brief Entry point of the package.
    Instanciate the node interface containing the Developer implementation
    @return nothing
    """
    rospy.init_node("{nodeName}", anonymous=False)

    node = {apply-capitalized_node_name}ROS()
    if not node.configure():
        rospy.logfatal("Could not configure the node")
        rospy.logfatal("Please check configuration parameters")
Esempio n. 49
0
def convert_ref_frame(arm_state, ref_frame, ref_frame_obj=Landmark()):
    """Transforms an arm frame to a new ref. frame.

    Args:
        arm_state (ArmState): The arm state to transform
        ref_frame (int): One of ArmState.*, the desired reference frame to
            transform into.
        ref_frame_obj (Landmark): The landmark to transform relative to.

    Returns:
        ArmState: A copy of arm_state, but transformed.
    """
    output_state = copy.deepcopy(arm_state)
    ref_frame_obj_copy = copy.deepcopy(ref_frame_obj)
    if ref_frame == ArmState.ROBOT_BASE:
        if arm_state.refFrame == ArmState.ROBOT_BASE:
            pass  # Nothing to do
        elif arm_state.refFrame == ArmState.OBJECT:
            # Transform from object to robot base.
            ee_in_obj = get_matrix_from_pose(arm_state.ee_pose)
            obj_pose = arm_state.refFrameLandmark.pose  # In base frame
            obj_to_base = get_matrix_from_pose(obj_pose)
            abs_ee_pose = get_pose_from_transform(
                np.dot(obj_to_base, ee_in_obj))
            output_state.ee_pose = abs_ee_pose
            output_state.refFrame = ArmState.ROBOT_BASE
            output_state.refFrameLandmark = Landmark()
        else:
            rospy.logerr(
                'Unhandled reference frame conversion: {} to {}'.format(
                    arm_state.refFrame, ref_frame))
    elif ref_frame == ArmState.OBJECT:
        if arm_state.refFrame == ArmState.ROBOT_BASE:
            # Transform from robot base to provided object.
            arm_in_base = get_matrix_from_pose(arm_state.ee_pose)
            base_to_obj = np.linalg.inv(
                get_matrix_from_pose(ref_frame_obj.pose))
            rel_ee_pose = get_pose_from_transform(
                np.dot(base_to_obj, arm_in_base))
            output_state.ee_pose = rel_ee_pose
            output_state.refFrame = ArmState.OBJECT
            output_state.refFrameLandmark = ref_frame_obj_copy
        elif arm_state.refFrame == ArmState.OBJECT:
            if arm_state.refFrameLandmark.name == ref_frame_obj.name:
                pass  # Nothing to do
            else:
                # Transform from arm state's object to provided object.
                ee_in_source_obj = get_matrix_from_pose(arm_state.ee_pose)
                source_obj_to_base = get_matrix_from_pose(
                    arm_state.refFrameLandmark.pose)
                base_to_target_obj = np.linalg.inv(
                    get_matrix_from_pose(ref_frame_obj.pose))
                rel_ee_pose = get_pose_from_transform(
                    np.dot(np.dot(base_to_target_obj, source_obj_to_base),
                           ee_in_source_obj))
                output_state.ee_pose = rel_ee_pose
                output_state.refFrame = ArmState.OBJECT
                output_state.refFrameLandmark = ref_frame_obj_copy
        else:
            rospy.logerr(
                'Unhandled reference frame conversion: {} to {}'.format(
                    arm_state.refFrame, ref_frame))
    return output_state
            goal_lifting.trajectory.joint_names.append('joint_lifting')
            temp_joint_lifting=JointTrajectoryPoint()
            temp_joint_lifting.positions=[goal_joint_position[0]]
            temp_joint_lifting.accelerations=[0.0]
            temp_joint_lifting.velocities=[0.0]
            temp_joint_lifting.effort=[0.0]
            temp_joint_lifting.time_from_start=rospy.Duration(secs=2.0)
            goal_lifting.trajectory.points.append(temp_joint_lifting)
            goal_lifting.trajectory.header.stamp=rospy.Time.now()+rospy.Duration(secs=1)
            client_lifting.send_goal(goal_lifting)
            flag = 2
            #client_lifting.wait_for_result(rospy.Duration.from_sec(2.0))


    def get_key(self):
        tty.setraw(sys.stdin.fileno())
        rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
        if rlist:
            key = sys.stdin.read(1)
        else:
            key = ''
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.setting)
        return key


if __name__ == '__main__':
    try:
        KeyboardTeleopRvizArm()
    except rospy.ROSInterruptException:
        rospy.logerr("ROS Error!")
Esempio n. 51
0
    # Note that TopicDiagnostic, HeaderlessDiagnosedPublisher,
    # HeaderlessDiagnosedPublisher and DiagnosedPublisher all descend from
    # CompositeDiagnosticTask, so you can add your own fields to them using
    # the addTask method.
    #
    # Each time pub1_freq is updated, lower will also get updated and its
    # output will be merged with the output from pub1_freq.
    pub1_freq.addTask(lower) # (This wouldn't work if lower was stateful).

    # If we know that the state of the node just changed, we can force an
    # immediate update.
    updater.force_update()

    # We can remove a task by refering to its name.
    if not updater.removeByName("Bound check"):
        rospy.logerr("The Bound check task was not found when trying to remove it.")

    while not rospy.is_shutdown():
        msg = std_msgs.msg.Bool()
        rospy.sleep(0.1)

        # Calls to pub1 have to be accompanied by calls to pub1_freq to keep
        # the statistics up to date.
        msg.data = False
        pub1.publish(msg)
        pub1_freq.tick()

        # We can call updater.update whenever is convenient. It will take care
        # of rate-limiting the updates.
        updater.update()
Esempio n. 52
0
    def update_object_pose(self):
        """ Function to externally update an object pose."""
        # Look down at the table.
        rospy.loginfo('Head attempting to look at table.')
        Response.force_gaze_action(GazeGoal.LOOK_DOWN)
        while (Response.gaze_client.get_state() == GoalStatus.PENDING
               or Response.gaze_client.get_state() == GoalStatus.ACTIVE):
            rospy.sleep(PAUSE_SECONDS)
        if Response.gaze_client.get_state() != GoalStatus.SUCCEEDED:
            rospy.logerr('Could not look down to take table snapshot')
            return False
        rospy.loginfo('Head is now (successfully) staring at table.')

        try:
            resp = self._segment_tabletop()
            rospy.loginfo("Adding landmarks")
            self._obj_sides = []
            self._reset_objects()

            # add the table
            xmin = resp.table.x_min
            ymin = resp.table.y_min
            xmax = resp.table.x_max
            ymax = resp.table.y_max
            depth = xmax - xmin
            width = ymax - ymin

            pose = resp.table.pose.pose
            pose.position.x = pose.position.x + xmin + depth / 2
            pose.position.y = pose.position.y + ymin + width / 2
            dimensions = Vector3(depth, width, 0.01)
            self._surface = _get_surface_marker(pose, dimensions)
            self._im_server.insert(self._surface, self.marker_feedback_cb)
            self._im_server.applyChanges()

            for cluster in resp.clusters:
                points = cluster.points
                if (len(points) == 0):
                    return Point(0, 0, 0)
                [minX, maxX, minY, maxY, minZ, maxZ] = [
                    points[0].x, points[0].x, points[0].y, points[0].y,
                    points[0].z, points[0].z
                ]
                for pt in points:
                    minX = min(minX, pt.x)
                    minY = min(minY, pt.y)
                    minZ = min(minZ, pt.z)
                    maxX = max(maxX, pt.x)
                    maxY = max(maxY, pt.y)
                    maxZ = max(maxZ, pt.z)
                object_sides_list = {
                    'minX': minX,
                    'minY': minY,
                    'minZ': minZ,
                    'maxX': maxX,
                    'maxY': maxY,
                    'maxZ': maxZ
                }
                self._obj_sides += [object_sides_list]
                self._add_new_object(
                    Pose(
                        Point((minX + maxX) / 2, (minY + maxY) / 2,
                              (minZ + maxZ) / 2), Quaternion(0, 0, 0, 1)),
                    Point(maxX - minX, maxY - minY, maxZ - minZ), False)
            return True

        except rospy.ServiceException, e:
            print "Call to segmentation service failed: %s" % e
            return False
Esempio n. 53
0
    def algorithm_setup(self):
        rospy.sleep(2)
        rospy.loginfo("Algorithm setup initiated: TPBP ")
        rospy.loginfo("%s, %s, %s, %s, %s, %s, %s, %s", self.dirname,
                      self.folder, self.num_robots, self.priority_nodes,
                      self.graph_name, self.algo, self.algo_params,
                      self.time_periods)
        print("Inside algo_setup Robot ids: " + str(self.robot_ids))
        if self.algo != "tpbp_walk":
            rospy.logwarn(" Algorithm is not TPBP_WALK. Received algo is <%s>",
                          self.algo)
            return
        self.dest_folder = self.dirname + '/processing/' + self.folder
        self.graph = nx.read_graphml(self.dirname + '/graph_ml/' +
                                     self.graph_name + '.graphml')
        for node in self.graph.nodes():
            self.graph.node[node]['idleness'] = 0.
            rospy.loginfo_throttle(1, ' node idleness set to 0')
        self.stamp = 0.0
        self.assigned = []
        for i in self.priority_nodes:
            self.assigned.append(False)
        self.robot_cur_walks = {}
        self.task_list = {}

        #self.task_done_subs = rospy.Subscriber('/task_done', TaskDone, self.task_done_CB)
        self.tpbp_service = rospy.Service('pmm_to_tpbp', TaskDoneSrv,
                                          self.task_done_CB)
        rospy.logdebug(" Constructing TPBP _line_=91 ")
        for index, ith_robot in zip(range(len(self.robot_ids)),
                                    self.robot_ids):
            rospy.loginfo("Computing initial tpbp_walk for robot %s",
                          ith_robot)
            time_start = time.clock()
            self.robot_cur_walks[ith_robot] = tpbp_walk(
                self.graph, self.algo_params, self.priority_nodes[index],
                self.priority_nodes, self.time_periods, self.assigned,
                self.dest_folder)
            print(self.robot_cur_walks)
            rospy.loginfo('PRINTING Dict')
            rospy.loginfo(self.robot_cur_walks)
            self.assigned[self.priority_nodes.index(
                self.robot_cur_walks[ith_robot][-1])] = True
            time_end = time.clock()
            time_spend = time_end - time_start
            rospy.loginfo(" Completed computation of tpbp_walk. Time spent %f",
                          time_spend)
            #self.task_list.append(map(int, self.robot_cur_walks[i]))
            rospy.loginfo("TASK_LIST PRINT:")
            self.task_list[ith_robot] = map(str,
                                            self.robot_cur_walks[ith_robot])
            rospy.loginfo(self.task_list)
        rospy.loginfo(self.robot_cur_walks)
        rospy.loginfo("Line _163_ Robot ids: %s", str(self.robot_ids))
        for ith_robot in self.robot_ids:
            rospy.loginfo("inside for of robot id %s", ith_robot)

            try:
                rospy.wait_for_service('tpbp_to_pmm')
                self.tpbp_to_pmm_proxy = rospy.ServiceProxy(
                    'tpbp_to_pmm', NextTaskSrv)
                try:
                    print(
                        self.tpbp_to_pmm_proxy(self.task_list[ith_robot],
                                               ith_robot))
                    rospy.loginfo(self.task_list[ith_robot])
                    rospy.loginfo(ith_robot)
                except Exception, e:
                    rospy.logwarn(
                        " Not able to publish the next task for robot %s Exception is:",
                        ith_robot)
                    rospy.logerr(e)
            except Exception, e:
                rospy.logerr(e)
                rospy.logerr(
                    "unable to create service client for robot %s for topic %s",
                    ith_robot)
Esempio n. 54
0
#!/usr/bin/env python3
import time
import rospy
from std_msgs.msg import String

if __name__ == '__main__':
    try:
        pub = rospy.Publisher('/move/cmd_str', String, queue_size=10)
        rospy.init_node('circle')
        rate = rospy.Rate(1) # 1hz
        now = time.time()
        future = now + 5
        while time.time() < future and not rospy.is_shutdown():
            rospy.loginfo("Turning Left.")
            pub.publish("left")

        pub.publish("stop")
    except Exception as e:
        rospy.logerr("An error occurred")
Esempio n. 55
0
  limbs= [None,None]
  limbs[RIGHT]= baxter_interface.Limb(LRTostr(RIGHT))
  limbs[LEFT]=  baxter_interface.Limb(LRTostr(LEFT))

  joint_names= [[],[]]
  #joint_names[RIGHT]= ['right_'+joint for joint in ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']]
  #joint_names[LEFT]=  ['left_' +joint for joint in ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']]
  joint_names[RIGHT]= limbs[RIGHT].joint_names()
  joint_names[LEFT]=  limbs[LEFT].joint_names()


  client= actionlib.SimpleActionClient('/robot/limb/%s/follow_joint_trajectory'%LRTostr(arm), control_msgs.msg.FollowJointTrajectoryAction)

  # Wait some seconds for the head action server to start or exit
  if not client.wait_for_server(rospy.Duration(5.0)):
    rospy.logerr('Exiting - Joint Trajectory Action Server Not Found')
    rospy.logerr('Run: rosrun baxter_interface joint_trajectory_action_server.py')
    rospy.signal_shutdown('Action Server not found')
    sys.exit(1)

  goal= control_msgs.msg.FollowJointTrajectoryGoal()
  goal.goal_time_tolerance= rospy.Time(0.1)
  goal.trajectory.joint_names= joint_names[arm]
  def add_point(goal, time, positions):
    point= trajectory_msgs.msg.JointTrajectoryPoint()
    point.positions= copy.deepcopy(positions)
    point.time_from_start= rospy.Duration(time)
    goal.trajectory.points.append(point)

  angles= limbs[arm].joint_angles()
  add_point(goal, 0.0, [angles[joint] for joint in joint_names[arm]])
Esempio n. 56
0
 def joyCB(self, status):
     # a callback function
     rospy.logerr("%s: no joyCB is overriden" % (self.name))
Esempio n. 57
0
 def close_gripper(self):
     try:
         self.gripper.grasp(-0.1)
     except:
         rospy.logerr('grasp close error')
def get_cur_pos():
  try:
    return rospy.wait_for_message("/rv7f/joint_states", JointState, 5.0)
  except (rospy.ROSException, rospy.ROSInterruptException):
    rospy.logerr('Unable to read current position')
    raise
Esempio n. 59
0
 def open_gripper(self):
     try:
         self.gripper.command(1.5)
     except:
         rospy.logerr('grasp open error')
Esempio n. 60
0
 def half_gripper(self):
     try:
         self.gripper.command(0.6)
     except:
         rospy.logerr('grasp close error')