Esempio n. 1
0
    def __init__(self):
        # Adjustable Parameters
        self.path_directory = rospy.get_param("~path_directory")
        self.mission_sound = rospy.get_param("~mission_sound")
        self.default_sound = rospy.get_param("~default_sound")
        self.battery_threshold = int(rospy.get_param("~battery_threshold",
                                                     50))  #[%]

        # Define Paths' File Name
        self.path_dict = [
            "HOME_A", "HOME_B", "HOME_1", "HOME_2", "HOME_3", "HOME_4",
            "HOME_5", "A_1", "A_2", "A_3", "A_4", "A_5", "B_1", "B_2", "B_3",
            "B_4", "B_5", "1_A", "2_A", "3_A", "4_A", "5_A", "1_B", "2_B",
            "3_B", "4_B", "5_B", "A_HOME", "B_HOME", "1_HOME", "2_HOME",
            "3_HOME", "4_HOME", "5_HOME", "HOME_CHARGE", "CHARGE_HOME",
            "rotate_HOME"
        ]

        # Define Actions Type
        self.action_dict = {0: "None", 1: "Table_Picking", 2: "Table_Dropping"}

        # Internal USE Variables - Modify with consultation
        self.rate = rospy.Rate(5)  # 5 [hz] <--> 0.2 [sec]
        self.tf2Buffer = tf2_ros.Buffer()
        self.tf2Listener = tf2_ros.TransformListener(self.tf2Buffer)
        # - Charging
        self.battery_level = 100
        self.battery_safe = True
        self.battery_full = False
        self.go_charge = False
        self.start_charge = False
        self.go_home = False
        # - Point to Point
        self.route_list = []
        self.route_seq = 0
        self.action_list = []
        self.action_seq = 0
        self.waypoint_list = []
        self.waypoint_seq = 0
        # - Status for Web
        self.delivery_status = ""
        self.action_status = True
        self.last_msg = None
        self.route_once = True
        self.XYZ = Point32()
        self.location = "HOME"
        self.speed = 0
        self.delivery_id = "0"
        self.delivery_mission = ""
        self.mission_activity = "NO ACTION"
        # - Others
        self.fsm_state = "STANDBY"
        self.restore = False

        # Publisher
        self.all_pub = rospy.Publisher("/web/all_status", String, queue_size=1)

        # Subscribers
        self.battery_sub = rospy.Subscriber("/battery/percent",
                                            String,
                                            self.batteryCB,
                                            queue_size=1)
        self.mission_sub = rospy.Subscriber("/web/mission",
                                            String,
                                            self.missionCB,
                                            queue_size=1)
        self.fsm_sub = rospy.Subscriber("/fsm_node/state",
                                        FSMState,
                                        self.fsmCB,
                                        queue_size=1)
        self.odom_sub = rospy.Subscriber("/gyro/odom",
                                         Odometry,
                                         self.odomCB,
                                         queue_size=1)

        # Service Servers
        self.route_done_srv = rospy.Service("/route/done", Empty,
                                            self.route_doneSRV)
        self.charging_done_srv = rospy.Service("/charging/done", Empty,
                                               self.charging_doneSRV)
        self.pick_table_done_srv = rospy.Service("/pick_table/done", Empty,
                                                 self.action_doneSRV)
        self.drop_table_done_srv = rospy.Service("/drop_table/done", Empty,
                                                 self.action_doneSRV)
        self.restore_srv = rospy.Service("/restore/call", Empty,
                                         self.restoreSRV)
        self.cancel_srv = rospy.Service("/mission/cancel", Empty,
                                        self.cancelSRV)

        # Service Clients
        self.path_call = rospy.ServiceProxy("/change_path", ChangePath)
        self.charging_call = rospy.ServiceProxy("charging/call", Empty)
        self.pick_table_call = rospy.ServiceProxy("/pick_table/call", Empty)
        self.drop_table_call = rospy.ServiceProxy("/drop_table/call", Empty)
        self.rosbag_start_call = rospy.ServiceProxy("/rosbag/start",
                                                    SetFileName)
        self.rosbag_stop_call = rospy.ServiceProxy("/rosbag/stop", Empty)
        self.fsm_call = rospy.ServiceProxy("/fsm_node/set_state", SetFSMState)
        self.sound_call = rospy.ServiceProxy("/sound/call", SetFileLocation)
        self.path_cancel_call = rospy.ServiceProxy("/path/cancel", Empty)

        # Startup
        self.sound_call(self.default_sound)
        # Main Loop()
        self.main_loop()
Esempio n. 2
0
    def run(self):
        try:
            vision = VisionProxy()

            tf_buffer = tf2_ros.Buffer(
                rospy.Duration(1200.0))  #tf buffer length
            tf_listener = tf2_ros.TransformListener(tf_buffer)

            i = raw_input("Enter the number of the part to be picked up:")
            i = int(i)
            if i > 0:
                rospy.logdebug("find object")
                expected_position = geometry_msgs.msg.PoseStamped()
                object_id = str(
                    i
                )  # The part number (see the definition in o2as_parts_description)
                item_pose = vision.find_object(expected_position,
                                               position_tolerance=0.2,
                                               object_id=object_id,
                                               camera="b_bot_camera")
                if item_pose != None:
                    # Transform pose from camera to workspace frame, so we can set the orientation more easily
                    target_frame = "workspace_center"
                    source_frame = item_pose.header.frame_id
                    transform = tf_buffer.lookup_transform(
                        target_frame, source_frame, rospy.Time(0),
                        rospy.Duration(1.0))
                    pick_pose = tf2_geometry_msgs.do_transform_pose(
                        item_pose, transform)

                    downward_orientation = geometry_msgs.msg.Quaternion(
                        *tf_conversions.transformations.quaternion_from_euler(
                            0, pi / 2, 0))
                    pick_pose.pose.orientation = downward_orientation

                    rospy.logdebug("pick object")
                    self.publish_marker(pick_pose, "pick_pose")  # debug
                    self.pick("b_bot",
                              pick_pose,
                              grasp_height=0.1,
                              approach_height=0.1,
                              speed_fast=0.2,
                              speed_slow=0.02,
                              gripper_command="easy_pick_only_inner")

                    # place_pose = pick_pose
                    # rospy.logdebug("place object")
                    # self.place("b_bot", place_pose, place_height = 0.03, approach_height = 0.05,
                    #                         speed_fast = 0.2, speed_slow = 0.02, gripper_command="easy_pick_only_inner",
                    #                         lift_up_after_place = False)

            else:
                rospy.loginfo("No valid entry. Skipping.")

            # rospy.logdebug("Go to home position")
            # self.go_to_named_pose("home_c", "c_bot")
            # self.go_to_named_pose("home_b", "b_bot")
            # self.go_to_named_pose("home_a", "a_bot")
            rospy.loginfo("============ Done!")

        except rospy.ROSInterruptException:
            pass
 except KeyError:
     logger.logwarn('No parameters found on the parameter server')
     exit(0)
 expected_keys = ['parent', 'child', 'rotation', 'translation']
 params_list = []
 for key, data in params.items():
     if cu.misc.has_keys(data, expected_keys):
         params_list.append(data)
 if len(params_list) == 0:
     logger.logwarn('No transformations found on the parameter server')
     exit(0)
 # Publish tf data
 num_tfs = len(params_list)
 rospy.loginfo('Publishing {0} transformation(s) to /tf'.format(num_tfs))
 tf_broadcaster = tf2_ros.TransformBroadcaster()
 tf_buffer = tf2_ros.Buffer()
 tf_listener = tf2_ros.TransformListener(tf_buffer)
 rate = rospy.Rate(publish_rate)
 tf_msg = TransformStamped()
 while not rospy.is_shutdown():
     for params in params_list:
         trans = np.array(params['translation'])
         rot = np.array(params['rotation'])
         child = params['child']
         parent = params['parent']
         tf_msg.header.stamp = rospy.Time.now()
         if invert:
             T = br.quaternion.to_transform(rot)
             T[:3, 3] = trans
             Tinv = br.transform.inverse(T)
             tf_msg.transform = cu.conversions.to_transform(Tinv)
#!/usr/bin/env python3

import rospy
import tf2_ros
from gazebo_msgs.msg import ModelStates
from geometry_msgs.msg import TransformStamped
import transforms3d
import numpy as np

rospy.init_node("localization_faker")

tf_buffer = tf2_ros.Buffer(cache_time=rospy.Duration(10.0))
tf_listener = tf2_ros.TransformListener(tf_buffer)
br = tf2_ros.TransformBroadcaster()


def model_state_to_tf(model_state_msg):
    t = TransformStamped()
    t.header.stamp = rospy.Time.now()
    for i, name in enumerate(model_state_msg.name):
        t.header.frame_id = name + "/map"
        t.child_frame_id = name + "/odom"
        try:
            robot_in_odom = tf_buffer.lookup_transform(
                name + "/odom",
                name + "/base_link",
                t.header.stamp,
                timeout=rospy.Duration(0.1))
        except tf2_ros.LookupException as ex:
            rospy.logwarn_throttle(5.0, rospy.get_name() + ": " + str(ex))
            return
def mover():
    global laser_range
    rospy.init_node('mover', anonymous=True)

    tfBuffer = tf2_ros.Buffer()
    tfListener = tf2_ros.TransformListener(tfBuffer)
    rospy.sleep(1.0)

    # subscribe to odometry data
    rospy.Subscriber('odom', Odometry, get_odom_dir)
    # subscribe to LaserScan data
    rospy.Subscriber('scan', LaserScan, get_laserscan)
    # subscribe to map occupancy data
    rospy.Subscriber('map', OccupancyGrid, callback, tfBuffer)

    rospy.on_shutdown(stopbot)

    rate = rospy.Rate(5)  # 5 Hz

    # save start time to file
    start_time = time.time()
    # initialize variable to write elapsed time to file
    timeWritten = 0

    # find direction with the largest distance from the Lidar,
    # rotate to that direction, and start moving
    rotatebot(0)
    rospy.loginfo(['len laser range', len(laser_range)])
    rospy.loginfo(['finished turning 0'])
    while laser_range[0] > stop_distance:
        movebot()
    stopbot()

    while not rospy.is_shutdown():
        if laser_range.size != 0:
            # check distances in front of TurtleBot and find values less
            # than stop_distance
            lri = (laser_range[front_angles] < float(stop_distance)).nonzero()
            rospy.loginfo('Distancessssssss: %s', str(lri))
            rospy.loginfo(['move!'])
            move()
        else:
            lri[0] = []

        # if the list is not empty
        if (len(lri[0]) > 0):
            rospy.loginfo(['Stop!'])
            # find direction with the largest distance from the Lidar
            # rotate to that direction
            # start moving
            rospy.loginfo(['running our function'])
            move()

        # check if SLAM map is complete
        if timeWritten:
            if closure(occdata):
                # map is complete, so save current time into file
                with open("maptime.txt", "w") as f:
                    f.write("Elapsed Time: " + str(time.time() - start_time))
                timeWritten = 1
                # play a sound
                # soundhandle = SoundClient()
                # rospy.sleep(1)
                # soundhandle.stopAll()
                # soundhandle.play(SoundRequest.NEEDS_UNPLUGGING)
                # rospy.sleep(2)
                # save the map
                cv2.imwrite('mazemap.png', occdata)

        rate.sleep()
Esempio n. 6
0
    def __init__(self, inertial_frame_id='world'):
        """Class constructor."""
        #assert inertial_frame_id in ['world', 'world_ned']
        # Reading current namespace
        self._namespace = rospy.get_namespace()
        self._logger = logging.getLogger('vehicle_model')

        self._inertial_frame_id = inertial_frame_id
        self._body_frame_id = None

        if self._inertial_frame_id == 'world_ned':
            self._body_frame_id = 'anahita/base_link'
        else:
            self._body_frame_id = 'base_link_ned'

        tf_buffer = tf2_ros.Buffer()
        listener = tf2_ros.TransformListener(tf_buffer)
        tf_trans_ned_to_enu = None
        try:
            tf_trans_ned_to_enu = tf_buffer.lookup_transform(
                'world', 'world_ned', rospy.Time(), rospy.Duration(1))
        except Exception as e:
            self._logger.error('No transform found between world and the '
                               'world_ned ' + self.namespace)
            self._logger.error(str(e))
            self.transform_ned_to_enu = None

        if tf_trans_ned_to_enu is not None:
            self.transform_ned_to_enu = quaternion_matrix(
                (tf_trans_ned_to_enu.transform.rotation.x,
                 tf_trans_ned_to_enu.transform.rotation.y,
                 tf_trans_ned_to_enu.transform.rotation.z,
                 tf_trans_ned_to_enu.transform.rotation.w))[0:3, 0:3]

        print('Transform world_ned (NED) to world (ENU)=\n' +
              str(self.transform_ned_to_enu))

        self._mass = 0
        if rospy.has_param('~mass'):
            self._mass = rospy.get_param('~mass')
            if self._mass <= 0:
                raise rospy.ROSException('Mass has to be positive')
        else:
            self._mass = 862.87

        self._inertial = dict(ixx=0, iyy=0, izz=0, ixy=0, ixz=0, iyz=0)
        if rospy.has_param('~inertial'):
            print('has inertial')
            inertial = rospy.get_param('~inertial')
            for key in self._inertial:
                if key not in inertial:
                    raise rospy.ROSException('Invalid moments of inertia')
            self._inertial = inertial
        else:
            self._inertial['ixx'] = 243.39
            self._inertial['iyy'] = 367.20
            self._inertial['izz'] = 319.23
            self._inertial['ixy'] = 1.44
            self._inertial['ixz'] = 33.41
            self._inertial['iyz'] = 2.6

        self._cog = [0, 0, 0]
        if rospy.has_param('~cog'):
            self._cog = rospy.get_param('~cog')
            if len(self._cog) != 3:
                raise rospy.ROSException('Invalid center of gravity vector')

        self._cob = [0, 0, 0]
        if rospy.has_param('~cob'):
            self._cob = rospy.get_param('~cob')
            if len(self._cob) != 3:
                raise rospy.ROSException('Invalid center of buoyancy vector')
        else:
            self._cob = [0, 0, 0.05]

        self._body_frame = 'base_link'
        if rospy.has_param('~base_link'):
            self._body_frame = rospy.get_param('~base_link')

        self._volume = 0.0
        if rospy.has_param('~volume'):
            self._volume = rospy.get_param('~volume')
            if self._volume <= 0:
                raise rospy.ROSException('Invalid volume')
        else:
            self._volume = 0.5

        # Fluid density
        self._density = 1028.0
        if rospy.has_param('~density'):
            self._density = rospy.get_param('~density')
            if self._density <= 0:
                raise rospy.ROSException('Invalid fluid density')

        # Bounding box
        self._height = 0.4
        self._length = 1.0
        self._width = 0.5

        if rospy.has_param('~height'):
            self._height = rospy.get_param('~height')
            if self._height <= 0:
                raise rospy.ROSException('Invalid height')

        if rospy.has_param('~length'):
            self._length = rospy.get_param('~length')
            if self._length <= 0:
                raise rospy.ROSException('Invalid length')

        if rospy.has_param('~width'):
            self._width = rospy.get_param('~width')
            if self._width <= 0:
                raise rospy.ROSException('Invalid width')

        # Calculating the rigid-body mass matrix
        self._M = np.zeros(shape=(6, 6), dtype=float)
        self._M[0:3, 0:3] = self._mass * np.eye(3)
        self._M[0:3, 3:6] = - self._mass * \
            cross_product_operator(self._cog)
        self._M[3:6, 0:3] = self._mass * \
            cross_product_operator(self._cog)
        self._M[3:6, 3:6] = self._calc_inertial_tensor()

        # Loading the added-mass matrix
        self._Ma = np.zeros((6, 6))
        if rospy.has_param('~Ma'):
            self._Ma = np.array(rospy.get_param('~Ma'))
            if self._Ma.shape != (6, 6):
                raise rospy.ROSException('Invalid added mass matrix')
        else:
            self._Ma[0][0] = 379.79
            self._Ma[0][1] = -3.8773
            self._Ma[0][2] = -53.32
            self._Ma[0][3] = 4.5426
            self._Ma[0][4] = -85.54
            self._Ma[0][5] = -3.8033
            self._Ma[1][0] = -3.8773
            self._Ma[1][1] = 622
            self._Ma[1][2] = 25.29
            self._Ma[1][3] = 209.44
            self._Ma[1][4] = -2.8488
            self._Ma[1][5] = 32.726
            self._Ma[2][0] = -53.32
            self._Ma[2][1] = 25.29
            self._Ma[2][2] = 1959.9
            self._Ma[2][3] = 3.1112
            self._Ma[2][4] = -196.42
            self._Ma[2][5] = 5.774
            self._Ma[3][0] = 4.5426
            self._Ma[3][1] = 209.44
            self._Ma[3][2] = 3.1112
            self._Ma[3][3] = 264.9
            self._Ma[3][4] = -5.027
            self._Ma[3][5] = 11.019
            self._Ma[4][0] = -85.54
            self._Ma[4][1] = -2.8488
            self._Ma[4][2] = -196.42
            self._Ma[4][3] = -5.027
            self._Ma[4][4] = 442.69
            self._Ma[4][5] = -1.1162
            self._Ma[5][0] = -3.8033
            self._Ma[5][1] = 32.726
            self._Ma[5][2] = 5.775
            self._Ma[5][3] = 11.019
            self._Ma[5][4] = -1.1162
            self._Ma[5][5] = 114.32

        # Sum rigid-body and added-mass matrices
        self._Mtotal = np.zeros(shape=(6, 6))
        self._calc_mass_matrix()

        # Acceleration of gravity
        self._gravity = 9.81

        # Initialize the Coriolis and centripetal matrix
        self._C = np.zeros((6, 6))

        # Vector of restoring forces
        self._g = np.zeros(6)

        # Loading the linear damping coefficients
        self._linear_damping = np.zeros(shape=(6, 6))
        if rospy.has_param('~linear_damping'):
            self._linear_damping = np.array(rospy.get_param('~linear_damping'))
            if self._linear_damping.shape == (6, ):
                self._linear_damping = np.diag(self._linear_damping)
            if self._linear_damping.shape != (6, 6):
                raise rospy.ROSException(
                    'Linear damping must be given as a 6x6 matrix or the diagonal coefficients'
                )
        else:
            self._linear_damping[0][0] = -34.82
            self._linear_damping[1][1] = -39.48
            self._linear_damping[2][2] = -368.4
            self._linear_damping[3][3] = -138.8
            self._linear_damping[4][4] = -159.77
            self._linear_damping[5][5] = -55

        # Loading the nonlinear damping coefficients
        self._quad_damping = np.zeros(shape=(6, ))
        if rospy.has_param('~quad_damping'):
            self._quad_damping = np.array(rospy.get_param('~quad_damping'))
            if self._quad_damping.shape != (6, ):
                raise rospy.ROSException(
                    'Quadratic damping must be given defined with 6 coefficients'
                )
        else:
            self._quad_damping[0] = -378.22
            self._quad_damping[1] = -502.53
            self._quad_damping[2] = -921.01
            self._quad_damping[3] = -342
            self._quad_damping[4] = -394.44
            self._quad_damping[5] = -263.27

        # Loading the linear damping coefficients proportional to the forward speed
        self._linear_damping_forward_speed = np.zeros(shape=(6, 6))
        if rospy.has_param('~linear_damping_forward_speed'):
            self._linear_damping_forward_speed = np.array(
                rospy.get_param('~linear_damping_forward_speed'))
            if self._linear_damping_forward_speed.shape == (6, ):
                self._linear_damping_forward_speed = np.diag(
                    self._linear_damping_forward_speed)
            if self._linear_damping_forward_speed.shape != (6, 6):
                raise rospy.ROSException(
                    'Linear damping proportional to the forward speed must be given as a 6x6 '
                    'matrix or the diagonal coefficients')

        # Initialize damping matrix
        self._D = np.zeros((6, 6))

        # Vehicle states
        self._pose = dict(pos=np.zeros(3), rot=quaternion_from_euler(0, 0, 0))
        # Velocity in the body frame
        self._vel = np.zeros(6)
        # Acceleration in the body frame
        self._acc = np.zeros(6)
        # Generalized forces
        self._gen_forces = np.zeros(6)
Esempio n. 7
0
    def __init__(self,rate):
        """
        
        Constructor of the CrackMap class. 
        
        
        ------
        
        Input : 
            
            rate : the rate associated to rospy.
        
        Parameters : 
            
            PicID : int, associated with the ID of the last depth message
            recorded.
            
            cur_PicID : int, associated with the ID of the currently processed
            depth message (by the checkCracks function)
            
            CIFrontColor : Camera Info object, containing all the intrinsic parameters
            of the camera.
            
            depthMsg : Image Message : ROS message containing the last message published by 
            the topic /phantomx/crack_image (a depth picture containing the depth of 
            all the detected cracks on a picture).
            
            depthPic : OpenCV Matrix, containing the last depth picture published by
            the topic /phantomx/crack_image.
            
            allCracksInCave : list, containing all the cracks detected since the beginning
            of the simulation.
            
            nb_cracks : int, containing the number of cracks detected since the beginning of the
            simulation.
            
            dist_center_cracks : float, indicating the minimum 3D distance between the barycenters
            of the cracks.
            
            R_rpic2rcam : rotationnal matrix from the picture frame to the camera sensor frame
            
            R_rcam2rpic : rotationnal matrix from the camera sensor frame to the picture frame
            
            buffer : BufferInterface object, stores the last transformation matrices between frames for
            a certain time (60 seconds here).
            
            listener : TransformListener object, used to express the coordinates of a pixel from the 
            camera sensor frame into the cave frame.
            
        """
        
        self.bridge = CvBridge()
        self.PicID = 0
        self.cur_PicID = 0
        self.CIFrontDepth = None
        self.CIFrontColor = None
        self.depthMsg = None
        self.depthPic = None
        self.allCracksInCave = []
        self.nb_cracks = len(self.allCracksInCave)
        
        self.dist_center_cracks = 0.5
        self.dist_close_cracks = 4.0
        #self.current_depthPic = None
        
        self.buffer = tf2.Buffer(rospy.Duration(60)) # prend 60s de tf 
        self.listener = tf2.TransformListener(self.buffer)
        
    
        self.CIDtopic = rospy.Subscriber("/phantomx/camera_front/depth/camera_info", CamInfoMSG,self.callbackCIFrontDepth)
        self.CIPtopic = rospy.Subscriber("/phantomx/camera_front/color/camera_info", CamInfoMSG,self.callbackCIFrontColor)
        rospy.Subscriber("/phantomx/crack_image", ImageMSG,self.callbackDepthPic)

    
        self.rate = rospy.Rate(rate)
        
        self.markerPub = rospy.Publisher('/phantomx/crack_markers', MarkerArray, queue_size=10)

        
        time.sleep(3)
        
        self.cameraModel = image_geometry.PinholeCameraModel()

        self.R_rcam2rpic = np.array([[0.0,-1.0,0.0],[0.,0.,-1.0],[1.0,0.0,0.0]])
        self.R_rpic2rcam = np.linalg.inv(self.R_rcam2rpic)
        

        
        while not rospy.is_shutdown():
            if (self.depthMsg is not None):
                self.checkCracks()
            self.displayCracks()
            self.rate.sleep()
Esempio n. 8
0
 def __init__(self):
     rospy.init_node('robot')
     self.tfBuffer = tf2_ros.Buffer()
     self.listener = tf2_ros.TransformListener(self.tfBuffer)
     self.pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
     self.zeroTime = rospy.Time()
Esempio n. 9
0
def main(unused_argv):
    vrb = ViveRobotBridge()
    #    vrb.__init__()

    rospy.init_node('vive_listener')

    tfBuffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(tfBuffer)

    #    pub = rospy.Publisher('tarpos_pub', geometry_msgs.msg.Transform, queue_size=1)
    #    _joy_sub = rospy.Subscriber('/vive/controller_LHR_FFFF3F47/joy', Joy, vive_controller_button_callback, queue_size=1)

    temp_flag = 0

    pre_position = [0, 0, 0]
    pre_pose = [1, 0, 0, 0]
    temp = [1, 0, 0, 0]

    rate = rospy.Rate(100.0)

    limits = 0.02

    env = Environment(FLAGS.assets_root,
                      disp=FLAGS.disp,
                      shared_memory=FLAGS.shared_memory,
                      hz=480)
    task = tasks.names[FLAGS.task]()
    task.mode = FLAGS.mode
    agent = task.oracle(env)
    env.set_task(task)
    obs = env.reset()
    info = None
    ee_pose = ((0.46562498807907104, -0.375, 0.3599780201911926), (0.0, 0.0,
                                                                   0.0, 1.0))
    while not rospy.is_shutdown():
        #act = agent.act(obs, info)

        #obs, reward, done, info = env.step(act)

        env.movep(ee_pose)
        if vrb.grasp == 1:
            env.ee.activate()
        else:
            env.ee.release()

        try:
            trans = tfBuffer.lookup_transform('world',
                                              'controller_LHR_FF777F05',
                                              rospy.Time())
#            rospy.loginfo(trans)
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                tf2_ros.ExtrapolationException):
            rospy.loginfo("error")
            rate.sleep()
            continue

        if vrb.offset_flag == 1:
            if temp_flag == 0:
                #                vrb.offset[0] = trans.transform.translation.x
                #                vrb.offset[1] = trans.transform.translation.y
                #                vrb.offset[2] = trans.transform.translation.z
                #                print(vrb.offset)
                pre_position[0] = trans.transform.translation.x
                pre_position[1] = trans.transform.translation.y
                pre_position[2] = trans.transform.translation.z

                pre_pose[0] = trans.transform.rotation.x
                pre_pose[1] = trans.transform.rotation.y
                pre_pose[2] = trans.transform.rotation.z
                pre_pose[3] = trans.transform.rotation.w
            else:
                msg = geometry_msgs.msg.Transform()
                #                msg.translation.x = (trans.transform.translation.x-pre_position[0])/10
                #                msg.translation.y = (trans.transform.translation.y-pre_position[1])/10
                #                msg.translation.z = (trans.transform.translation.z-pre_position[2])/10

                #                compute delta distance
                msg.translation.x = trans.transform.translation.x - pre_position[
                    0]
                msg.translation.y = trans.transform.translation.y - pre_position[
                    1]
                msg.translation.z = trans.transform.translation.z - pre_position[
                    2]

                #                if msg.translation.x >0.02:
                #                    msg.translation.x = 0.02
                #                if msg.translation.y >0.02:
                #                    msg.translation.y = 0.02
                #                if msg.translation.z >0.02:
                #                    msg.translation.z = 0.02
                #                if msg.translation.x <-0.02:
                #                    msg.translation.x = -0.02
                #                if msg.translation.y <-0.02:
                #                    msg.translation.y = -0.02
                #                if msg.translation.z <-0.02:
                #                    msg.translation.z = -0.02

                if msg.translation.x > limits:
                    msg.translation.x = limits
                if msg.translation.y > limits:
                    msg.translation.y = limits
                if msg.translation.z > limits:
                    msg.translation.z = limits
                if msg.translation.x < -limits:
                    msg.translation.x = -limits
                if msg.translation.y < -limits:
                    msg.translation.y = -limits
                if msg.translation.z < -limits:
                    msg.translation.z = -limits

                print(msg.translation)

                #                temp[0] = trans.transform.rotation.x
                #                temp[1] = trans.transform.rotation.y
                #                temp[2] = trans.transform.rotation.z
                #                temp[3] = trans.transform.rotation.w
                #
                #
                #
                #                q = Quaternion(pre_pose) * Quaternion(temp).inverse
                #
                #
                #                msg.rotation.x = q.x
                #                msg.rotation.y = q.y
                #                msg.rotation.z = q.z
                #                msg.rotation.w = q.w#

                msg.rotation.x = trans.transform.rotation.x
                msg.rotation.y = trans.transform.rotation.y
                msg.rotation.z = trans.transform.rotation.z
                msg.rotation.w = trans.transform.rotation.w

                ee_position = list(p.getLinkState(env.ur5, env.ee_tip)[4])
                #ee_orientation = p.getLinkState(self.ur5, self.ee_tip)[5]
                ee_orientation = (0, 0, 0, 1)
                ee_position[0] = ee_position[0] + msg.translation.y
                ee_position[1] = ee_position[1] - msg.translation.x
                # z axis limit
                z_control = ee_position[2] + msg.translation.z
                if z_control < 0.02:
                    z_control = 0.02
                ee_position[2] = z_control

                ee_pose = (tuple(ee_position), ee_orientation)

                # rectified quaternion

                #                theta_x = np.arcsin(2*(trans.transform.rotation.w*trans.transform.rotation.y
                #                                       - trans.transform.rotation.z*trans.transform.rotation.x))
                #                temp = (trans.transform.rotation.x**2 + trans.transform.rotation.z**2)**0.5
                #                z_v = trans.transform.rotation.z / temp
                #                x_v = trans.transform.rotation.x / temp
                #                msg.rotation.x = x_v * np.sin(theta_x)#0#np.sin(0.5*theta_x)
                #                msg.rotation.y = 0#y_v * np.sin(theta_x)#0
                #                msg.rotation.z = z_v * np.sin(theta_x)
                #                msg.rotation.w = trans.transform.rotation.w
                #
                #                msg.translation.x = 0
                #                msg.translation.y = 0
                #                msg.translation.z = 0
                #
                #                msg.rotation.x = 0
                #                msg.rotation.y = 0
                #                msg.rotation.z = 0
                #                msg.rotation.w = 1

                print(msg.rotation)

                vrb.pub.publish(msg)

                pre_position[0] = trans.transform.translation.x
                pre_position[1] = trans.transform.translation.y
                pre_position[2] = trans.transform.translation.z

        temp_flag = vrb.offset_flag
        rate.sleep()
def controller(turtlebot_frame, goal_frame):
    """
  Controls a turtlebot whose position is denoted by turtlebot_frame,
  to go to a position denoted by target_frame
  Inputs:
  - turtlebot_frame: the tf frame of the AR tag on your turtlebot
  - target_frame: the tf frame of the target AR tag
  """

    ################################### YOUR CODE HERE ##############

    #Create a publisher and a tf buffer, which is primed with a tf listener
    pub = rospy.Publisher("/yellow/mobile_base/commands/velocity",
                          Twist,
                          queue_size=10)
    tfBuffer = tf2_ros.Buffer()
    tfListener = tf2_ros.TransformListener(tfBuffer)
    br = tf2_ros.TransformBroadcaster()

    # Create a timer object that will sleep long enough to result in
    # a 10Hz publishing rate
    r = rospy.Rate(10)  # 10hz

    K1 = 0.3
    K2 = 1
    Ks = [K1, K2]
    # Loop until the node is killed with Ctrl-C
    while not rospy.is_shutdown():
        try:
            #print("Before")
            rospy.sleep(0.1)
            t = TransformStamped()
            t.header.stamp = rospy.Time.now()
            t.transform.translation.x = 0
            t.transform.translation.y = 0
            t.transform.translation.z = 0

            t.transform.rotation.x = 0
            t.transform.rotation.y = 0
            t.transform.rotation.z = -0.707
            t.transform.rotation.w = 0.707
            t.header.frame_id = "left"
            t.child_frame_id = "ball"

            br.sendTransform(t)

            #t.header.frame_id = "test"
            trans = tfBuffer.lookup_transform(turtlebot_frame, "left",
                                              rospy.Time())

            #print("After")
            # Process trans to get your state error
            # Generate a control command to send to the robot
            #print(trans)
            x = trans.transform.translation.x * Ks[0]
            theta = trans.transform.translation.y * Ks[1]
            #print(turtlebot_frame)
            control_command = Twist(Vector3(x, 0, 0), Vector3(0, 0, theta))

            #################################### end your code ###############

            pub.publish(control_command)
            print(control_command)
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                tf2_ros.ExtrapolationException) as e:
            print(e)
            pass
        # Use our rate object to sleep until it is time to publish again
        r.sleep()
    def depthImageCallback(self, data):

        time_start = time.time()

        # neck_rad = self.whole_body.joint_positions['head_tilt_joint']
        # if neck_rad != -np.radians(45.0):
        #     self.whole_body.move_to_joint_positions({'head_tilt_joint':-np.radians(45.0)})

        try:
            tf_buffer = tf2_ros.Buffer()
            tf_listener = tf2_ros.TransformListener(tf_buffer)
            trans = tf_buffer.lookup_transform("map",
                                               "head_rgbd_sensor_rgb_frame",
                                               rospy.Time(0),
                                               rospy.Duration(1.0))
        except Exception as e:
            rospy.loginfo("%s", e)
            return

        depthImage = CvBridge().imgmsg_to_cv2(data, '16UC1')
        heightImage = trans.transform.translation.z - np.array(
            depthImage) * np.sin(np.radians(45.0)) * 0.001 - self.floor_adjust
        heightImage[depthImage < 10.0] = 0.0

        if self.use_color:
            colorImage = CvBridge().imgmsg_to_cv2(self.colorImageData, 'rgb8')
            colorImage[heightImage < self.heightThreshold] = [0, 0, 0]
            grayImage = cv2.cvtColor(colorImage, cv2.COLOR_BGR2GRAY)
            retval, bw = cv2.threshold(grayImage, 50, 225,
                                       cv2.THRESH_BINARY | cv2.THRESH_OTSU)
        else:
            depthImage[heightImage < self.heightThreshold] = 0.0
            grayImage = np.uint8(depthImage)
            retval, bw = cv2.threshold(grayImage, 1, 225, cv2.THRESH_BINARY)

        image, contours, hierarchy = cv2.findContours(bw, cv2.RETR_LIST,
                                                      cv2.CHAIN_APPROX_NONE)

        # s = cv2.cvtColor(colorImage, cv2.COLOR_BGR2RGBA)
        # s[heightImage<self.heightThreshold] = [0,0,0,0]
        # cv2.imwrite("hoge.png", s)

        print "depth time : ", time.time() - time_start

        br = tf.TransformBroadcaster()
        xywh = np.array([
            cv2.boundingRect(c) for c in contours
            if 200 < cv2.contourArea(c) < 2000 and len(c) > 0
        ])
        if len(xywh) < 1:
            return
        if self.use_color:
            for x, y, w, h in xywh:
                cv2.rectangle(colorImage, (x, y), (x + w, y + h), (0, 255, 0),
                              2)
        elif self.output_flag:
            for x, y, w, h in xywh:
                cv2.rectangle(depthImage, (x, y), (x + w, y + h), 255, 2)
        x_pixel_uncheck = xywh[:, 0] + xywh[:, 2] / 2
        y_pixel_uncheck = xywh[:, 1] + xywh[:, 3] / 2
        dist_uncheck = depthImage[y_pixel_uncheck, x_pixel_uncheck] * 0.001

        check_array = (0.8 < dist_uncheck)
        x_pixel = x_pixel_uncheck[check_array]
        y_pixel = y_pixel_uncheck[check_array]
        dist = dist_uncheck[check_array]

        x_deg = width_pixel_deg * (x_pixel - image_width / 2)
        y_deg = height_pixel_deg * (y_pixel - image_height / 2)
        tf_x = np.sin(np.radians(x_deg)) * dist
        tf_y = np.cos(np.radians(y_deg)) * dist

        for count, (tf_xi, tf_yi) in enumerate(zip(tf_x, tf_y)):
            name = "followeme_" + str(count)
            br.sendTransform((tf_xi, 0.0, tf_yi), (0.0, 0.0, 0.0, 1.0),
                             rospy.Time.now(), name,
                             "head_rgbd_sensor_rgb_frame")

        print "fin time : ", time.time() - time_start

        if self.use_color:
            self.image_pub.publish(CvBridge().cv2_to_imgmsg(
                colorImage, "rgb8"))
        elif self.output_flag:
            self.image_pub.publish(CvBridge().cv2_to_imgmsg(
                depthImage, "16UC1"))
Esempio n. 12
0
 def __init__(self):
     rospy.init_node('node_tf_echo')
     self._tfBuffer = tf2_ros.Buffer()
     self._listener = tf2_ros.TransformListener(self._tfBuffer)
    def __init__(self):
        action_ns = rospy.get_param('~action_ns', '')
        robot_name = rospy.get_param('~robot_name')
        self.lock = threading.RLock()

        self.publish_rate = rospy.get_param('~publish_rate', 50)

        sec = rospy.get_param('~tf_exp_time', 90.0)
        self.tf_exp_time = rospy.Duration(sec)

        self.human_frame = rospy.get_param('~human_frame_id',
                                           'human_footprint')
        self.robot_root_frame = rospy.get_param('~robot_root_frame',
                                                robot_name + '/odom')
        drone_goto_action_ns = rospy.get_param(
            '~drone_goto_action_ns', '/' + robot_name + '/goto_action')
        drone_shape_action_ns = rospy.get_param(
            '~drone_followpath_action_ns',
            '/' + robot_name + '/followshape_action')
        drone_followme_action_ns = rospy.get_param(
            '~drone_followme_action_ns', '/' + robot_name + '/followme_action')

        self.ray_origin_frame = rospy.get_param('~ray_origin_frame', 'eyes')
        self.ray_direction_frame = rospy.get_param('~ray_direction_frame',
                                                   'pointer')
        self.ray_inverse = rospy.get_param('~ray_inverse', False)

        pose_topic = rospy.get_param('~robot_pose_topic',
                                     '/' + robot_name + '/odom/pose/pose')
        pose_topic_class, pose_real_topic, pose_eval_func = rostopic.get_topic_class(
            pose_topic)
        self.robot_pose_msg_eval = pose_eval_func

        ############ FIXME ############
        trigger_topic = rospy.get_param('~trigger_topic',
                                        '/' + robot_name + '/joy/buttons[6]')
        trigger_topic_class, trigger_real_topic, trigger_eval_func = rostopic.get_topic_class(
            trigger_topic)
        self.trigger_msg_eval = trigger_eval_func
        # self.trigger_sub = rospy.Subscriber(trigger_real_topic, trigger_topic_class, self.trigger_cb)
        self.trigger_val = None
        self.last_trigger_val = None
        ###############################

        self.timewindow = rospy.get_param('~timewindow', 5.0)
        self.sync_freq = rospy.get_param('~freq', 20.0)
        self.sample_size = rospy.get_param('~sample_size',
                                           self.sync_freq * 3.0)

        self.residual_threshold = np.radians(
            rospy.get_param('~residual_threshold_deg', 3.0))

        self.robot_motion_span_min = rospy.get_param('~robot_motion_span_min',
                                                     0.20)  # 20 cm

        if self.timewindow and self.sync_freq:
            self.queue_size = int(self.timewindow * self.sync_freq)
            # 5.0 seconds at 5 Hz
            rospy.loginfo('Max queue size: {}'.format(self.queue_size))
            if self.sample_size > self.queue_size:
                rospy.loginfo(
                    'sample_size [{}] is bigger than queue_size [{}]. Setting sample_size = queue_size'
                    .format(self.sample_size, self.queue_size))
                self.sample_size = self.queue_size
        else:
            rospy.logwarn(
                'Either timewindow or queue_size is set to 0. Using unbound queue.'
            )
            self.queue_size = None
        self.deque = collections.deque(maxlen=self.queue_size)

        self.relloc_deque = collections.deque(
            maxlen=self.sync_freq *
            1.0)  # Use 1s of relloc data to trigger selection

        self.robot_pose_msg = None
        self.robot_sub = rospy.Subscriber(pose_real_topic, pose_topic_class,
                                          self.robot_pose_cb)

        self.is_valid_pub = rospy.Publisher('is_relloc_valid',
                                            Bool,
                                            queue_size=10)

        self.initial_guess = np.array([0, 0, 0, 0])
        self.reset_state()

        self.tf_buff = tf2_ros.Buffer()
        self.tf_ls = tf2_ros.TransformListener(self.tf_buff)
        self.tf_br = tf2_ros.TransformBroadcaster()

        self.drone_goto_client = actionlib.SimpleActionClient(
            drone_goto_action_ns, GoToAction)
        rospy.loginfo('Waiting for ' + drone_goto_action_ns)
        self.drone_goto_client.wait_for_server()

        # self.drone_shape_client = actionlib.SimpleActionClient(drone_shape_action_ns, FollowShapeAction)
        # rospy.loginfo('Waiting for ' + drone_shape_action_ns)
        # self.drone_shape_client.wait_for_server()

        self.drone_followme_client = actionlib.SimpleActionClient(
            drone_followme_action_ns, FollowMeAction)
        rospy.loginfo('Waiting for ' + drone_followme_action_ns)
        self.drone_followme_client.wait_for_server()

        self.relloc_server = actionlib.SimpleActionServer(
            action_ns + '/relloc_action', MotionRellocContAction,
            self.execute_relloc, False)
        self.relloc_server.start()

        self.relloc_cont_server = actionlib.SimpleActionServer(
            action_ns + '/relloc_cont_action', MotionRellocContAction,
            self.execute_relloc_cont, False)
        self.relloc_cont_server.start()
Esempio n. 14
0
    if not os.path.isdir(os.path.join(save_dir, str(i)+'_npy')):
        os.makedirs(os.path.join(save_dir, str(i)+'_npy', 'data'))
        os.makedirs(os.path.join(save_dir, str(i)+'_npy', 'gt'))

exit()
'''

origin_tag = '0'
all_tags = [
    '0', '1', '2', '3', '4', '5', '16', '7', '8', '9', '10', '11', '17', '14',
    '15'
]

rospy.init_node("data_node", anonymous=True)

buf = tf.Buffer()
ls = tf.TransformListener(buf)

gt_dict = dict()
data_dict = dict()


def data_synced(tag, tag_odom):
    global origin_tag, data_dict, gt_dict

    if len(tag.detections) > 0:
        if len(tag.detections) > 1:
            detected_tag = sort_l2(tag.detections)
        else:
            detected_tag = tag.detections[0]
    def __init__(self):
        rospy.init_node("show_world_model_objects")
        self.marker_publisher = rospy.Publisher(
            "visualization_world_model_markers", Marker, queue_size=10)

        # object properties
        self.ball_diameter = 0.17
        self.lifetime_ball = rospy.get_param('behavior/body/ball_lost_time')
        self.lifetime_goal = rospy.get_param('behavior/body/goal_lost_time')
        self.lifetime_ball_kick_area = 1
        self.post_diameter = 0.15
        self.post_height = 1.10
        position = Pose()
        position.position.x = 1.0
        position.position.y = 1.0
        position.position.z = 0.0
        position.orientation.w = 1.0
        position.orientation.x = 0.0
        position.orientation.y = 0.0
        position.orientation.z = 0.0

        self.base_footprint_frame = rospy.get_param('~base_footprint_frame',
                                                    'base_footprint')

        # init ball marker
        self.marker_ball = Marker()  # type: Marker
        self.marker_ball.id = 0
        self.marker_ball.type = Marker.SPHERE
        self.ball_pose = Pose()
        scale = Vector3(self.ball_diameter, self.ball_diameter,
                        self.ball_diameter)
        self.marker_ball.scale = scale
        self.ball_color = ColorRGBA()
        self.ball_color.a = 0.0
        self.ball_color.g = 1.0
        self.marker_ball.color = self.ball_color
        self.marker_ball.pose = position
        self.marker_ball.lifetime = rospy.Duration(self.lifetime_ball)

        # init goal markers
        self.marker_goal_first_post = Marker()  # type:Marker
        self.marker_goal_first_post.id = 1
        self.marker_goal_first_post.type = Marker.CYLINDER
        self.goal_post_pose = Pose()
        scale = Vector3(self.post_diameter, self.post_diameter,
                        self.post_height)
        self.marker_goal_first_post.scale = scale
        self.post_left_color = ColorRGBA()
        self.post_left_color.a = 0.0
        self.post_left_color.g = 1.0
        self.marker_goal_first_post.color = self.post_left_color
        self.marker_goal_first_post.pose = position
        self.marker_goal_first_post.lifetime = rospy.Duration(
            self.lifetime_goal)

        self.marker_goal_second_post = Marker()  # type:Marker
        self.marker_goal_second_post.id = 2
        self.marker_goal_second_post.type = Marker.CYLINDER
        self.goal_post_pose = Pose()
        scale = Vector3(self.post_diameter, self.post_diameter,
                        self.post_height)
        self.marker_goal_second_post.scale = scale
        self.post_right_color = ColorRGBA()
        self.post_right_color.a = 0.0
        self.post_right_color.b = 1.0
        self.marker_goal_second_post.color = self.post_right_color
        self.marker_goal_second_post.pose = position
        self.marker_goal_second_post.lifetime = rospy.Duration(
            self.lifetime_goal)

        # init ball kick area markers
        self.marker_kick_area = Marker()
        self.marker_kick_area.id = 3
        self.marker_kick_area.type = Marker.LINE_STRIP
        self.marker_kick_area.scale.x = 0.01
        self.kick_area_color = ColorRGBA()
        self.kick_area_color.a = 0.5
        self.kick_area_color.r = 0.8
        self.kick_area_color.g = 0.8
        self.kick_area_color.b = 0.8
        self.marker_kick_area.color = self.kick_area_color
        self.marker_kick_area.lifetime = rospy.Duration(
            self.lifetime_ball_kick_area)
        self.marker_kick_area.pose.orientation.w = 1.0

        # init subscribers
        rospy.Subscriber("debug/viz_ball",
                         PointStamped,
                         self.ball_cb,
                         queue_size=10)
        rospy.Subscriber("debug/viz_goal",
                         PoseWithCertaintyArray,
                         self.goal_cb,
                         queue_size=10)
        rospy.Subscriber("debug/viz_ball_kick_area",
                         String,
                         self.kick_area_cb,
                         queue_size=10)

        # load kick area info
        kick_max_x = rospy.get_param('behavior/body/kick_max_x')
        kick_max_y = rospy.get_param('behavior/body/kick_max_y')
        kick_min_x = rospy.get_param('behavior/body/kick_min_x')
        kick_min_y = rospy.get_param('behavior/body/kick_min_y')
        self.kick_area_info = [kick_max_x, kick_max_y, kick_min_x, kick_min_y]

        # init tf listener for ball_kick_area_viz
        self.tfBuffer = tf2_ros.Buffer()
        listener = tf2_ros.TransformListener(self.tfBuffer)

        self.last_received_kick_info = rospy.Time.now()

        self.kick_area()
        rospy.spin()
Esempio n. 16
0
def compute_grasps_handle(req):
    tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0))
    tf_listener = tf2_ros.TransformListener(tf_buffer)
    print("Request for Grasp compute recieved by graspnet!!")
    #return AddTwoIntsResponse(req.a + req.b)

    ## Subscribers ##
    #camera_info_topic = '/panda/camera/depth/camera_info'
    #depth_img_topic = '/panda/camera/depth/image_raw'
    #rgb_img_topic ='/panda/camera/color/image_raw'
    cloud_topic = '/transformed_cloud_camLink'

    cloud_msg = rospy.wait_for_message(cloud_topic, PointCloud2)
    pc = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(cloud_msg)
    pc_colors=100*np.ones((pc.shape[0],3))

## output data info ##
    print('Total number of points: %s', pc.shape)
    print('HighestPoint %s', np.nanmax(pc[:,2]))
    print('LowestPoint %s', np.nanmin(pc[:,2]))
    print('Farthest Point %s', np.nanmax(pc[:,0]))
    print('Nearest Point %s', np.nanmin(pc[:,0]))
    #print('Total number of points above the table: %s', pc.shape)

    # Smoothed pc comes from averaging the depth for 10 frames and removing
    # the pixels with jittery depth between those 10 frames.
    #object_pc = data['smoothed_object_pc']

    #points_obj = xyzrgb_array_to_pointcloud2(object_pc, pc_colors, stamp = rospy.Time.now(), frame_id = 'world')
    #pub2.publish(points_obj)
    ## Segment out the points that fit the table plane
    cloud_seg = pcl_helper.plane_seg(pc)
    #cloud_arr = cloud_seg.to_array()
    #cloud_colors_arr =100*np.ones((cloud_arr.shape[0],3))

    clc = cloud_clustering(cloud_seg)
    clc.euclid_cluster()
    cloud = clc.clusters[0]     # Run grasp-detection only on the closest cloud cluster

    points = cloud.to_array()
    cloud_colors =100*np.ones((points.shape[0],3))
    ## Grasp-Estimation ##

    latents = estimator.sample_latents()
    generated_grasps, generated_scores, _ = estimator.predict_grasps(
        sess,
        points,
        latents,
        num_refine_steps=cfg.num_refine_steps,
    )


    ## Sorting grasps ##
    scores_np = np.asarray(generated_scores)
    sorting = np.argsort(-scores_np)    #Negative sign is for descending-order sorting
    sorted_scores = scores_np[sorting]

    grasps_np = np.asarray(generated_grasps)
    sorted_grasps = grasps_np[sorting]
    print('Total generated grasps '+str(len(sorted_grasps.tolist())))
    filtered_grasps, filtered_scores, grasp_msgs = filter_grasps(sorted_grasps)
    print('Filtered grasps '+str(len(filtered_grasps.tolist())))
    try:
        tf_stamped = tf_buffer.lookup_transform('world', 'panda_camera_link' , rospy.Time(0))

    except Exception as inst:
        exc_type, exc_obj, exc_tb = sys.exc_info()
        print('exception: '+str(inst)+' in '+ str(exc_tb.tb_lineno))

    # Transform the grasps to world frame - which is the ref. frame for Panda robot
    transformed_grasps = transform_poses(filtered_grasps.tolist(),tf_stamped)

    for i, grasp in enumerate(transformed_grasps):
        if grasp.position.z < 0.45: #predefined table height
            transformed_grasps.pop(i)

    print('Transformed grasps above the table '+str(len(transformed_grasps)))
    points_obj = pcl_helper.xyzrgb_array_to_pointcloud2(points, cloud_colors, stamp = rospy.Time.now(), frame_id = 'panda_camera_link')
    pub2.publish(points_obj)
    pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id='world'), poses = transformed_grasps))
    return PoseArray(header=Header(stamp=rospy.Time.now(), frame_id='world'), poses = transformed_grasps)
Esempio n. 17
0
    def __init__(self):
        # self.br = tf2_ros.TransformBroadcaster()
        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)

        rospy.wait_for_service('add_compound')
        self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)

        self.spawn_frame = rospy.get_param("~frame", "spawn_frame")
        self.ts = TransformStamped()
        self.ts.header.frame_id = "map"
        self.ts.child_frame_id = self.spawn_frame
        self.ts.transform.rotation.w = 1.0
        self.timer = rospy.Timer(rospy.Duration(0.1), self.update)

        self.count = 0
        self.server = InteractiveMarkerServer("body_spawn")
        self.im = InteractiveMarker()
        self.im.header.frame_id = self.spawn_frame
        self.im.name = "body_spawner"
        self.im.description = "Spawn a new body"

        self.menu_handler = MenuHandler()

        # TODO(lucasw) click and drag resizing will be best served
        # by and interactive marker child that has a frame tf defined
        # by the parent.

        menu = InteractiveMarkerControl()
        menu.interaction_mode = InteractiveMarkerControl.MENU
        menu.description = "spawn"
        menu.name = "spawn_menu"

        box = Marker()
        box.header.frame_id = self.spawn_frame
        box.type = Marker.CUBE
        box.scale.x = 0.5
        box.scale.y = 0.5
        box.scale.z = 0.5
        box.color.r = 0.8
        box.color.g = 0.8
        box.color.b = 0.8
        box.color.a = 1.0
        box.frame_locked = True
        menu.markers.append(box)
        self.im.controls.append(menu)

        self.menu_handler.insert("spawn", callback=self.process_feedback)
        # TODO(lucasw) have a submenu that is updated with a list
        # of all tf frames, and selecting one will cause that tf to
        # be the parent of this marker.

        self.server.insert(self.im, self.process_feedback)
        self.server.applyChanges()
        # TODO(lucasw) what does this do?
        self.menu_handler.apply(self.server, self.im.name)

        # resize x
        self.resize_x_server = InteractiveMarkerServer("body_spawn/resize_x")
        self.resize_x_im = InteractiveMarker()
        self.resize_x_im.header.frame_id = self.spawn_frame
        self.resize_x_im.name = "body_spawner_resize_x"
        self.resize_x_im.description = "resize x of new body"

        self.resize_x = InteractiveMarkerControl()
        self.resize_x.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        self.resize_x.name = "resize_x"
        self.resize_x.orientation.w = 1
        self.resize_x.orientation.x = 1
        self.resize_x.orientation.y = 0
        self.resize_x.orientation.z = 0

        arrow = Marker()
        arrow.type = Marker.ARROW
        # arrow.pose.position.x = box.scale.x * 0.5
        arrow.scale.x = 0.8
        arrow.scale.y = 0.3
        arrow.scale.z = 0.3
        arrow.color.r = 1.0
        arrow.color.g = 0.2
        arrow.color.b = 0.15
        arrow.color.a = 1.0
        arrow.frame_locked = True
        self.resize_x.markers.append(arrow)

        self.resize_x_im.controls.append(self.resize_x)
        self.resize_x_server.insert(self.resize_x_im, self.process_resize_feedback)
        self.resize_x_server.applyChanges()

        # resize y
        self.resize_y_server = InteractiveMarkerServer("body_spawn/resize_y")
        self.resize_y_im = InteractiveMarker()
        self.resize_y_im.header.frame_id = self.spawn_frame
        self.resize_y_im.name = "body_spawner_resize_y"
        self.resize_y_im.description = "resize y of new body"

        self.resize_y = InteractiveMarkerControl()
        self.resize_y.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        self.resize_y.name = "resize_y"
        self.resize_y.orientation.w = 1
        self.resize_y.orientation.x = 0
        self.resize_y.orientation.y = 0
        self.resize_y.orientation.z = 1

        arrow_y = copy.deepcopy(arrow)
        arrow_y.color.r = 0.2
        arrow_y.color.g = 1.0
        arrow_y.color.b = 0.15
        arrow_y.pose.orientation = self.resize_y.orientation
        self.resize_y.markers.append(arrow_y)
        self.resize_y_im.controls.append(self.resize_y)
        self.resize_y_server.insert(self.resize_y_im, self.process_resize_feedback)
        self.resize_y_server.applyChanges()

        # resize y
        self.resize_z_server = InteractiveMarkerServer("body_spawn/resize_z")
        self.resize_z_im = InteractiveMarker()
        self.resize_z_im.header.frame_id = self.spawn_frame
        self.resize_z_im.name = "body_spawner_resize_z"
        self.resize_z_im.description = "resize z of new body"

        self.resize_z = InteractiveMarkerControl()
        self.resize_z.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        self.resize_z.name = "resize_z"
        self.resize_z.orientation.w = 1
        self.resize_z.orientation.x = 0
        self.resize_z.orientation.y = 1
        self.resize_z.orientation.z = 0

        arrow_z = copy.deepcopy(arrow)
        arrow_z.color.r = 0.2
        arrow_z.color.g = 0.04
        arrow_z.color.b = 1.0
        arrow_z.pose.orientation = self.resize_z.orientation
        self.resize_z.markers.append(arrow_z)
        self.resize_z_im.controls.append(self.resize_z)
        self.resize_z_server.insert(self.resize_z_im, self.process_resize_feedback)
        self.resize_z_server.applyChanges()

        # add linear impulse
        self.linear_vel_server = InteractiveMarkerServer("body_spawn/linear_vel")
        self.linear_vel_im = InteractiveMarker()
        self.linear_vel_im.header.frame_id = self.spawn_frame
        self.linear_vel_im.name = "body_spawner_linear_vel"
        self.linear_vel_im.description = "add impulse to body"

        self.linear_vel_pt = [0, 0, 0, 0]
        self.linear_vel_control = InteractiveMarkerControl()
        self.linear_vel_control.interaction_mode = InteractiveMarkerControl.MOVE_3D
        self.linear_vel_control.name = "linear_vel"
        self.linear_vel_control.orientation.w = 1
        self.linear_vel_control.orientation.x = 0
        self.linear_vel_control.orientation.y = 1
        self.linear_vel_control.orientation.z = 0

        box = Marker()
        box.header.frame_id = self.spawn_frame
        box.type = Marker.CUBE
        box.scale.x = 0.1
        box.scale.y = 0.1
        box.scale.z = 0.1
        box.color.r = 0.8
        box.color.g = 0.8
        box.color.b = 0.2
        box.color.a = 1.0
        box.frame_locked = True
        self.linear_vel_control.markers.append(box)

        self.linear_vel_im.controls.append(self.linear_vel_control)
        self.linear_vel_server.insert(self.linear_vel_im, self.process_vel_feedback)
        self.linear_vel_server.applyChanges()

        self.marker_pub = rospy.Publisher("/body_spawn/misc_markers", Marker, queue_size=1)
        self.linear_vel_marker = Marker()
        self.linear_vel_marker.header.frame_id = self.spawn_frame
        self.linear_vel_marker.type = Marker.LINE_LIST
        self.linear_vel_marker.scale.x = 0.07
        self.linear_vel_marker.scale.y = 0.07
        self.linear_vel_marker.scale.z = 0.07
        self.linear_vel_marker.color.r = 0.8
        self.linear_vel_marker.color.g = 0.8
        self.linear_vel_marker.color.b = 0.2
        self.linear_vel_marker.color.a = 1.0
        self.linear_vel_marker.frame_locked = True
        pt = Point()
        self.linear_vel_marker.points.append(pt)
        pt = Point()
        self.linear_vel_marker.points.append(pt)
        self.marker_pub.publish(self.linear_vel_marker)
Esempio n. 18
0
    def __init__(self, yaml_config_file="turtlebot3_world_mapping.yaml"):
        """
        This Task Env is designed for having two TurtleBot3 robots in the turtlebot3 world closed room with columns.

        It will learn how to move around without crashing.
        """
        # This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there
        ros_ws_abspath = rospy.get_param("/turtlebot3/ros_ws_abspath", None)
        if os.environ.get('ROS_WS') != None:
            ros_ws_abspath = os.environ.get('ROS_WS')

        assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
        assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
                                               " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
                                               "/src;cd " + ros_ws_abspath + ";catkin_make"

        # Load Params from the desired Yaml file
        LoadYamlFileParamsTest(rospackage_name="openai_ros",
                               rel_path_from_package_to_file="src/openai_ros/task_envs/turtlebot3_my_envs/config",
                               yaml_file_name=yaml_config_file)


        # Depending on which environment we're in, decide to launch Gazebo with or without GUI.
        gazebo_launch_file = "start_empty_tb3_world.launch"

        if os.environ.get('ENV') == 'deploy' or os.environ.get('ENV') == 'dev-no-gazebo':
            gazebo_launch_file = "start_empty_tb3_world_no_gui.launch"

        ROSLauncher(rospackage_name="coop_mapping",
                    launch_file_name=gazebo_launch_file,
                    ros_ws_abspath=ros_ws_abspath)

        # Here we will add any init functions prior to starting the MyRobotEnv
        super(TurtleBot3WorldMapping2RobotsEnv, self).__init__(ros_ws_abspath,
                                                               ros_launch_file_package="coop_mapping",
                                                               ros_launch_file_name="spawn_2_robots.launch")

        ### ACTIONS
        # Only variable needed to be set here
        self.number_actions = rospy.get_param('/turtlebot3/n_actions')
        self.number_robots = len(self.robot_namespaces)  # should be 2

        # 3x3 possible actions (a % 3 -> robot 1 action, a / 3 -> robot 2 action)
        self.action_space = spaces.Discrete(
            pow(self.number_actions, self.number_robots))

        # We set the reward range, which is not compulsory but here we do it.
        self.reward_range = (-np.inf, np.inf)

        ### OBSERVATIONS
        self.linear_forward_speed = rospy.get_param(
            '/turtlebot3/linear_forward_speed')
        self.linear_turn_speed = rospy.get_param(
            '/turtlebot3/linear_turn_speed')
        self.angular_speed = rospy.get_param('/turtlebot3/angular_speed')
        self.init_linear_forward_speed = rospy.get_param(
            '/turtlebot3/init_linear_forward_speed')
        self.init_linear_turn_speed = rospy.get_param(
            '/turtlebot3/init_linear_turn_speed')

        self.new_ranges = rospy.get_param('/turtlebot3/new_ranges')
        self.min_range = rospy.get_param('/turtlebot3/min_range')
        self.max_laser_value = rospy.get_param('/turtlebot3/max_laser_value')
        self.min_laser_value = rospy.get_param('/turtlebot3/min_laser_value')

        """
        An observation is a MultiDiscrete element, with 4 components.
        
        1. LaserScan rays component with R integers, where R is the number of laser scan 
        rays we are using for observation (one for each robot).
            - Each value represents the distance to an obstacle rounded to the nearest integer (in meteres).
            
        2. Position information with 2 integers (x, y) (one for each robot).
            - Each value represents the position of the robot along a normalized axis, rouned to the nearest integer.

        3. Rotation information with 1 integer (rotation along the z axis) (one for each robot).
            - Each value represents the orientation in a normalized scale, rounded to the nearest integer.

        4. Simplified map exploration with NxN integers, where N is the dimension of the matrix 
        that portrays the level of exploration in the map (one for BOTH robots).
            - Each value represents the average number of pixels explored (-1 is unexplored, 1 is explored). 
            The value is normalized and then rounded to the nearest integer.
        """

        # We create two arrays based on the binary values that will be assigned
        # In the discretization method.
        laser_scan = self.laser_scan[self.robot_namespaces[0]]
        num_laser_readings = len(laser_scan.ranges)/self.new_ranges
        high = np.full((num_laser_readings), self.max_laser_value)
        low = np.full((num_laser_readings), self.min_laser_value)

        self.position_num_discrete_values = 40

        self.rotation_num_discrete_values = 6

        self.simplified_grid_dimension = 4
        self.simplified_grid_num_discrete_values = 40

        laser_scan_component_shape = [
            round(self.max_laser_value)] * (self.new_ranges * self.number_robots)

        position_component_shape = [
            self.position_num_discrete_values] * (2 * self.number_robots)

        rotation_component_shape = [
            self.rotation_num_discrete_values] * (1 * self.number_robots)

        map_exploration_component_shape = [
            self.simplified_grid_num_discrete_values] * self.simplified_grid_dimension * self.simplified_grid_dimension

        multi_discrete_shape = list(itertools.chain(laser_scan_component_shape,
                                                    position_component_shape,
                                                    rotation_component_shape,
                                                    map_exploration_component_shape))

        self.observation_space = spaces.MultiDiscrete(
            multi_discrete_shape)

        rospy.loginfo("ACTION SPACES TYPE===>"+str(self.action_space))
        rospy.loginfo("OBSERVATION SPACES TYPE===>" +
                      str(self.observation_space))

        # Rewards

        self.no_crash_reward_points = rospy.get_param(
            "/turtlebot3/no_crash_reward_points")
        self.crash_reward_points = rospy.get_param(
            "/turtlebot3/crash_reward_points")
        self.exploration_multi_factor = rospy.get_param(
            "/turtlebot3/exploration_multi_factor")

        self.cumulated_steps = 0.0

        # Init dictionary for both robots actions
        self.last_action = {}

        # Set the logging system
        rospack = rospkg.RosPack()
        pkg_path = rospack.get_path('coop_mapping')

        # Control variables for launching nodes from .launch files
        self._gmapping_launch_file = pkg_path + os.path.sep + \
            'launch' + os.path.sep + 'init_2_robots_mapping.launch'
        self._gmapping_running = False
        self._gmapping_launch = None

        self._map_merge_launch_file = pkg_path + os.path.sep + \
            'launch' + os.path.sep + 'init_2_robots_multi_map_merge.launch'
        self._map_merge_running = False
        self._map_merge_launch = None

        self._hector_saver_launch_file = pkg_path + os.path.sep + \
            'launch' + os.path.sep + 'init_2_robots_hector_saver.launch'
        self._hector_saver_running = False
        self._hector_saver_launch = None

        # Variables for map comparison
        self.map_data = None
        self._num_white_pixels_to_explore = get_number_of_almost_white_pixels(self.actual_map_file)

        # The minimum difference that has been observed
        self.current_min_map_difference = None

        # The thresholds to calculate map exploration
        self.threshold_occupied = 65
        self.threshold_free = 25

        # The area in pixels that has been explored
        self.previous_max_explored_area = None
        self.current_max_explored_area = None

        # Start subscriber to /map to save it to file
        self._map_file_name = "/tmp/ros_merge_map"
        self._map_updated_after_action = False
        rospy.Subscriber('map', OccupancyGrid, self._map_callback)

        # Logging episode information
        self._first_episode = True

        # TF listener to get robots position
        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
    def __init__(self, node_name):

        self.debug = False

        # Initialize DTROS Parent Class
        super(EncoderLocalization, self).__init__(node_name=node_name,
                                                  node_type=NodeType.GENERIC)

        self.veh_name = rospy.get_namespace().strip("/")
        self.log(f"Using vehicle name {self.veh_name}")

        # Get Baseline and Radius
        self.baseline, self.radius = self.get_calib_params()
        self.log(
            f'Parameters: baseline = {self.baseline}, radius = {self.radius}')

        # Initialize Encoder Variables
        self.encoder_received = False
        self.encoder_resolution = 135

        self.initialised_ticks_left = False
        self.total_ticks_left = 0
        self.initial_ticks_left = 0
        self.wheel_distance_left = 0.0

        self.initialised_ticks_right = False
        self.total_ticks_right = 0
        self.initial_ticks_right = 0
        self.wheel_distance_right = 0.0

        # Initialize Estimation Variables (in map reference frame)
        self.theta = np.pi
        self.x = 1.0
        self.y = 0.0

        # Initialize Transform Message
        self.transform_msg = TransformStamped()  #published in self.run()
        self.transform_msg.header.frame_id = "map"  #parent frame
        self.transform_msg.child_frame_id = "encoder_baselink"
        self.transform_msg.transform.translation.z = 0.0  # project to ground

        # Initialize TF Broadcaster
        self.broadcaster = tf2_ros.TransformBroadcaster()

        # Initialize TF Listener
        self.tfBuffer = tf2_ros.Buffer()
        self.listener = tf2_ros.TransformListener(self.tfBuffer)

        # Initialize Publishers
        pub_topic = f'/{self.veh_name}/encoder_localization/transform'
        self.pub_transform = rospy.Publisher(pub_topic,
                                             TransformStamped,
                                             queue_size=10)

        # Initialize Subscribers
        left_encoder_topic = f'/{self.veh_name}/left_wheel_encoder_node/tick'
        right_encoder_topic = f'/{self.veh_name}/right_wheel_encoder_node/tick'
        self.sub_encoder_ticks_left = rospy.Subscriber(
            left_encoder_topic,
            WheelEncoderStamped,
            self.cb_encoder_to_transform,
            callback_args="left")
        self.sub_encoder_ticks_right = rospy.Subscriber(
            right_encoder_topic,
            WheelEncoderStamped,
            self.cb_encoder_to_transform,
            callback_args="right")

        # Initialize Services
        self.serv_reset = rospy.Service(
            f'/{self.veh_name}/encoder_localization/reset', Trigger,
            self.reset)
        self.serv_update_map_frame = rospy.Service(
            f'/{self.veh_name}/encoder_localization/update_map_frame', Trigger,
            self.update_map_frame)

        # Broadcast and publish initial transform
        self.latest_timestamp = rospy.Time.now()
        self.update_transform()
        self.broadcaster.sendTransform(self.transform_msg)
        self.pub_transform.publish(self.transform_msg)

        self.log("Initialized.")
def main():
    # init node
    rospy.init_node("grasp_commander")

    # ========== publisher to jamming gripper ========== #
    grasp_pub = rospy.Publisher('/jamming_grasp', Int32, queue_size=1)
    
    # ========== Moveit init ========== #
    # moveit_commander init
    robot = moveit_commander.RobotCommander()
    arm = moveit_commander.MoveGroupCommander("arm")
    arm_initial_pose = arm.get_current_pose().pose
    target_pose = geometry_msgs.msg.Pose()

    # ========== TF Lister ========== #
    tf_buffer = tf2_ros.Buffer()
    tf_listner = tf2_ros.TransformListener(tf_buffer)
    get_tf_flg = False
    # Get target TF
    while not get_tf_flg :
        try :
            trans = tf_buffer.lookup_transform('world', 'ar_marker', rospy.Time(0),rospy.Duration(10))
            print "world -> ar_marker"
            print trans.transform
            print "Target Place & Pose"
            # Go to up from target
            target_pose.position.x = trans.transform.translation.x
            target_pose.position.y = trans.transform.translation.y + 0.03
            target_pose.position.z = trans.transform.translation.z + 0.42
            q = (trans.transform.rotation.x,
                 trans.transform.rotation.y,
                 trans.transform.rotation.z,
                 trans.transform.rotation.w)
            (roll,pitch,yaw) = tf.transformations.euler_from_quaternion(q)
            # roll -= math.pi/6.0
            pitch += math.pi/2.0
            # yaw += math.pi/4.0
            tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
            target_pose.orientation.x = tar_q[0]
            target_pose.orientation.y = tar_q[1]
            target_pose.orientation.z = tar_q[2]
            target_pose.orientation.w = tar_q[3]
            print target_pose
            arm.set_pose_target(target_pose)
            arm.go()
            arm.clear_pose_targets()
            # rospy.sleep(1)
            
            # Get Grasp
            # waypoints = []
            
            # waypoints.append(arm.get_current_pose().pose)

            # wpose = geometry_msgs.msg.Pose()
            # wpose.orientation.w = 1.0
            # wpose.position.x = waypoints[0].position.x
            # wpose.position.y = waypoints[0].position.y - 0.085
            # wpose.position.z = waypoints[0].position.z
            # waypoints.append(copy.deepcopy(wpose))
            # (plan, fraction) = arm.compute_cartesian_path(waypoints, 0.005, 0.0, False)

            
            target_pose.position.x = trans.transform.translation.x
            target_pose.position.y = trans.transform.translation.y + 0.03
            target_pose.position.z = trans.transform.translation.z + 0.283
            target_pose.orientation = target_pose.orientation
            print target_pose
            arm.set_pose_target(target_pose)
            arm.go()
            arm.clear_pose_targets()

            # Grasp
            grasp_pub.publish(1)
            print "!! Grasping !!"
            rospy.sleep(1.0)

            # Start Return
            # waypoints = []
            
            # waypoints.append(arm.get_current_pose().pose)
            
            # wpose = geometry_msgs.msg.Pose()
            # wpose.orientation.w = 1.0
            # wpose.position.x = waypoints[0].position.x
            # wpose.position.y = waypoints[0].position.y + 0.085
            # wpose.position.z = waypoints[0].position.z
            # waypoints.append(copy.deepcopy(wpose))
            # (plan, fraction) = arm.compute_cartesian_path(waypoints, 0.005, 0.0, False)
            target_pose.position.x = trans.transform.translation.x
            target_pose.position.y = trans.transform.translation.y + 0.03
            target_pose.position.z = trans.transform.translation.z + 0.42
            target_pose.orientation = target_pose.orientation
            print target_pose
            arm.set_pose_target(target_pose)
            arm.go()
            arm.clear_pose_targets()
            

            # Go to Home Position
            target_pose.position.x = trans.transform.translation.x
            target_pose.position.y = -trans.transform.translation.y
            target_pose.position.z = trans.transform.translation.z + 0.42
            target_pose.orientation = target_pose.orientation
            arm.set_pose_target(target_pose)
            arm.go()
            arm.clear_pose_targets()

            
            target_pose.position.x = trans.transform.translation.x
            target_pose.position.y = -trans.transform.translation.y
            target_pose.position.z = trans.transform.translation.z + 0.30
            target_pose.orientation = target_pose.orientation
            arm.set_pose_target(target_pose)
            arm.go()
            arm.clear_pose_targets()
            
            
            # Release
            print " !! Release !!"
            grasp_pub.publish(0)
            rospy.sleep(2)
            get_tf_flg = True

            target_pose.position.x = 0.503
            target_pose.position.y = 0
            target_pose.position.z = 0.6563
            target_pose.orientation.x = 0
            target_pose.orientation.y = 0
            target_pose.orientation.z = 0
            target_pose.orientation.w = 1
            arm.set_pose_target(target_pose)
            arm.go()
            arm.clear_pose_targets()
            
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) :
            continue
Esempio n. 21
0
    def __init__(self):

        map_frame = rospy.get_param('~map_frame', 'map')  # map frame_id
        odom_frame = rospy.get_param('~odom_frame', 'odom')
        meas_model_as = rospy.get_param('~mbes_as',
                                        '/mbes_sim_server')  # map frame_id
        auv_odom_top = rospy.get_param("~odometry_topic", '/odom')
        auv_mbes_top = rospy.get_param("~mbes_pings_topic", '/mbes')
        auv_exp_mbes_top = rospy.get_param("~expected_mbes_topic",
                                           '/expected_mbes')
        pf_pose_top = rospy.get_param("~average_pose_topic", '/avg_pose')

        self.auv_mbes = message_filters.Subscriber(auv_mbes_top, PointCloud2)
        self.exp_mbes = message_filters.Subscriber(auv_exp_mbes_top,
                                                   PointCloud2)
        self.auv_pose = message_filters.Subscriber(auv_odom_top, Odometry)
        #  self.pf_pose = message_filters.Subscriber(pf_pose_top, PoseWithCovarianceStamped)
        self.ts = message_filters.ApproximateTimeSynchronizer(
            [self.auv_mbes, self.exp_mbes, self.auv_pose],
            10,
            slop=10.0,
            allow_headerless=False)

        # Initialize tf listener
        tfBuffer = tf2_ros.Buffer()
        tf2_ros.TransformListener(tfBuffer)
        try:
            rospy.loginfo("Waiting for transforms")
            mbes_tf = tfBuffer.lookup_transform('hugin/base_link',
                                                'hugin/mbes_link',
                                                rospy.Time(0),
                                                rospy.Duration(20.))
            self.base2mbes_mat = self.matrix_from_tf(mbes_tf)

            m2o_tf = tfBuffer.lookup_transform(map_frame, odom_frame,
                                               rospy.Time(0),
                                               rospy.Duration(20.))
            self.m2o_mat = self.matrix_from_tf(m2o_tf)

            rospy.loginfo("Transforms locked - Car detector node")
        except:
            rospy.loginfo(
                "ERROR: Could not lookup transform from base_link to mbes_link"
            )

        # Register cb after tf is locked
        self.ts.registerCallback(self.pingCB)

        plt.ion()
        plt.show()
        self.scale = 1
        self.max_height = 100  # TODO: this should equal the n beams in ping
        self.new_msg = False
        first_msg = True
        self.waterfall = []

        while not rospy.is_shutdown():
            if self.new_msg:
                # Blob detection to find the car on waterfall image
                #Visualize
                if len(self.waterfall) == self.max_height:
                    waterfall_detect = self.car_detection(
                        np.array(self.waterfall), self.scale)
                    plt.imshow(np.array(waterfall_detect),
                               norm=plt.Normalize(0., 60.),
                               cmap='gray',
                               aspect='equal')
                else:
                    plt.imshow(np.array(self.waterfall),
                               norm=plt.Normalize(0., 60.),
                               cmap='gray',
                               aspect='equal')
                if first_msg:
                    first_msg = False
                    plt.colorbar()
                    plt.title("Bathymetry difference (m)")

                plt.pause(0.01)

            self.new_msg = False
    def __init__(self):
        self.pose_cam_1_sub = message_filters.Subscriber(
            POSE_CAM_1_TOPIC, CustomMarkerArray)
        self.pose_cam_2_sub = message_filters.Subscriber(
            POSE_CAM_2_TOPIC, CustomMarkerArray)
        self.pose_cam_4_sub = message_filters.Subscriber(
            POSE_CAM_4_TOPIC, CustomMarkerArray)

        self.tfBuffer = tf2_ros.Buffer()
        self.listener = tf2_ros.TransformListener(self.tfBuffer)

        self.pub = rospy.Publisher(FUSION_POSES_TOPIC,
                                   MarkerArray,
                                   queue_size=10)

        self.check_cam_1_pub = rospy.Publisher(FUSION_CHECK_CAM_1_POSES_TOPIC,
                                               MarkerArray,
                                               queue_size=10)
        self.check_cam_2_pub = rospy.Publisher(FUSION_CHECK_CAM_2_POSES_TOPIC,
                                               MarkerArray,
                                               queue_size=10)
        self.check_cam_4_pub = rospy.Publisher(FUSION_CHECK_CAM_4_POSES_TOPIC,
                                               MarkerArray,
                                               queue_size=10)

        self.sync = message_filters.ApproximateTimeSynchronizer(
            [self.pose_cam_1_sub, self.pose_cam_2_sub, self.pose_cam_4_sub],
            10, 0.1)
        self.sync.registerCallback(self.poses_callback)

        self.occluded_points_cam_2 = [4, 6, 8]
        self.occluded_points_cam_4 = [3, 5, 7]
        # Kalman filter parameters
        f = KalmanFilter(dim_x=6, dim_z=3)
        self.keypoints = 13
        self.initialized = [False for k in range(self.keypoints)]
        self.dt = 1 / 15.0  #15 fps

        f.inv = np.linalg.pinv
        # State transition matrix
        f.F = np.array([[1., 0., 0., self.dt, 0., 0.],
                        [0., 1., 0., 0., self.dt, 0.],
                        [0., 0., 1., 0., 0., self.dt],
                        [0., 0., 0., 1., 0., 0.], [0., 0., 0., 0., 1., 0.],
                        [0., 0., 0., 0., 0., 1.]])
        # Measurement function
        f.H = np.array([[1., 0., 0., 0., 0., 0.], [0., 1., 0., 0., 0., 0.],
                        [0., 0., 1., 0., 0., 0.]])
        # Process uncertainty/noise
        f.Q = np.identity(6) * 1e-4

        # Measurement uncertainty/noise
        self.min_cov = 0.0025
        self.max_cov = 0.04
        self.steepness = 20
        self.steep_point = 0.6
        f.R = np.identity(3) * (self.min_cov + self.max_cov) / 2
        f.R[2, 2] = 0.05

        # Distance threshold
        if USE_MAHALANOBIS:
            self.distance_threshold = 60
        else:
            self.distance_threshold = 0.4
        # Covariance matrix
        f.P = np.identity(6) * 100

        # Create the KalmanFilter array
        self.kf_array = [copy.deepcopy(f) for kp in range(self.keypoints)]

        # Last-update time array
        self.time_since_update = [time.time() for kp in range(self.keypoints)]

        self.time_threshold = 0.25  # in secs

        # Auxiliar variables (for visualization)
        self.cam_1_check_covariances = [
            np.zeros((3, 3)) for kp in range(self.keypoints)
        ]
        self.cam_2_check_covariances = [
            np.zeros((3, 3)) for kp in range(self.keypoints)
        ]
        self.cam_4_check_covariances = [
            np.zeros((3, 3)) for kp in range(self.keypoints)
        ]
Esempio n. 23
0
 def setup(self):
     self.tfBuffer = tf2_ros.Buffer()
     self.tfListener = tf2_ros.TransformListener(self.tfBuffer)
Esempio n. 24
0
    def __init__(self):
        action_ns = rospy.get_param('~action_ns', '')

        # Monitor manual control and cancel any actions if user presses any button
        joy_safety_topic = rospy.get_param('~joy_safety_topic', '/drone/joy')

        self.distance_threshold = rospy.get_param('~distance_threshold', 0.10)
        self.yaw_threshold = rospy.get_param('~yaw_threshold',
                                             math.pi / 180.0 * 10.0)

        self.robot_desired_pose_topic = rospy.get_param(
            '~robot_desired_pose_topic', 'desired_pose')
        robot_odom_topic = rospy.get_param('~robot_odom_topic', '/drone/odom')

        robot_state_topic = rospy.get_param('~robot_state_topic',
                                            '/drone/flight_state')

        takeoff_service = rospy.get_param('~takeoff_service',
                                          '/drone/safe_takeoff')
        land_service = rospy.get_param('~land_service', '/drone/safe_land')
        feedback_service = rospy.get_param('~feedback_service',
                                           '/drone/give_feedback')

        target_source_topic = rospy.get_param('~target_source_topic',
                                              '/drone/target_source')
        set_pose_topic = rospy.get_param('~set_pose_topic', '/drone/set_pose')
        set_xy_service = rospy.get_param('~set_xy_service', '/drone/set_xy')

        self.robot_desired_pose = PoseStamped()
        self.robot_current_pose = PoseStamped()

        self.tf_buff = tf2_ros.Buffer()
        self.tf_ls = tf2_ros.TransformListener(self.tf_buff)

        # Action client to self, see below
        self.goto_client = actionlib.SimpleActionClient(
            action_ns + '/goto_action', GoToAction)

        # Action servers
        self.goto_server = ActionHub.add_server(action_ns + '/goto_action',
                                                GoToAction, self.execute_goto,
                                                False)
        self.goto_server.start()
        # self.shape_server = ActionHub.add_server(action_ns + '/followshape_action', FollowShapeAction, self.execute_shape, False)
        # self.shape_server.start()
        self.precise_shape_server = ActionHub.add_server(
            action_ns + '/precise_shape_action', FollowShapeAction,
            self.execute_precise_shape, False)
        self.precise_shape_server.start()

        self.takeoff_server = ActionHub.add_server(
            action_ns + '/takeoff_action', EmptyAction, self.execute_takeoff,
            False)
        self.takeoff_server.start()
        self.land_server = ActionHub.add_server(action_ns + '/land_action',
                                                EmptyAction, self.execute_land,
                                                False)
        self.land_server.start()

        self.feedback_server = ActionHub.add_server(
            action_ns + '/feedback_action', EmptyAction, self.execute_feedback,
            False)
        self.feedback_server.start()

        self.followme_server = ActionHub.add_server(
            action_ns + '/followme_action', FollowMeAction,
            self.execute_followme, False)
        self.followme_server.start()

        self.waypoints_server = ActionHub.add_server(
            action_ns + '/waypoints_action', WaypointsAction,
            self.execute_waypoints, False)
        self.waypoints_server.start()

        self.reset_odom_server = ActionHub.add_server(
            action_ns + '/reset_odom_action', EmptyAction,
            self.execute_reset_odom, False)
        self.reset_odom_server.start()

        self.pub_desired_pose = rospy.Publisher(self.robot_desired_pose_topic,
                                                PoseStamped,
                                                queue_size=10)
        self.sub_odom = rospy.Subscriber(robot_odom_topic, Odometry,
                                         self.robot_odom_cb)

        self.takeoff_svc = rospy.ServiceProxy(takeoff_service,
                                              std_srvs.srv.Trigger)
        self.land_svc = rospy.ServiceProxy(land_service, std_srvs.srv.Trigger)
        self.feedback_svc = rospy.ServiceProxy(feedback_service,
                                               std_srvs.srv.Trigger)

        self.pub_target_source = rospy.Publisher(target_source_topic,
                                                 TargetSource,
                                                 queue_size=1)
        self.pub_set_pose = rospy.Publisher(set_pose_topic, Pose, queue_size=1)
        self.set_xy_service = rospy.ServiceProxy(set_xy_service, SetXY)

        self.last_known_flight_state = FlightState.Landed  #None
        self.sub_flight_state = rospy.Subscriber(robot_state_topic,
                                                 FlightState,
                                                 self.flight_state_cb)

        self.sub_joy_safety = rospy.Subscriber(joy_safety_topic, Joy,
                                               self.joy_safety_cb)

        # self.ros_thread = threading.Thread(target=self.run)
        # self.ros_thread.start()

        rospy.wait_for_service(takeoff_service)
        rospy.wait_for_service(land_service)
        rospy.wait_for_service(feedback_service)
#!/usr/bin/env python

import rospy
import math
from geometry_msgs.msg import Point,PointStamped,PoseStamped
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from actionlib_msgs.msg import *
import actionlib
import tf2_ros
import tf2_geometry_msgs
import utm

rospy.init_node('GPS_goals')

tf_buffer = tf2_ros.Buffer(rospy.Duration(10))
tf_listener = tf2_ros.TransformListener(tf_buffer)

client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
client.wait_for_server()

gps_pub = rospy.Publisher('/GPS_odom_point', PointStamped, queue_size=10, latch=True)

utm_point = PointStamped()
utm_point.header.frame_id = "utm"
odom_point = PointStamped()

goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "odom"
goal.target_pose.pose.orientation.x = 0
goal.target_pose.pose.orientation.y = 0
goal.target_pose.pose.orientation.z = 0
Esempio n. 26
0
def main():
  global tfBuffer, tfListener, stepCounter
  global ROBOT_NAME, RIGHT_FOOT_FRAME_NAME, LEFT_FOOT_FRAME_NAME

  global web
  web = addasteppages.Pages()
  thread.start_new_thread(web.run, ())

  try:
    rospy.init_node('AddAStep')

    if not rospy.has_param('/ihmc_ros/robot_name'):
      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)
      fss_name = "/ihmc_ros/{0}/output/footstep_status".format(ROBOT_NAME)
      fsp_name = "/ihmc_ros/{0}/control/footstep_list".format(ROBOT_NAME)
      atp_name = "/ihmc_ros/{0}/control/arm_trajectory".format(ROBOT_NAME)
      ctp_name = "/ihmc_ros/{0}/control/chest_trajectory".format(ROBOT_NAME)
      pose_name = "/ihmc_ros/{0}/output/robot_pose".format(ROBOT_NAME)

    if rospy.has_param(right_foot_frame_parameter_name) and rospy.has_param(left_foot_frame_parameter_name):
      RIGHT_FOOT_FRAME_NAME = rospy.get_param(right_foot_frame_parameter_name)
      LEFT_FOOT_FRAME_NAME = rospy.get_param(left_foot_frame_parameter_name)

    footStepStatusSubscriber = rospy.Subscriber( fss_name, FootstepStatusRosMessage, recievedFootStepStatus)
    robotPoseSubscriber = rospy.Subscriber( pose_name, Odometry, recievedRobotPose )
    footStepListPublisher = rospy.Publisher( fsp_name, FootstepDataListRosMessage, queue_size=1)
    armTrajectoryPublisher = rospy.Publisher( atp_name, ArmTrajectoryRosMessage, queue_size=1)
    chestTrajectoryPublisher = rospy.Publisher( ctp_name, ChestTrajectoryRosMessage, queue_size=1)

    tfBuffer = tf2_ros.Buffer()
    tfListener = tf2_ros.TransformListener(tfBuffer)

    rate = rospy.Rate(10) # 10hz
    time.sleep(1)

    # make sure the simulation is running otherwise wait
    if footStepListPublisher.get_num_connections() == 0:
      rospy.loginfo('waiting for subsciber...')
      while footStepListPublisher.get_num_connections() == 0:
        rate.sleep()

    stepCounter = 0
    backsent = armsent = footsent = False
    backnote = armnote = footnote = 'Wait '
    while not rospy.is_shutdown():
      done, curtim = monitor()
      print(backnote,footnote,armnote)
      if curtim > 28.0 and not backsent:
        footStepMsg,backnote = createBackStepCmd()
        footStepListPublisher.publish(footStepMsg)
        backsent = True
        print('\a')
      if curtim > 35.0 and not armsent:
        footStepMsg,footnote = createFootStepCmd()
        armMsgL, armMsgR, armnote = createArmCmd()
        armTrajectoryPublisher.publish(armMsgL)
        rospy.sleep(rospy.Duration(0,10000000))
        armTrajectoryPublisher.publish(armMsgR)
        chestMsg, _ = createChestCmd()
        chestTrajectoryPublisher.publish(chestMsg)
        #armTrajectoryPublisher.publish(armMsgR)
        armsent = True
        print('\a')
      if curtim > 40.0 and not footsent:
        footStepListPublisher.publish(footStepMsg)
        footsent = True
        print('\a')
        print('\a')
      if (done):
        print('\a')
        print('\a')
        return
      rate.sleep()

  except rospy.ROSInterruptException:
        pass
Esempio n. 27
0
import tf2_geometry_msgs

import tf2_ros

moveit_commander.roscpp_initialize(sys.argv)

rospy.init_node('points_to_xyz_with_moveit_node')

robot = moveit_commander.RobotCommander()

scene = moveit_commander.PlanningSceneInterface()

group = moveit_commander.MoveGroupCommander("manipulator")

tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0))  # tf buffer length
tf_listener = tf2_ros.TransformListener(tf_buffer)

display_trajectory_publisher = rospy.Publisher(
    '/move_group/display_planned_path',
    moveit_msgs.msg.DisplayTrajectory,
    queue_size=20)

rospy.sleep(1)

group.set_planning_time(10)

group.set_joint_value_target([0, 0, 0, 0, -pi / 2, 0])

group.go()
Esempio n. 28
0
    def __init__(self):
        rospy.init_node("logger", anonymous=True)

        # if true -> show plots on shutdown
        self.show_plots = rospy.get_param("~show_plots", False)
        # if true -> save data with the start time in the name
        self.track_time = rospy.get_param("~track_time", False)
        # path to the output data folder
        self.module_path = rospy.get_param("~output_file")
        # output folder name
        self.output_folder = rospy.get_param("~output_folder")

        self.parent_frame = rospy.get_param("~parent_frame", 'odom')
        self.robot_frame = rospy.get_param("~robot_frame", 'base_link')
        self.kinetic_model_frame = rospy.get_param("~kinetic_model_frame",
                                                   'model_link')
        self.nn_model_frame = rospy.get_param("~nn_model_frame",
                                              'nn_model_link')

        self.timeout = int(rospy.get_param("~timeout", 0))

        self.module_path = self.module_path + self.output_folder
        if self.track_time:
            self.module_path += '/' + str(
                datetime.now().strftime('%Y-%m-%d-%H:%M:%S'))

        # declare tf buffer and listener for working with TF transformations
        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)

        # node init time
        self.init_time = None
        #
        self.prev_time = None
        # flag for the first callback (used in Tf callback)
        self.first_tick = True
        self.fisrt_fill_state = True
        # container for trajectory (/path) fron path_publisher node
        self.trajectory = {'x': [], 'y': []}
        # container for /cmd_vel (input control) from path_follower node
        self.control = {'x': [], 'yaw': []}
        # container for robot state
        self.robot_state = {'x': [], 'y': [], 'yaw': [], 'v': [], 'w': []}
        # container for kinetic model state
        self.kinetic_model_state = {
            'x': [],
            'y': [],
            'yaw': [],
            'v': [],
            'w': []
        }
        # container for nn model state
        self.nn_model_state = {'x': [], 'y': [], 'yaw': [], 'v': [], 'w': []}
        # time spent running the simulator (for automated tests)
        self.delta_time = {'dt': []}
        self.time = {'t': []}
        self.time_spent = 0
        self.current_control = list()
        # declare subscribers
        self.trajectory_sub = rospy.Subscriber("/path", Path,
                                               self.path_callback)
        self.control_sub = rospy.Subscriber('/cmd_vel', Twist,
                                            self.cmd_vel_callback)
        # self.timer_ = rospy.Timer(rospy.Duration(0.033), self.timer_callback)
        self.tf_sub = rospy.Subscriber('/tf', Twist, self.tf_callback)
        if self.timeout > 0:
            rospy.Timer(rospy.Duration(1), self.timeout_callback)

        # set the function that will be executed when shutdown
        rospy.on_shutdown(self.on_shutdown)
Esempio n. 29
0
#!/usr/bin/env python
import math
import rospy
from clever import srv
from mavros_msgs.srv import CommandBool
from std_srvs.srv import Trigger
from geometry_msgs.msg import Point, PoseStamped, Quaternion, TransformStamped
from tf.transformations import quaternion_from_euler, euler_from_quaternion, quaternion_multiply
import tf2_ros

rospy.init_node('cow_despasturer_helper')

tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
rate = rospy.Rate(10.0)
aruco_frame = 'aruco_0'
frame = 'map'

destination_pose = rospy.Publisher('~/destination_pose',
                                   PoseStamped,
                                   queue_size=40)
pose = PoseStamped()
pose.header.frame_id = "map"
trans = TransformStamped()
trans2 = TransformStamped()
trans3 = TransformStamped()
#trans.header.frame_id = "map"
broadcaster = tf2_ros.TransformBroadcaster()
static_bloadcaster = tf2_ros.StaticTransformBroadcaster()

while not rospy.is_shutdown():
Esempio n. 30
0
    def __init__(self, init_trans, update_freq):
        rospy.Subscriber('aruco/markers', MarkerArray, self.update_callback)
        rospy.Subscriber('sign_pose', MarkerArray, self.update_callback)
        #rospy.Subscriber('marker_measurements', MarkerArray, self.update_callback)

        rospy.Subscriber("cf1/pose", PoseStamped, self.cf1_pose_callback)
        rospy.Subscriber("cf1/velocity", TwistStamped, self.cf1_vel_callback)
        self.cf1_pose_cov_pub = rospy.Publisher("cf1/localizatiton/pose_cov",
                                                PoseWithCovarianceStamped,
                                                queue_size=1)

        init_trans.header.frame_id = "map"
        init_trans.child_frame_id = "cf1/odom"
        self.init_trans = init_trans  # remove?
        self.last_transform = init_trans

        self.cf1_pose = None
        self.cf1_vel = None

        self.old_msg = None
        self.measurement_msg = None
        # what translation/rotation variables to use in filtering
        self.filter_config = rospy.get_param("localization/measurement_config")

        self.tf_buf = tf2_ros.Buffer()
        self.tf_lstn = tf2_ros.TransformListener(
            self.tf_buf, queue_size=1)  # should there be a queue_size?
        self.broadcaster = tf2_ros.TransformBroadcaster()
        self.static_broadcaster = tf2_ros.StaticTransformBroadcaster()

        self.update_freq = update_freq
        self.kf = KalmanFilter(
            initial_cov=np.diag(rospy.get_param("localization/initial_cov")),
            R=np.diag(rospy.get_param("localization/process_noise")),
            maha_dist_thres=rospy.get_param("localization/maha_dist_thres"),
            cov_norm_thres=rospy.get_param("localization/cov_norm_thres"),
            delta_t=1.0 / self.update_freq)

        self.maha_dist_thres = rospy.get_param("localization/maha_dist_thres")
        self.cov_norm_thres = rospy.get_param("localization/cov_norm_thres")

        self.converged_pub = rospy.Publisher("cf1/localization/converged",
                                             Bool,
                                             queue_size=1)
        self.measurement_fb_pub = rospy.Publisher(
            "cf1/localization/measurement_feedback",
            Int32MultiArray,
            queue_size=1)

        self.odom_new_pub = rospy.Publisher("cf1/pose/odom_new",
                                            PoseStamped,
                                            queue_size=1)
        self.believed_pub = rospy.Publisher("cf1/pose/believed",
                                            PoseStamped,
                                            queue_size=1)
        self.measured_valid_pub = rospy.Publisher("cf1/pose/measured_valid",
                                                  PoseArray,
                                                  queue_size=1)
        self.measured_invalid_pub = rospy.Publisher(
            "cf1/pose/measured_invalid", PoseArray, queue_size=1)
        self.filtered_pub = rospy.Publisher("cf1/pose/filtered",
                                            PoseStamped,
                                            queue_size=1)

        rospy.Service("cf1/localization/reset_kalman_filter",
                      ResetKalmanFilter, self.reset_kalman_filter)