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!") 

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!") 

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!") 

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 

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() --------------------------------------------------------------------
# ============================================================================== 

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 

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 

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!") 

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!") 

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() 

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.') 

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))) 

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.") 

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.") 

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.") 

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 

def __init__(self, role_name):
        """
        Constructor
        """
        rospy.loginfo("Wait for vehicle info...")
        try:
            vehicle_info = rospy.wait_for_message("/carla/{}/vehicle_info".format(role_name),
                                                  CarlaEgoVehicleInfo)
        except rospy.ROSInterruptException as e:
            if not rospy.is_shutdown():
                raise e
            else:
                sys.exit(0)
        if not vehicle_info.wheels:  # pylint: disable=no-member
            rospy.logerr("Cannot determine max steering angle: Vehicle has no wheels.")
            sys.exit(1)

        self.max_steering_angle = vehicle_info.wheels[0].max_steer_angle  # pylint: disable=no-member
        if not self.max_steering_angle:
            rospy.logerr("Cannot determine max steering angle: Value is %s",
                         self.max_steering_angle)
            sys.exit(1)
        rospy.loginfo("Vehicle info received. Max steering angle=%s",
                      self.max_steering_angle)

        rospy.Subscriber("/carla/{}/twist".format(role_name), Twist, self.twist_received)

        self.pub = rospy.Publisher("/carla/{}/vehicle_control_cmd".format(role_name),
                                   CarlaEgoVehicleControl, queue_size=1) 

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:
            rospy.logerr("Error while waiting for world info!")
            sys.exit(1)
        rospy.loginfo("CARLA world available. Waiting for ego vehicle...")

        client = carla.Client(self.host, self.port)
        client.set_timeout(self.timeout)
        self.world = client.get_world()

        try:
            r = rospy.Rate(10)  # 10hz
            while not rospy.is_shutdown():
                self.find_ego_vehicle_actor()
                r.sleep()
        except rospy.ROSInterruptException:
            pass

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

def __init__(self, role_name, target_speed):
        """
        Constructor
        """
        self._route_assigned = False
        self._target_speed = target_speed
        self._waypoints = []
        self._current_pose = Pose()
        rospy.on_shutdown(self.on_shutdown)

        #wait for ros bridge to create relevant topics
        try:
            rospy.wait_for_message(
                 "/carla/{}/odometry".format(role_name), Odometry)
        except rospy.ROSInterruptException as e:
            if not rospy.is_shutdown():
                raise e

        self._odometry_subscriber = rospy.Subscriber(
            "/carla/{}/odometry".format(role_name), Odometry, self.odometry_updated)

        self.control_publisher = rospy.Publisher(
            "/carla/{}/walker_control_cmd".format(role_name), CarlaWalkerControl, queue_size=1)

        self._route_subscriber = rospy.Subscriber(
            "/carla/{}/waypoints".format(role_name), Path, self.path_updated)

        self._target_speed_subscriber = rospy.Subscriber(
            "/carla/{}/target_speed".format(role_name), Float64, self.target_speed_updated) 

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:
            rospy.logerr("Timeout while waiting for world info!")
            sys.exit(1)

        rospy.loginfo("CARLA world available. Spawn ego vehicle...")

        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() --------------------------------------------------------------------
# ============================================================================== 

def get_actor_waypoint(self, actor_id):
        """
        helper method to get waypoint for actor via ros service
        Only used if risk should be avoided.
        """
        try:
            response = self._get_actor_waypoint_client(actor_id)
            return response.waypoint
        except (rospy.ServiceException, rospy.ROSInterruptException) as e:
            if not rospy.is_shutdown:
                rospy.logwarn("Service call failed: {}".format(e)) 

def get_waypoint(self, location):
        """
        Helper to get waypoint for location via ros service.
        Only used if risk should be avoided.
        """
        if rospy.is_shutdown():
            return None
        try:
            response = self._get_waypoint_client(location)
            return response.waypoint
        except (rospy.ServiceException, rospy.ROSInterruptException) as e:
            if not rospy.is_shutdown():
                rospy.logwarn("Service call 'get_waypoint' failed: {}".format(e)) 

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.") 

def main():
    """
    This is an example script to demonstrate the functionality of the MotionTrajectory
    to read a yaml file. This script reads in the yaml file creates a trajectory object
    and sends the trajectory to the robot.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)

    parser.add_argument(
        "-f",  "--file_name", type=str, required=True,
        help="The name of the yaml file to ready the trajectory from")
    parser.add_argument(
        "--timeout", type=float, default=None,
        help="Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout.")

    args = parser.parse_args(rospy.myargv()[1:])


    try:

        rospy.init_node('send_traj_from_file')
        limb = Limb()
        traj = MotionTrajectory(limb = limb)

        read_result=traj.load_yaml_file(args.file_name)

        if not read_result:
            rospy.logerr('Trajectory not successfully read from file')

        result = traj.send_trajectory(timeout=args.timeout)
        if result is None:
            rospy.logerr('Trajectory FAILED to send')
            return

        if result.result:
            rospy.loginfo('Motion controller successfully finished the trajectory!')
        else:
            rospy.logerr('Motion controller failed to complete the trajectory with error %s',
                         result.errorId)
    except rospy.ROSInterruptException:
        rospy.logerr('Keyboard interrupt detected from the user. Exiting before trajectory completion.')