Python rospy.ROSInterruptException方法代码示例

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSInterruptException [as 别名]
def talker_ctrl():
    global rate_value
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:  
            pub_motor.publish(thruster_ctrl_msg())
            pub_rudder.publish(rudder_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSInterruptException [as 别名]
def talker_ctrl():
    global rate_value
    global result
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:    
            pub_motor.publish(thruster_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSInterruptException [as 别名]
def call_service(self, req):
        """Calling the appropriate service depending on the given request type.
        Requests have to inherit from 'HMMRepRequestAbstractclass'.

        :param req: The request class instance for the request you want to make

        :return: The qsr_type and resulting data tuple. The data is in the data type produced by the response.
        """
        assert(issubclass(req.__class__, RepRequestAbstractclass))
        s = rospy.ServiceProxy(self.services[req.__class__],QSRProbRep)
        try:
            s.wait_for_service(timeout=10.)
            res = s(QSRProbRepRequest(data=json.dumps(req.kwargs)))
        except (rospy.ROSException, rospy.ROSInterruptException, rospy.ServiceException) as e:
            rospy.logerr(e)
            return None
        try:
            data = json.loads(res.data)
        except ValueError:
            data = str(res.data)
        return data 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSInterruptException [as 别名]
def run(self):
        """
        main loop
        """
        # wait for ros-bridge to set up CARLA world
        rospy.loginfo("Waiting for CARLA world (topic: /carla/world_info)...")
        try:
            rospy.wait_for_message("/carla/world_info", CarlaWorldInfo, timeout=10.0)
        except rospy.ROSException as e:
            rospy.logerr("Timeout while waiting for world info!")
            raise e
        rospy.loginfo("CARLA world available. Spawn infrastructure...")
        client = carla.Client(self.host, self.port)
        client.set_timeout(self.timeout)
        self.world = client.get_world()
        self.restart()
        try:
            rospy.spin()
        except rospy.ROSInterruptException:
            pass

# ==============================================================================
# -- main() --------------------------------------------------------------------
# ============================================================================== 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSInterruptException [as 别名]
def run(self):
        """

        Control loop

        :return:
        """

        while not rospy.is_shutdown():
            self.update_current_values()
            self.vehicle_control_cycle()
            self.send_ego_vehicle_control_info_msg()
            try:
                self.control_loop_rate.sleep()
            except rospy.ROSInterruptException:
                pass 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSInterruptException [as 别名]
def run(self):
        """

        Control loop

        :return:
        """
        r = rospy.Rate(10)
        while not rospy.is_shutdown():
            if self._global_plan:
                control = self.run_step()
                if control:
                    control.steer = -control.steer
                    self.vehicle_control_publisher.publish(control)
            else:
                try:
                    r.sleep()
                except rospy.ROSInterruptException:
                    pass 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSInterruptException [as 别名]
def talker_ctrl():
    global rate_value
    global currentHeading
    global windDir 
    global isTacking
    global heeling
    global spHeading

    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    #pub_sail = rospy.Publisher('angleLimits', Float64, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    pub_heading = rospy.Publisher('currentHeading', Float64, queue_size=10)
    pub_windDir = rospy.Publisher('windDirection', Float64, queue_size=10)
    pub_heeling = rospy.Publisher('heeling', Float64, queue_size=10)
    pub_spHeading = rospy.Publisher('spHeading', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:
            pub_rudder.publish(rudder_ctrl_msg())
            #pub_sail.publish(sail_ctrl())
            pub_result.publish(verify_result())
            pub_heading.publish(currentHeading)
            pub_windDir.publish(windDir)
            pub_heeling.publish(heeling)
            pub_spHeading.publish(spHeading)
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSInterruptException [as 别名]
def talker_ctrl():
    global rate_value
    global currentHeading
    global windDir 
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    #pub_sail = rospy.Publisher('angleLimits', Float64, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    pub_heading= rospy.Publisher('currentHeading', Float64, queue_size=10)
    pub_windDir= rospy.Publisher('windDirection', Float64, queue_size=10)

    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:
            pub_rudder.publish(rudder_ctrl_msg())
            #pub_sail.publish(sail_ctrl())
            pub_result.publish(verify_result())
            pub_heading.publish(currentHeading)
            pub_windDir.publish(windDir)
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSInterruptException [as 别名]
def send_command(self, msg, pub_rate):
        """
        @param msg: either an InteractionControlCommand message or
                    InteractionOptions object to be published
        @param pub_rate: the rate in Hz to publish the command

        Note that this function is blocking for non-zero pub_rates until
        the node is shutdown (e.g. via cntl+c) or the robot is disabled.
        A pub_rate of zero will publish the function once and return.
        """
        repeat = False
        if pub_rate > 0:
            rate = rospy.Rate(pub_rate)
            repeat = True
        elif pub_rate < 0:
            rospy.logerr('Invalid publish rate!')

        if isinstance(msg, InteractionOptions):
            msg = msg.to_msg()

        try:
            self.pub.publish(msg)
            while repeat and not rospy.is_shutdown() and self.enable.state().enabled:
                rate.sleep()
                self.pub.publish(msg)
        except rospy.ROSInterruptException:
            rospy.logerr('Keyboard interrupt detected from the user. %s',
                         'Exiting the node...')
        finally:
            if repeat:
                self.send_position_mode_cmd() 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSInterruptException [as 别名]
def main():
    """
    Send a STOP command to the motion controller, which will safely stop the
    motion controller if it is actively running a trajectory. This is useful
    when the robot is executing a long trajectory that needs to be canceled.
    Note: This will only stop motions that are running through the motion
    controller. It will not stop the motor controllers from receiving commands
    send directly from a custom ROS node.

    $ rosrun intera_examples stop_motion_trajectory.py
    """

    try:
        rospy.init_node('stop_motion_trajectory')
        traj = MotionTrajectory()
        result = traj.stop_trajectory()

        if result is None:
            rospy.logerr('FAILED to send stop request')
            return

        if result.result:
            rospy.loginfo('Motion controller successfully stopped the motion!')
        else:
            rospy.logerr('Motion controller failed to stop the motion: %s',
                         result.errorId)

    except rospy.ROSInterruptException:
        rospy.logerr('Keyboard interrupt detected from the user. Exiting before stop completion.') 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSInterruptException [as 别名]
def set_velocity(x, phi, turtle_name, logger):
    position_vector = Vector3(x, 0, 0)
    rotation_vector = Vector3(0, 0, phi)
    twist_msg = Twist(position_vector, rotation_vector)
    try:
        logger.info("move_to_position: publish twist to turtle".format(turtle_name))
        turtle_vel_publisher = rospy.Publisher("/" + turtle_name + "/cmd_vel", Twist, queue_size=10, latch=True)
        turtle_vel_publisher.publish(twist_msg)
        rate = rospy.Rate(10)
        rate.sleep()
    except rospy.ROSInterruptException as e:
        logger.error("Failed to send a velocity command to turtle {}: {}".format(turtle_name, str(e))) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSInterruptException [as 别名]
def main():
    import signal

    rospy.init_node('uwb_multi_range_node')
    u = UWBMultiRange()

    def sigint_handler(sig, _):
        if sig == signal.SIGINT:
            u.stop()
    signal.signal(signal.SIGINT, sigint_handler)

    try:
        u.exec_()
    except (rospy.ROSInterruptException, select.error):
        rospy.logwarn("Interrupted... Stopping.") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSInterruptException [as 别名]
def main():
    import signal

    rospy.init_node('uwb_tracker_node')
    u = UWBTracker()

    def sigint_handler(sig, _):
        if sig == signal.SIGINT:
            u.stop()
    signal.signal(signal.SIGINT, sigint_handler)

    try:
        u.exec_()
    except (rospy.ROSInterruptException, select.error):
        rospy.logwarn("Interrupted... Stopping.") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSInterruptException [as 别名]
def run(self):
        msg_count = 0
        try:
            bag = rosbag.Bag(self.bagfile, mode='a' if self.append else 'w')
            rate = rospy.Rate(self.lookup_frequency)
            last_stamp = rospy.Time()
            while not rospy.is_shutdown():
                try:
                    transform = self.tf_buffer.lookup_transform(
                        self.parent_frame, self.child_frame, rospy.Time())
                    rate.sleep()
                except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                        tf2_ros.ExtrapolationException):
                    rate.sleep()
                    continue
                if last_stamp == transform.header.stamp:
                    continue
                pose = transformstamped_to_posestamped(transform)
                bag.write(self.output_topic, pose, t=pose.header.stamp)
                msg_count += 1
                last_stamp = transform.header.stamp
                rospy.loginfo_throttle(
                    10, "Recorded {} PoseStamped messages.".format(msg_count))

        except rospy.ROSInterruptException:
            pass
        finally:
            bag.close()
            rospy.loginfo("Finished recording.") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSInterruptException [as 别名]
def example_movement():  # TODO replace or remove functionality
    """Used to test if the arm can be moved with gripper control.

    Function moves the arm through a simple motion plan and then tried moving the grippers.
    To use this test, add the ``-m`` or ``--motion-example`` flag to the command line.
    """
    pass
    # try:
    #     franka = FrankaRos(debug=True)
    #
    #     motion_plan = []
    #     resolution = 100
    #     for i in range(1, resolution):
    #         motion_plan.append((0.4+i/resolution, 0.4+i/resolution, 0.4+i/resolution, 0.100))
    #
    #     # print(motion_plan)
    #     for x, y, z, speed in motion_plan:
    #         franka.move_to(x, y, z, speed)
    #         time.sleep(0.1)  # 10 Hz control loop
    #
    #     # now test the grippers are operational
    #     width = 0.06  # 2.2 cm = 0 width
    #     speed = 0.1
    #     force = 1
    #     franka.move_gripper(width, speed)
    #     time.sleep(5)
    #     width = 0.037  # 2.2 cm = 0 width
    #     franka.grasp(width, speed, force)
    #     time.sleep(5)
    #     width = 0.06  # 2.2 cm = 0 width
    #     franka.move_gripper(width, speed)
    #
    # except rospy.ROSInterruptException:
    #     pass