def talker_ctrl():
    global rate_value
    # 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)
    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("usv_position_setpoint", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        pub_motor.publish(thruster_ctrl_msg())
        pub_rudder.publish(rudder_ctrl_msg())
        rate.sleep() 

def spin(self):
    #############################################################
    
        r = rospy.Rate(self.rate)
        idle = rospy.Rate(10)
        then = rospy.Time.now()
        self.ticks_since_target = self.timeout_ticks
    
        ###### main loop  ######
        while not rospy.is_shutdown():
        
            while not rospy.is_shutdown() and self.ticks_since_target < self.timeout_ticks:
                self.spinOnce()
                r.sleep()
            idle.sleep()
                
    ############################################################# 

def move_circle():

    # Create a publisher which can "talk" to Turtlesim and tell it to move
    pub = rospy.Publisher('turtle1/cmd_vel', Twist, queue_size=1)
     
    # Create a Twist message and add linear x and angular z values
    move_cmd = Twist()
    move_cmd.linear.x = 1.0
    move_cmd.angular.z = 1.0

    # Save current time and set publish rate at 10 Hz
    now = rospy.Time.now()
    rate = rospy.Rate(10)

    # For the next 6 seconds publish cmd_vel move commands to Turtlesim
    while rospy.Time.now() < now + rospy.Duration.from_sec(6):
        pub.publish(move_cmd)
        rate.sleep() 

def spin(self):
    #############################################################
    
        r = rospy.Rate(self.rate)
        idle = rospy.Rate(10)
        then = rospy.Time.now()
        self.ticks_since_target = self.timeout_ticks
    
        ###### main loop  ######
        while not rospy.is_shutdown():
        
            while not rospy.is_shutdown() and self.ticks_since_target < self.timeout_ticks:
                self.spinOnce()
                r.sleep()
            idle.sleep()
                
    ############################################################# 

def spin(self):
    ###############################################
        r = rospy.Rate
        self.secs_since_target = self.timeout_secs
        self.then = rospy.Time.now()
        self.latest_msg_time = rospy.Time.now()
        rospy.loginfo("-D- spinning")
        
        ###### main loop #########
        while not rospy.is_shutdown():
            while not rospy.is_shutdown() and self.secs_since_target < self.timeout_secs:
                self.spinOnce()
                r.sleep
                self.secs_since_target = rospy.Time.now() - self.latest_msg_time
                self.secs_since_target = self.secs_since_target.to_sec()
                #rospy.loginfo("  inside: secs_since_target: %0.3f" % self.secs_since_target)
                
            # it's been more than timeout_ticks since we recieved a message
            self.secs_since_target = rospy.Time.now() - self.latest_msg_time
            self.secs_since_target = self.secs_since_target.to_sec()
            # rospy.loginfo("  outside: secs_since_target: %0.3f" % self.secs_since_target)
            self.velocity = 0
            r.sleep
        
    ############################################### 

def scaleWheel():
    rospy.init_node("wheel_scaler")
    rospy.loginfo("wheel_scaler started")
    
    scale = rospy.get_param('distance_scale', 1)
    rospy.loginfo("wheel_scaler scale: %0.2f", scale)
    
    rospy.Subscriber("lwheel", Int16, lwheelCallback)
    rospy.Subscriber("rwheel", Int16, rwheelCallback)
    
    lscaled_pub = rospy.Publisher("lwheel_scaled", Int16, queue_size=10)
    rscaled_pub = rospy.Publisher("rwheel_scaled", Int16, queue_size=10) 
    
    ### testing sleep CPU usage
    r = rospy.Rate(50) 
    while not rospy.is_shutdown:
        r.sleep()
        
    rospy.spin() 

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 navigation():
    pub = rospy.Publisher('usv_position_setpoint', Odometry, queue_size=10) # only create a rostopic, navigation system TODO
    rospy.init_node('navigation_publisher')
    rate = rospy.Rate(60) # 10h

    x = -20.0
    y = -20.0

    msg = Odometry()
   # msg.header = Header()
    msg.header.stamp = rospy.Time.now()
    msg.header.frame_id = "navigation"
    msg.pose.pose.position = Point(x, y, 0.)
 
    while not rospy.is_shutdown():
            pub.publish(msg)
            rate.sleep() 

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 __init__(self, robot_name, print_debug, email_cred_file='', log_file='', control_rate=100, gripper_attached='default'):
        super(WidowXController, self).__init__(robot_name, print_debug, email_cred_file, log_file, control_rate, gripper_attached)
        self._redist_rate = rospy.Rate(50)

        self._arbotix = ArbotiX('/dev/ttyUSB0')
        assert self._arbotix.syncWrite(MAX_TORQUE_L, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring"
        assert self._arbotix.syncWrite(TORQUE_LIMIT, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring"

        self._joint_lock = Lock()
        self._angles, self._velocities = {}, {}
        rospy.Subscriber("/joint_states", JointState, self._joint_callback)
        time.sleep(1)        

        self._joint_pubs = [rospy.Publisher('/{}/command'.format(name), Float64, queue_size=1) for name in JOINT_NAMES[:-1]]
        self._gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command', Float64, queue_size=1)

        p.connect(p.DIRECT)
        widow_x_urdf = '/'.join(__file__.split('/')[:-1]) + '/widowx/widowx.urdf'
        self._armID = p.loadURDF(widow_x_urdf, useFixedBase=True) 
        p.resetBasePositionAndOrientation(self._armID, [0, 0, 0], p.getQuaternionFromEuler([np.pi, np.pi, np.pi]))       
        self._n_errors = 0 

def spin(self):
    ###############################################
        r = rospy.Rate
        self.secs_since_target = self.timeout_secs
        self.then = rospy.Time.now()
        self.latest_msg_time = rospy.Time.now()
        rospy.loginfo("-D- spinning")
        
        ###### main loop #########
        while not rospy.is_shutdown():
            while not rospy.is_shutdown() and self.secs_since_target < self.timeout_secs:
                self.spinOnce()
                r.sleep
                self.secs_since_target = rospy.Time.now() - self.latest_msg_time
                self.secs_since_target = self.secs_since_target.to_sec()
                #rospy.loginfo("  inside: secs_since_target: %0.3f" % self.secs_since_target)
                
            # it's been more than timeout_ticks since we recieved a message
            self.secs_since_target = rospy.Time.now() - self.latest_msg_time
            self.secs_since_target = self.secs_since_target.to_sec()
            # rospy.loginfo("  outside: secs_since_target: %0.3f" % self.secs_since_target)
            self.velocity = 0
            r.sleep
        
    ############################################### 

def spin(self):
    #############################################################
    
        r = rospy.Rate(self.rate)
        idle = rospy.Rate(10)
        then = rospy.Time.now()
        self.ticks_since_target = self.timeout_ticks
    
        ###### main loop  ######
        while not rospy.is_shutdown():
        
            while not rospy.is_shutdown() and self.ticks_since_target < self.timeout_ticks:
                self.spinOnce()
                r.sleep()
            idle.sleep()
                
    ############################################################# 

def spin(self):
    ###############################################
        r = rospy.Rate
        self.secs_since_target = self.timeout_secs
        self.then = rospy.Time.now()
        self.latest_msg_time = rospy.Time.now()
        rospy.loginfo("-D- spinning")
        
        ###### main loop #########
        while not rospy.is_shutdown():
            while not rospy.is_shutdown() and self.secs_since_target < self.timeout_secs:
                self.spinOnce()
                r.sleep
                self.secs_since_target = rospy.Time.now() - self.latest_msg_time
                self.secs_since_target = self.secs_since_target.to_sec()
                #rospy.loginfo("  inside: secs_since_target: %0.3f" % self.secs_since_target)
                
            # it's been more than timeout_ticks since we recieved a message
            self.secs_since_target = rospy.Time.now() - self.latest_msg_time
            self.secs_since_target = self.secs_since_target.to_sec()
            # rospy.loginfo("  outside: secs_since_target: %0.3f" % self.secs_since_target)
            self.velocity = 0
            r.sleep
        
    ############################################### 

def spin(self):
    #############################################################
    
        r = rospy.Rate(self.rate)
        idle = rospy.Rate(10)
        then = rospy.Time.now()
        self.ticks_since_target = self.timeout_ticks
    
        ###### main loop  ######
        while not rospy.is_shutdown():
        
            while not rospy.is_shutdown() and self.ticks_since_target < self.timeout_ticks:
                self.spinOnce()
                r.sleep()
            idle.sleep()
                
    ############################################################# 

def spin(self):
    ###############################################
        r = rospy.Rate
        self.secs_since_target = self.timeout_secs
        self.then = rospy.Time.now()
        self.latest_msg_time = rospy.Time.now()
        rospy.loginfo("-D- spinning")
        
        ###### main loop #########
        while not rospy.is_shutdown():
            while not rospy.is_shutdown() and self.secs_since_target < self.timeout_secs:
                self.spinOnce()
                r.sleep
                self.secs_since_target = rospy.Time.now() - self.latest_msg_time
                self.secs_since_target = self.secs_since_target.to_sec()
                #rospy.loginfo("  inside: secs_since_target: %0.3f" % self.secs_since_target)
                
            # it's been more than timeout_ticks since we recieved a message
            self.secs_since_target = rospy.Time.now() - self.latest_msg_time
            self.secs_since_target = self.secs_since_target.to_sec()
            # rospy.loginfo("  outside: secs_since_target: %0.3f" % self.secs_since_target)
            self.velocity = 0
            r.sleep
        
    ############################################### 

def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("--lr", default="r")
    args = parser.parse_args()

    rospy.init_node("pr2_target_policy")
    with open('config/environment/pr2.yaml') as yaml_string:
        pr2_env = utils.from_yaml(yaml_string)

    tool_link_name = "%s_gripper_tool_frame" % args.lr
    pol = policies.Pr2TargetPolicy(pr2_env, tool_link_name, np.zeros(3))


    rate = rospy.Rate(30.0)
    while not rospy.is_shutdown():
        state = pr2_env.get_state()
        target_state = pol.get_target_state()
        action = (target_state - state) / pr2_env.dt
        pr2_env.step(action)
        # pr2_env.reset(pol.get_target_state())
        rate.sleep() 

def attach_springs(self):
        """
        Switches to joint torque mode and attached joint springs to current
        joint positions.
        """
        # record initial joint angles
        self._start_angles = self._limb.joint_angles()

        # set control rate
        control_rate = rospy.Rate(self._rate)

        # for safety purposes, set the control rate command timeout.
        # if the specified number of command cycles are missed, the robot
        # will timeout and return to Position Control Mode
        self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds)

        # loop at specified rate commanding new joint torques
        while not rospy.is_shutdown():
            if not self._rs.state().enabled:
                rospy.logerr("Joint torque example failed to meet "
                             "specified control rate timeout.")
                break
            self._update_forces()
            control_rate.sleep() 

def wobble(self):
        self.set_neutral()
        """
        Performs the wobbling
        """
        command_rate = rospy.Rate(1)
        control_rate = rospy.Rate(100)
        start = rospy.get_time()
        while not rospy.is_shutdown() and (rospy.get_time() - start < 10.0):
            angle = random.uniform(-2.0, 0.95)
            while (not rospy.is_shutdown() and
                   not (abs(self._head.pan() - angle) <=
                       intera_interface.HEAD_PAN_ANGLE_TOLERANCE)):
                self._head.set_pan(angle, speed=0.3, timeout=0)
                control_rate.sleep()
            command_rate.sleep()

        self._done = True
        rospy.signal_shutdown("Example finished.") 

def publish_pc2(pc, obj):
    """Publisher of PointCloud data"""
    pub = rospy.Publisher("/points_raw", PointCloud2, queue_size=1000000)
    rospy.init_node("pc2_publisher")
    header = std_msgs.msg.Header()
    header.stamp = rospy.Time.now()
    header.frame_id = "velodyne"
    points = pc2.create_cloud_xyz32(header, pc[:, :3])

    pub2 = rospy.Publisher("/points_raw1", PointCloud2, queue_size=1000000)
    header = std_msgs.msg.Header()
    header.stamp = rospy.Time.now()
    header.frame_id = "velodyne"
    points2 = pc2.create_cloud_xyz32(header, obj)

    r = rospy.Rate(0.1)
    while not rospy.is_shutdown():
        pub.publish(points)
        pub2.publish(points2)
        r.sleep() 

def sendSetpoint():
    # Input data
    global setpoint, yawSetPoint, run, position_control
    # Output data
    local_setpoint_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=0)

    rate = rospy.Rate(5)

    while run:
        q = tf.transformations.quaternion_from_euler(0, 0, deg2radf(yawSetPoint), axes="sxyz")

        msg = PoseStamped()
        msg.header.stamp = rospy.Time.now()
        msg.pose.position.x = float(setpoint.x)
        msg.pose.position.y = float(setpoint.y)
        msg.pose.position.z = float(setpoint.z)
        msg.pose.orientation.x = q[0]
        msg.pose.orientation.y = q[1]
        msg.pose.orientation.z = q[2]
        msg.pose.orientation.w = q[3]
        local_setpoint_pub.publish(msg)
        rate.sleep() 

def execute(self, inputs, outputs, gvm):
    turtle_name = inputs["turtle_name"]
    x = inputs["x_pos"]
    y = inputs["y_pos"]
    phi = inputs["phi"]

    service = "/spawn"
    rospy.wait_for_service(service)
    spawn_turtle_service = rospy.ServiceProxy(service, Spawn)
    resp1 = spawn_turtle_service(x, y, phi, turtle_name)
    self.logger.info("ROS external module: executed the {} service".format(service))

    turtle_pos_subscriber = rospy.Subscriber("/" + turtle_name + "/pose", Pose, check_turtle_pose)

    r = rospy.Rate(10)
    global pose_received
    # wait until the first pose message was received
    while not pose_received:
        r.sleep()

    return 0 

def status_two(self, msg):
        # """ message ID x4E1 or 1249
        # Byte 0 bits 0-1 - Rolling Count
        # Byte 0 bits 2-7 - Maximum tracks (number of objects of interest)
        # Byte 1 bit 7 - Overheat Error
        # Byte 1 bit 6 - Range Perf Error
        # Byte 1 bit 5 - Internal Error
        # Byte 1 bit 4 - XCVR Operational
        # 0 - Not radiating, 1 - radiating
        # Byte 1 bit 3 - Raw Data Mode
        # Byte 1 bits 0-2, Byte 2 - Steering Angle Ack
        # Byte 3 - Temperature
        # Byte 4 bits 2-7 - Speed Comp Factor
        # Byte 4 bits 0-1 - Grouping Mode
        # Byte 5 - Yaw Rate Bias
        # Bytes 6-7 - SW Version DSP
        # """
        self.data["status_two_rolling_count"] = (msg[0] & 0x03)
        self.data["maximum_tracks"] = ((msg[0] & 0xFC) >> 2)
        self.data["overheat_error"] = ((msg[1] & 0x80) >> 7)
        self.data["range_perf_error"] = ((msg[1] & 0x40) >> 6)
        self.data["internal_error"] = ((msg[1] & 0x20) >> 5)
        self.data["radiating"] = ((msg[1] & 0x10) >> 4)
        self.data["raw_data_mode"] = ((msg[1] & 0x08) >> 3)
        self.data["steering_angle_ack"] = (((msg[1] & 0x07) << 8) | msg[2]) # spans multiple bytes
        self.data["temperature"] = msg[3]
        self.data["speed_comp_factor"] = ((msg[4] & 0xFC) >> 2)
        self.data["grouping_mode"] = (msg[4] & 0x03)
        self.data["yaw_rate_bias"] = msg[5]
        self.data["sw_version_dsp"] = ((msg[6] << 8) | msg[7]) # spans multiple bytes 

def spin(self):
    #############################################################################
        r = rospy.Rate(self.rate)
        while not rospy.is_shutdown():
            self.update()
            r.sleep()
       
     
    ############################################################################# 

def spin(self):
    #####################################################
        self.r = rospy.Rate(self.rate) 
        self.then = rospy.Time.now()
        self.ticks_since_target = self.timeout_ticks
        self.wheel_prev = self.wheel_latest
        self.then = rospy.Time.now()
        while not rospy.is_shutdown():
            self.spinOnce()
            self.r.sleep()
            
    ##################################################### 

def run(self):
        rospy.Rate(1)
        rospy.spin() 

def run(self, update_rate=10):
        """
        Publish data continuously with given rate.

        :type   update_rate:    int
        :param  update_rate:    The update rate.

        """
        r = rospy.Rate(update_rate)
        while not rospy.is_shutdown():
            self._publish_tf(update_rate)
            self._publish_image()
            self._publish_objects()
            self._publish_joint_state()
            self._publish_imu()
            self._publish_battery()
            self._publish_odometry()
            self._publish_diagnostics()
            # send message repeatedly to avoid idle mode.
            # This might cause low battery soon
            # TODO improve this!
            self._cozmo.drive_wheels(*self._wheel_vel)
            # sleep
            r.sleep()
        # stop events on cozmo
        self._cozmo.stop_all_motors() 

def talker():
    pub = rospy.Publisher('/barco_auv/thruster_command', JointState, queue_size=10)
    rospy.init_node('ctrl_jointState_publisher', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    hello_str = JointState()
    hello_str.header = Header()
    hello_str.name = ['fwd_left']
    hello_str.position = [10]
    hello_str.velocity = []
    hello_str.effort = []

    while not rospy.is_shutdown():
        rospy.loginfo(hello_str)
        pub.publish(hello_str)
        rate.sleep() 

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 __init__(self):
        rospy.init_node('usv_vel_ctrl', anonymous=False)
        self.rate = 10
        r = rospy.Rate(self.rate) # 10hertz

        self.usv_vel = Odometry()
        self.usv_vel_ant = Odometry()
        self.target_vel = Twist()
        self.target_vel_ant = Twist()
        self.thruster_msg = JointState()
        self.thruster_msg.header = Header()
        self.kp_lin = 80
        self.ki_lin = 200
        thruster_name = 'fwd_left, fwd_right'
        self.I_ant_lin = 0
        self.I_ant_ang = 0
        self.lin_vel = 0
        self.lin_vel_ang = 0
        self.ang_vel = 0
        self.kp_ang = 80 
        self.ki_ang = 100
        self.thruster_max = 30
        self.vel_left = 0
        self.vel_right = 0
        self.thruster_command = numpy.array([0, 0])
        self.erro = 0

        self.pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)

        rospy.Subscriber("state", Odometry, self.get_usv_vel)  
        rospy.Subscriber("cmd_vel", Twist, self.get_target_vel)

        while not rospy.is_shutdown():
            self.pub_motor.publish(self.thruster_ctrl_msg(self.vel_ctrl(), thruster_name))
            r.sleep()