def set_position(self, position):
     model_state_resp = self.get_model_state(model_name="mobile_base")
     model_state = SetModelState()
     model_state.model_name = "mobile_base"
     model_state.twist = Twist()
     model_state.reference_frame = "world"
     model_state.pose = model_state_resp.pose
     model_state.pose.position.x = position[0]
     model_state.pose.position.y = position[1]
     self.set_model_state(model_state=model_state)
 def position_indicator(self):
     state = SetModelState()
     state.model_name = "indicator"
     pose = Pose()
     pose.position.x = self.center[0]
     pose.position.y = (-1) * self.center[1]
     pose.position.z = (-1) * self.center[2]
     state.pose = pose
     state.twist = Twist()
     state.reference_frame = ""
     self.setModelState(state)
 def set_position(self, position):
     model_state_resp = self.get_model_state(model_name="piano2")
     model_state = SetModelState()
     model_state.model_name = "piano2"
     model_state.pose = model_state_resp.pose
     model_state.twist = Twist()
     model_state.reference_frame = "world"
     model_state.pose.position.x = position.X
     model_state.pose.position.y = position.Y
     model_state.pose.position.z = position.Z
     self.set_model_state(model_state=model_state)
    def teleport(self, location):
        """
        Teleport robot's position.

        location: string
            location name
        """

        if location == 0:
            print("\teleporting to lobby")
        else:
            print("\teleporting to room {}".format(location))
        location = self.numbered_locations[location]

        # get location data
        lx = self.locations[location]["lx"]
        ly = self.locations[location]["ly"]
        az = 0.0
        if "az" in self.locations[location]:
            az = self.locations[location]["az"]

        # send location data to service
        rospy.wait_for_service("/gazebo/set_model_state")

        set_model_state = rospy.ServiceProxy("/gazebo/set_model_state",
                                             SetModelState)

        model_state = SetModelState()

        model_state.model_name = "mobile_base"

        pose = Pose()
        pose.position.x = lx
        pose.position.y = ly

        orientation = quaternion_from_euler(0, 0, az)
        pose.orientation.x = orientation[0]
        pose.orientation.y = orientation[1]
        pose.orientation.z = orientation[2]
        pose.orientation.w = orientation[3]

        model_state.pose = pose

        twist = Twist()
        twist.linear.x = 0.0
        twist.angular.z = 0.0

        model_state.twist = twist

        model_state.reference_frame = "map"

        set_model_state(model_state)

        print("reached {}\n".format(location))
 def set_steering_angle(self, quat):
     model_state_resp = self.get_model_state(model_name="piano2")
     model_state = SetModelState()
     model_state.model_name = "piano2"
     model_state.pose = model_state_resp.pose
     model_state.twist = Twist()
     model_state.reference_frame = "world"
     model_state.pose.orientation.x = quat[0]
     model_state.pose.orientation.y = quat[1]
     model_state.pose.orientation.z = quat[2]
     model_state.pose.orientation.w = quat[3]
     self.set_model_state(model_state=model_state)
 def set_heading_angle(self, heading_angle):
     model_state_resp = self.get_model_state(model_name="mobile_base")
     model_state = SetModelState()
     model_state.model_name = "mobile_base"
     model_state.pose = model_state_resp.pose
     model_state.twist = Twist()
     model_state.reference_frame = "world"
     quat = tf.transformations.quaternion_from_euler(0, 0, heading_angle)
     model_state.pose.orientation.x = quat[0]
     model_state.pose.orientation.y = quat[1]
     model_state.pose.orientation.z = quat[2]
     model_state.pose.orientation.w = quat[3]
     self.set_model_state(model_state=model_state)
 def set_state(self, se3):
     model_state_resp = self.get_model_state(model_name="piano2")
     model_state = SetModelState()
     model_state.model_name = "piano2"
     model_state.pose = model_state_resp.pose
     model_state.twist = Twist()
     model_state.reference_frame = "world"
     model_state.pose.position.x = se3.X
     model_state.pose.position.y = se3.Y
     model_state.pose.position.z = se3.Z
     model_state.pose.orientation.x = se3.q[0]
     model_state.pose.orientation.y = se3.q[1]
     model_state.pose.orientation.z = se3.q[2]
     model_state.pose.orientation.w = se3.q[3]
     self.set_model_state(model_state=model_state)
示例#8
0
 def set_state(self, x, y, s, vx=0, vy=0, vs=0):
     model_state_resp = self.get_model_state(model_name="ackermann_vehicle")
     model_state = SetModelState()
     model_state.model_name = "ackermann_vehicle"
     model_state.pose = model_state_resp.pose
     model_state.twist = Twist()
     model_state.reference_frame = "world"
     model_state.pose.position.x = x
     model_state.pose.position.y = y
     quat = tf.transformations.quaternion_from_euler(0,0,s)
     model_state.pose.orientation.x = quat[0]
     model_state.pose.orientation.y = quat[1]
     model_state.pose.orientation.z = quat[2]
     model_state.pose.orientation.w = quat[3]
     model_state.twist.linear.x = vx
     model_state.twist.linear.y = vy
     model_state.twist.angular.z = vs
     self.set_model_state(model_state=model_state)
示例#9
0
def moveModel(model, position, r, p, y):
    try:
        service = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
        twist = Twist()
        pose = Pose()
        pose.position = position
        #rpy to quaternion:
        quat = quaternion_from_euler(r, p, y)
        pose.orientation.x = quat[0]
        pose.orientation.y = quat[1]
        pose.orientation.z = quat[2]
        pose.orientation.w = quat[3]
        req = SetModelState()
        req.model_name = model
        req.pose = pose
        req.twist = twist
        req.reference_frame = ""

        resp = service(req)

    except rospy.ServiceException, e:
        print("Service call failed: %s" %e)
    def _reset(self):

        if self.proc is not None:
            os.system("rosnode kill /rviz")
            os.system("rosnode kill /move_base")
            os.system("rosnode kill /pcl_filter")
            rospy.sleep(5)
        state = SetModelState()
        state.model_name = 'mobile_base'
        state.pose = self.start
        state.twist = Twist()
        state.reference_frame = ''
        rospy.wait_for_service('/gazebo/set_model_state')
        try:
            self.set_model_state(state)
        except (rospy.ServiceException) as e:
            print("/gazebo/set_model_state service call failed")

        reset = String()
        reset.data = "r"
        self.reset_pub.publish(reset)
        rospy.sleep(3)

        self.goal = Pose()
        self.goal.position.x = random.random() * (
            self.map_size_x_max - self.map_size_x_min) + self.map_size_x_min
        self.goal.position.y = random.random() * (
            self.map_size_y_max - self.map_size_y_min) + self.map_size_x_min
        self.goal.orientation.w = 1
        print(self.goal.position)
        # Unpause simulation to make observation
        rospy.wait_for_service('/gazebo/unpause_physics')
        try:
            #resp_pause = pause.call()
            self.unpause()
        except (rospy.ServiceException) as e:
            print("/gazebo/unpause_physics service call failed")

        #try to initialize SLAM
        vel_cmd = Twist()
        vel_cmd.linear.x = -0.4
        r = rospy.Rate(10)
        count = 0
        status_ = Bool()
        while status_.data is False and count < 10:
            t = time.time()
            if count % 5 is 0:
                vel_cmd.linear.x = -vel_cmd.linear.x
                #rospy.sleep(2)
            while time.time() - t < 2.0:
                self.vel_pub.publish(vel_cmd)
                r.sleep()
            status_ = rospy.wait_for_message('/ORB_SLAM2/Status',
                                             Bool,
                                             timeout=5)
            print(status_)
            count += 1
            rospy.sleep(1)
            # what will happen if SLAM is still not initialized

        if status_ is False:
            state = self._reset()
            return state

        rospy.sleep(2)
        #self.port = os.environ["ROS_PORT_SIM"]
        self.proc = subprocess.Popen([
            "roslaunch", "-p", self.port,
            "/home/rrc/gym-gazebo/gym_gazebo/envs/assets/launch/navigation_stack_rl.launch"
        ])
        rospy.sleep(5)
        goal_published = PoseStamped()
        goal_published.pose = self.goal
        goal_published.header.frame_id = "odom"
        self.goal_pub.publish(goal_published)

        rospy.sleep(1)
        goal_done_ = self.goal_done
        dist_ = self.dist
        costmap_data_ = self.costmap_data
        collision_ = self.collision

        feature_ = None
        while feature_ is None:
            try:
                feature_ = rospy.wait_for_message('/ORB_SLAM2/FeatureInfo',
                                                  Bool,
                                                  timeout=5)
            except:
                pass

        # pause simulation
        rospy.wait_for_service('/gazebo/pause_physics')
        try:
            #resp_pause = pause.call()
            self.pause()
        except (rospy.ServiceException) as e:
            print("/gazebo/pause_physics service call failed")

        temp1 = np.asarray(
            [self.prev_cmd_vel.linear.x, self.prev_cmd_vel.angular.z, dist_])
        temp2 = np.asarray(feature_.data)
        temp3 = np.asarray(costmap_data_)
        state = np.concatenate((temp1, temp2, temp3), axis=0)

        return state