Python std_msgs.msg.Float64() Examples

def __init__(self):
        rospy.init_node('gripper_controller')
        self.msg = None
       
       
        self.r_pub = rospy.Publisher('gripper_controller/command',
                Float64,
                queue_size=10)
        """
        self.t_pub = rospy.Publisher('arm_controller/command',
                JointTrajectory,
                queue_size=10)
        self.js_sub = rospy.Subscriber('joint_states', JointState,
                self._jointStateCb)
        """

        # subscribe to command and then spin
        self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
        self.server.start()
        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 __init__(self):
        """
        Create HeadLiftJoy object.

        """
        # params
        self._head_axes = rospy.get_param('~head_axes', 3)
        self._lift_axes = rospy.get_param('~lift_axes', 3)
        self._head_button = rospy.get_param('~head_button', 4)
        self._lift_button = rospy.get_param('~lift_button', 5)

        # pubs
        self._head_pub = rospy.Publisher('head_angle', Float64, queue_size=1)
        self._lift_pub = rospy.Publisher('lift_height', Float64, queue_size=1)

        # subs
        self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_cb, queue_size=1) 

def __init__(self):
        # setup
        CozmoTeleop.settings = termios.tcgetattr(sys.stdin)
        atexit.register(self.reset_terminal)

        # vars
        self.head_angle = STD_HEAD_ANGLE
        self.lift_height = STD_LIFT_HEIGHT

        # params
        self.lin_vel = rospy.get_param('~lin_vel', 0.2)
        self.ang_vel = rospy.get_param('~ang_vel', 1.5757)

        # pubs
        self._head_pub = rospy.Publisher('head_angle', Float64, queue_size=1)
        self._lift_pub = rospy.Publisher('lift_height', Float64, queue_size=1)
        self._cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1) 

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 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 __init__(self, configs):
        """
        Constructor of the LoCoBotCamera class.

        :param configs: Object containing configurations for camera,
                        pan joint and tilt joint.

        :type configs: YACS CfgNode
        """
        use_camera = rospy.get_param("use_camera", False)
        use_sim = rospy.get_param("use_sim", False)
        use_camera = use_camera or use_sim
        if not use_camera:
            rospy.logwarn(
                "Neither use_camera, nor use_sim, is not set"
                " to True when the LoCoBot driver is launched."
                "You may not be able to command the camera"
                " correctly using PyRobot!!!"
            )
            return
        super(LoCoBotCamera, self).__init__(configs=configs)

        rospy.Subscriber(
            self.configs.ARM.ROSTOPIC_JOINT_STATES,
            JointState,
            self._camera_pose_callback,
        )

        self.set_pan_pub = rospy.Publisher(
            self.configs.CAMERA.ROSTOPIC_SET_PAN, Float64, queue_size=1
        )
        self.set_tilt_pub = rospy.Publisher(
            self.configs.CAMERA.ROSTOPIC_SET_TILT, Float64, queue_size=1
        )
        self.pan = None
        self.tilt = None
        self.tol = 0.01 

def __init__(self):

        # Initialization parameters to control the Interbotix Arm
        rospy.wait_for_service("get_robot_info")
        srv_robot_info = rospy.ServiceProxy("get_robot_info", RobotInfo)                                                # ROS Service to get joint limit information among other things
        self.resp = srv_robot_info()                                                                                    # Store the robot information in this variable
        self.joint_indx_dict = dict(zip(self.resp.joint_names, range(self.resp.num_single_joints)))                     # Map each joint-name to their respective index in the joint_names list (which conveniently matches their index in the joint-limit lists)
        self.joy_msg = ArmJoyControl()                                                                                  # Incoming message coming from the 'joy_control' node
        self.arm_model = rospy.get_param("~robot_name")                                                                 # Arm-model type
        self.num_joints = self.resp.num_joints                                                                          # Number of joints in the arm
        self.speed_max = 3.0                                                                                            # Max scaling factor when bumping up joint speed
        self.gripper_pwm = 200                                                                                          # Initial gripper PWM value
        self.gripper_moving = False                                                                                     # Boolean that is set to 'True' if the gripper is moving - 'False' otherwise
        self.gripper_command = Float64()                                                                                # Float64 message to be sent to the 'gripper' joint
        self.gripper_index = self.num_joints + 1                                                                        # Index of the 'left_finger' joint in the JointState message
        self.follow_pose = True                                                                                         # True if going to 'Home' or 'Sleep' pose
        self.joint_states = None                                                                                        # Holds the most up-to-date JointState message
        self.js_mutex = threading.Lock()                                                                                # Mutex to make sure that 'self.joint_states' variable isn't being modified and read at the same time
        self.joint_commands = JointCommands()                                                                           # JointCommands message to command the arm joints as a group
        self.target_positions = list(self.resp.sleep_pos)                                                               # Holds the 'Sleep' or 'Home' joint values
        self.robot_des = getattr(mrd, self.arm_model)                                                                   # Modern Robotics robot description
        self.joy_speeds = {"course" : 2.0, "fine" : 2.0, "current" : 2.0}                                               # Dictionary containing the desired joint speed scaling factors
        self.pub_joint_commands = rospy.Publisher("joint/commands", JointCommands, queue_size=100, latch=True)          # ROS Publisher to command joint positions [rad]
        self.pub_gripper_command = rospy.Publisher("gripper/command", Float64, queue_size=100)                          # ROS Publisher to command gripper PWM values
        self.sub_joy_commands = rospy.Subscriber("joy/commands", ArmJoyControl, self.joy_control_cb)                    # ROS Subscriber to get the joystick commands
        self.sub_joint_states = rospy.Subscriber("joint_states", JointState, self.joint_state_cb)                       # ROS Subscriber to get the current joint states
        while (self.joint_states == None and not rospy.is_shutdown()): pass                                             # Wait until we know the current joint values of the robot
        self.joint_positions = list(self.joint_states.position[:self.num_joints])                                       # Holds the desired joint positions for the robot arm at any point in time
        self.yaw = 0.0                                                                                                  # Holds the desired 'yaw' of the end-effector w.r.t. the 'base_link' frame
        self.T_sy = np.identity(4)                                                                                      # Transformation matrix of a virtual frame with the same x, y, z, roll, and pitch values as 'base_link' but containing the 'yaw' of the end-effector
        self.T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, self.resp.sleep_pos)                           # Transformation matrix of the end-effector w.r.t. the 'base_link' frame
        self.T_yb = np.dot(mr.TransInv(self.T_sy), self.T_sb)                                                           # Transformation matrix of the end-effector w.r.t. T_sy
        tmr_controller = rospy.Timer(rospy.Duration(0.02), self.controller)                                             # ROS Timer to control the Interbotix Arm

    ### @brief Used to convert 'self.yaw' to a rotation matrix
    ### @param yaw - yaw angle to convert to a rotation matrix 

def actionCb(self, goal):
        """ Take an input command of width to open gripper. """
        rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position)
        command = goal.command.position
        
        # publish msgs
        rmsg = Float64(command)
        
        self.r_pub.publish(rmsg)
        rospy.sleep(3.0)
        self.server.set_succeeded()
        rospy.loginfo('Gripper Controller: Done.') 

def _create_cmd_pub(list_ctrlrs, ctrlr_name):
    # Create a command publisher.
    _wait_for_ctrlr(list_ctrlrs, ctrlr_name)
    return rospy.Publisher(ctrlr_name + "/command", Float64) 

def start(self):
        self.running = True
        self.joint_state_pub = rospy.Publisher(self.controller_namespace + '/state', JointState, queue_size=1)
        self.command_sub = rospy.Subscriber(self.controller_namespace + '/command', Float64, self.process_command)
        self.motor_states_sub = rospy.Subscriber('motor_states/%s' % self.port_namespace, MotorStateList, self.process_motor_states) 

def update_target_speed(self, speed):
        super(RosVehicleControl, self).update_target_speed(speed)
        rospy.loginfo("{}: Target speed changed to {}".format(self._role_name, speed))
        self._target_speed_publisher.publish(Float64(data=speed)) 

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 __init__(self, role_name, target_speed, avoid_risk):
        """
        Constructor
        """
        self._route_assigned = False
        self._global_plan = None
        self._agent = None
        self._target_speed = target_speed
        rospy.on_shutdown(self.on_shutdown)

        # wait for ego vehicle
        vehicle_info = None
        try:
            vehicle_info = rospy.wait_for_message(
                "/carla/{}/vehicle_info".format(role_name), CarlaEgoVehicleInfo)
        except rospy.ROSException:
            rospy.logerr("Timeout while waiting for world info!")
            sys.exit(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)

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

        self._agent = BasicAgent(role_name, vehicle_info.id,  # pylint: disable=no-member
                                 avoid_risk) 

def set_gripper_command(self, command, delay=0):
        gripper_command = Float64(command)
        self.pub_gripper_command.publish(gripper_command)
        if (delay > 0):
            rospy.sleep(delay)

    ### @brief Returns a floating point number containing the linear distance between the two gripper fingers
    ### @param <float> [out] - linear distance between the gripper fingers [m] 

def __init__(self):
        self.cv_bridge = CvBridge()
        self.camera_info_lock = threading.RLock()
        self.ar_tag_lock = threading.RLock()

        # Setup to control camera.
        self.joint_cmd_srv = rospy.ServiceProxy(JOINT_COMMAND_TOPIC, JointCommand)

        # Subscribe to camera pose and instrinsic streams.
        rospy.Subscriber(JOINT_STATE_TOPIC, JointState, self._camera_pose_callback)
        rospy.Subscriber(
            ROSTOPIC_CAMERA_INFO_STREAM, CameraInfo, self.camera_info_callback
        )
        self.img = None
        rospy.Subscriber(
            ROSTOPIC_CAMERA_IMAGE_STREAM,
            Image,
            lambda x: self.img_callback(x, "img", "bgr8"),
        )
        self.depth = None
        rospy.Subscriber(
            ROSTOPIC_CAMERA_DEPTH_STREAM,
            Image,
            lambda x: self.img_callback(x, "depth", None),
        )
        self.depth_registered = None
        rospy.Subscriber(
            ROSTOPIC_ALIGNED_CAMERA_DEPTH_STREAM,
            Image,
            lambda x: self.img_callback(x, "depth_registered", None),
        )
        rospy.Subscriber(ROSTOPIC_AR_POSE_MARKER, AlvarMarkers, self.alvar_callback)
        self.img = None
        self.ar_tag_pose = None

        self._transform_listener = TransformListener()
        self._update_camera_extrinsic = True
        self.camera_extrinsic_mat = None
        self.set_pan_pub = rospy.Publisher(ROSTOPIC_SET_PAN, Float64, queue_size=1)
        self.set_tilt_pub = rospy.Publisher(ROSTOPIC_SET_TILT, Float64, queue_size=1) 

def show_reward(self, reward):
        """
        Publishing reward value as marker.
        :param reward
        """
        # Publish reward as Marker
        msg = Marker()
        msg.header.frame_id = "/base_footprint"
        msg.ns = ""
        msg.id = 0
        msg.type = msg.TEXT_VIEW_FACING
        msg.action = msg.ADD

        msg.pose.position.x = 0.0
        msg.pose.position.y = 1.0
        msg.pose.position.z = 0.0
        msg.pose.orientation.x = 0.0
        msg.pose.orientation.y = 0.0
        msg.pose.orientation.z = 0.0
        msg.pose.orientation.w = 1.0

        msg.text = "%f"%reward

        msg.scale.x = 10.0
        msg.scale.y = 10.0
        msg.scale.z = 1.0

        msg.color.r = 0.3
        msg.color.g = 0.4
        msg.color.b = 1.0
        msg.color.a = 1.0
        self.__rew_pub.publish(msg)

        # Publish reward as number
        msg = Float64()
        msg.data = reward
        self. __rew_num_pub.publish(msg) 

def __init__(self, ns, stack_offset):
        self.__ns = ns
        self.__stack_offset = stack_offset
        print("stack_offset: %d"%self.__stack_offset)
        self.__input_images = deque(maxlen=4 * self.__stack_offset)

        #Input state
        self.__input_img_pub1 = rospy.Publisher('%s/state_image1' % (self.__ns), Image, queue_size=1)
        self.__input_img_pub2 = rospy.Publisher('%s/state_image2' % (self.__ns), Image, queue_size=1)
        self.__input_img_pub3 = rospy.Publisher('%s/state_image3' % (self.__ns), Image, queue_size=1)
        self.__input_img_pub4 = rospy.Publisher('%s/state_image4' % (self.__ns), Image, queue_size=1)
        self.__occ_grid_pub = rospy.Publisher('%s/rl_map' % (self.__ns), OccupancyGrid, queue_size=1)
        self.__input_scan_pub = rospy.Publisher('%s/state_scan' % (self.__ns), LaserScan, queue_size=1)

        # reward info
        self.__rew_pub = rospy.Publisher('%s/reward' % (self.__ns), Marker, queue_size=1)
        self.__rew_num_pub = rospy.Publisher('%s/reward_num' % (self.__ns), Float64, queue_size=1)

        # Waypoint info
        self.__wp_pub1 = rospy.Publisher('%s/wp_vis1' % (self.__ns), PointStamped, queue_size=1)
        self.__wp_pub2 = rospy.Publisher('%s/wp_vis2' % (self.__ns), PointStamped, queue_size=1)
        self.__wp_pub3 = rospy.Publisher('%s/wp_vis3' % (self.__ns), PointStamped, queue_size=1)
        self.__wp_pub4 = rospy.Publisher('%s/wp_vis4' % (self.__ns), PointStamped, queue_size=1) 

def take_sim_step(self):
        """
        Executing one simulation step of 0.1 sec
        """
        msg = Float64()
        msg.data = self.__update_rate
        rospy.wait_for_service('%s/step' % self.NS)
        self.__sim_step(msg)
        return 

def close_gripper(self, wait = False):                                      # should likely wrap separate gripper control class for max re-usability
        self._gripper_pub.publish(Float64(self.GRIPPER_CLOSE))
        time.sleep(GRIPPER_WAIT) 

def open_gripper(self, wait = False):                                       # should likely wrap separate gripper control class for max re-usability
        self._gripper_pub.publish(Float64(self.GRIPPER_OPEN))
        time.sleep(GRIPPER_WAIT) 

def move_to_neutral(self, duration=2):
        self._n_errors = 0
        self._lerp_joints(np.array(NEUTRAL_VALUES), duration)
        self._gripper_pub.publish(Float64(GRIPPER_DROP[0]))
        time.sleep(GRIPPER_WAIT) 

def _move_to_target_joints(self, joint_values):
        for value, pub in zip(joint_values, self._joint_pubs):
            pub.publish(Float64(value)) 

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 __init__(self):
        rospy.init_node("gazebo_keyboard_teleoperation", anonymous=True)

        # subscriber to get joint angles froim gazebo
        rospy.Subscriber("/joint_state", JointState, self._callback_joint_states)

        # listener for transoforms
        self.listener = tf.TransformListener()

        # publishers for joint angles to gazebo
        self.head_pan_pub = rospy.Publisher(
            "/head_pan_cntrl/command", Float64, queue_size=1
        )
        self.head_tilt_pub = rospy.Publisher(
            "/head_tilt_cntrl/command", Float64, queue_size=1
        )
        self.joint_1_pub = rospy.Publisher(
            "/joint_1_cntrl/command", Float64, queue_size=1
        )
        self.joint_2_pub = rospy.Publisher(
            "/joint_2_cntrl/command", Float64, queue_size=1
        )
        self.joint_3_pub = rospy.Publisher(
            "/joint_3_cntrl/command", Float64, queue_size=1
        )
        self.joint_4_pub = rospy.Publisher(
            "/joint_4_cntrl/command", Float64, queue_size=1
        )
        self.joint_5_pub = rospy.Publisher(
            "/joint_5_cntrl/command", Float64, queue_size=1
        )
        self.joint_6_pub = rospy.Publisher(
            "/joint_6_cntrl/command", Float64, queue_size=1
        )
        self.joint_7_pub = rospy.Publisher(
            "/joint_7_cntrl/command", Float64, queue_size=1
        )

        # service to reset the gazebo world
        rospy.wait_for_service("/gazebo/reset_world")
        self.reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty)

        # service to reset the gazebo robot
        rospy.wait_for_service("/gazebo/reset_simulation")
        self.reset_simulation = rospy.ServiceProxy("/gazebo/reset_simulation", Empty)

        # services to pause and unpause the simulations
        rospy.wait_for_service("/gazebo/pause_physics")
        self.pause_sim = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
        rospy.wait_for_service("/gazebo/unpause_physics")
        self.unpause_sim = rospy.ServiceProxy("/gazebo/unpause_physics", Empty) 

def __init__(self, actor, args=None):
        super(RosVehicleControl, self).__init__(actor)
        self._carla_actor = actor
        self._role_name = actor.attributes["role_name"]
        if not self._role_name:
            rospy.logerr("Invalid role_name!")

        rospy.init_node('ros_agent_{}'.format(self._role_name))
        self._current_target_speed = None
        self._current_path = None

        self._target_speed_publisher = rospy.Publisher(
            "/carla/{}/target_speed".format(self._role_name), Float64, queue_size=1, latch=True)

        self._path_publisher = rospy.Publisher(
            "/carla/{}/waypoints".format(self._role_name), Path, queue_size=1, latch=True)

        if "launch" in args and "launch-package" in args:
            uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
            roslaunch.configure_logging(uuid)
            launch_file = args["launch"]
            launch_package = args["launch-package"]

            cli_args = [launch_package, launch_file]
            cli_args.append('role_name:={}'.format(self._role_name))

            # add additional launch parameters
            launch_parameters = []
            for key, value in args.items():
                if not key == "launch" and not key == "launch-package":
                    launch_parameters.append('{}:={}'.format(key, value))
                    cli_args.append('{}:={}'.format(key, value))

            rospy.loginfo("{}: Launching {} from package {} (parameters: {})...".format(
                self._role_name, launch_file, launch_package, launch_parameters))
            roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(cli_args)
            roslaunch_args = cli_args[2:]
            launch_files = [(roslaunch_file[0], roslaunch_args)]
            parent = roslaunch.parent.ROSLaunchParent(uuid, launch_files)
            parent.start()
            rospy.loginfo("{}: Successfully started ros vehicle control".format(self._role_name))
        else:
            rospy.logerr(
                "{}: Missing value for 'launch' and/or 'launch-package'.".format(self._role_name)) 

def __init__(self, robot_name, mrd=None, robot_model=None, moving_time=2.0, accel_time=0.3, gripper_pressure=0.5, use_time=True):
        rospy.init_node(robot_name + "_robot_manipulation")                                                                         # Initialize ROS Node
        rospy.wait_for_service(robot_name + "/get_robot_info")                                                                      # Wait for ROS Services to become available
        rospy.wait_for_service(robot_name + "/set_operating_modes")
        rospy.wait_for_service(robot_name + "/set_motor_register_values")
        srv_robot_info = rospy.ServiceProxy(robot_name + "/get_robot_info", RobotInfo)                                              # Create Service Proxies
        self.srv_set_op = rospy.ServiceProxy(robot_name + "/set_operating_modes", OperatingModes)
        self.srv_set_register = rospy.ServiceProxy(robot_name + "/set_motor_register_values", RegisterValues)
        self.resp = srv_robot_info()                                                                                                # Get Robot Info like joint limits
        self.use_time = use_time                                                                                                    # Determine if 'Drive Mode' is set to 'Time' or 'Velocity'
        self.set_trajectory_time(moving_time, accel_time)                                                                           # Change the Profile Velocity/Acceleration Registers in the Arm motors
        self.joint_indx_dict = dict(zip(self.resp.joint_names, range(self.resp.num_single_joints)))                                 # Map joint names to their respective indices in the joint limit lists
        self.joint_states = None                                                                                                    # Holds the current joint states of the arm
        self.js_mutex = threading.Lock()                                                                                            # Mutex to prevent writing/accessing the joint states variable at the same time
        self.pub_joint_commands = rospy.Publisher(robot_name + "/joint/commands", JointCommands, queue_size=100)                    # ROS Publisher to command the arm
        self.pub_single_command = rospy.Publisher(robot_name + "/single_joint/command", SingleCommand, queue_size=100)              # ROS Publisher to command a specified joint
        self.sub_joint_states = rospy.Subscriber(robot_name + "/joint_states", JointState, self.joint_state_cb)                     # ROS Subscriber to get the current joint states
        while (self.joint_states == None and not rospy.is_shutdown()): pass                                                         # Wait until the first JointState message is received
        if robot_model is None:
            robot_model = robot_name
        if (mrd is not None):
            self.robot_des = getattr(mrd, robot_model)                                                                              # Contains the Modern Robotics description of the desired arm model
            self.gripper_moving = False                                                                                             # When in PWM mode, False means the gripper PWM is 0; True means the gripper PWM in nonzero
            self.gripper_command = Float64()                                                                                        # ROS Message to hold the gripper PWM command
            self.set_gripper_pressure(gripper_pressure)                                                                             # Maps gripper pressure to a PWM range
            self.gripper_index = self.joint_states.name.index("left_finger")                                                        # Index of the 'left_finger' joint in the JointState message
            self.initial_guesses = [[0.0] * self.resp.num_joints for i in range(3)]                                                 # Guesses made up of joint values to seed the IK function
            self.initial_guesses[1][0] = np.deg2rad(-120)
            self.initial_guesses[2][0] = np.deg2rad(120)
            self.pub_gripper_command = rospy.Publisher(robot_name + "/gripper/command", Float64, queue_size=100)                    # ROS Publisher to command the gripper
            self.pub_joint_traj = rospy.Publisher(robot_name + "/arm_controller/joint_trajectory", JointTrajectory, queue_size=100) # ROS Pubilsher to command Cartesian trajectories
            self.waist_index = self.joint_states.name.index("waist")                                                                # Index of the 'waist' joint in the JointState 'name' list
            self.joint_positions = list(self.joint_states.position[self.waist_index:(self.resp.num_joints+self.waist_index)])       # Holds the desired joint positions
            self.T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, self.joint_positions)                                  # Transformation matrix describing the pose of the end-effector w.r.t. the Space frame
            tmr_controller = rospy.Timer(rospy.Duration(0.02), self.controller)                                                     # ROS Timer to check gripper position
        if self.use_time:
            rospy.loginfo("\nRobot Name: %s\nRobot Model: %s\nMoving Time: %.2f seconds\nAcceleration Time: %.2f seconds\nGripper Pressure: %d%%\nDrive Mode: Time-Based-Profile" % (robot_name, robot_model, moving_time, accel_time, gripper_pressure * 100))
        else:
            rospy.loginfo("\nRobot Name: %s\nRobot Model: %s\nMax Velocity: %d \nMax Acceleration: %d\nGripper Pressure: %d%%\nDrive Mode: Velocity-Based-Profile" % (robot_name, robot_model, moving_time, accel_time, gripper_pressure * 100))
        rospy.sleep(1)                                                                                                              # Give time for the ROS Publishers to get set up

# ******************************** PRIVATE FUNCTIONS - DO NOT CALL THEM ********************************

    ### @brief ROS Subscriber Callback function to update the latest arm joint states
    ### @param msg - latest JointState message