def __init__(self, movo_ip='10.66.171.5'):
self.init_success = False
"""
Create the thread to run MOVO Linear actuator command interface
"""
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
self.comm = IoEthThread((movo_ip, 6236),
self.tx_queue_,
self.rx_queue_,
max_packet_size=1248)
if (False == self.comm.link_up):
rospy.logerr(
"Could not open socket for MOVO linear actuator...exiting")
self.Shutdown()
return
"""
Initialize the publishers and subscribers for the node
"""
self.cmd_data = LinearActuatorCmd()
self.s = rospy.Subscriber("/movo/linear_actuator_cmd",
LinearActuatorCmd,
self._add_motion_command_to_queue)
self.init_success = True
def __init__(self,rmp_ip='10.66.171.5'):
self.init_success = False
"""
Create the thread to run the super aux interface
"""
self._cmd_buffer = multiprocessing.Queue()
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
self.comm = IoEthThread((rmp_ip,6236),
self.tx_queue_,
self.rx_queue_,
max_packet_size=SUPER_AUX_PACKET_SIZE)
if (False == self.comm.link_up):
rospy.logerr("Could not open socket for super aux...exiting")
self.Shutdown()
return
"""
Initialize the publishers and subscribers for the node
"""
self.battery_data = SuperAux()
self.battery_pub = rospy.Publisher("/segway/feedback/super_aux", SuperAux, queue_size=10)
"""
Start the receive handler thread
"""
self.need_to_terminate = False
self.terminate_mutex = threading.RLock()
self.last_rsp_rcvd = rospy.get_time()
self._rcv_thread = threading.Thread(target = self._run)
self._rcv_thread.start()
rospy.loginfo("Super Aux interface is up and running")
self.init_success = True
def __init__(self):
self.init_success = False
"""
Create the thread to run VECTOR Linear actuator command interface
"""
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
self.comm = IoEthThread((os.environ['VECTOR_IP_ADDRESS'], 6236),
self.tx_queue_,
self.rx_queue_,
max_packet_size=1248)
if (False == self.comm.link_up):
rospy.logerr(
"Could not open socket for VECTOR linear actuator...exiting")
self.Shutdown()
return
"""
Initialize the publishers and subscribers for the node
"""
self.cmd_data = LinearActuatorCmd()
rospy.Subscriber("/vector/linear_actuator_cmd", LinearActuatorCmd,
self._add_motion_command_to_queue)
self.init_success = True
def __init__(self):
"""
Variables to track communication frequency for debugging
"""
self.summer = 0
self.samp = 0
self.avg_freq = 0.0
self.start_frequency_samp = False
self.need_to_terminate = False
self.flush_rcvd_data = True
self.update_base_local_planner = False
self.last_move_base_update = rospy.Time.now().to_sec()
"""
Make sure we have a valid platform
"""
self.platform = rospy.get_param('~platform', None)
if not (self.platform in SUPPORTED_PLATFORMS):
rospy.logerr("Platform defined is not supported: %s",
self.platform)
return
"""
The 440 platforms use the same drivers but different URDF
"""
pattern = re.compile("RMP_440")
if not (None == pattern.search(self.platform)):
self.platform = "RMP_440"
"""
Initialize the publishers for RMP
"""
self.rmp_data = RMP_DATA()
"""
Initialize faultlog related items
"""
self.is_init = True
self.extracting_faultlog = False
"""
Initialize the dynamic reconfigure server for RMP
"""
self.param_server_initialized = False
self.dyn_reconfigure_srv = Server(segwayConfig,
self._dyn_reconfig_callback)
"""
Wait for the parameter server to set the configs and then set the IP address from that.
Note that this must be the current ethernet settings of the platform. If you want to change it
set the ethernet settings at launch to the current ethernet settings, power up, change them, power down, set the
the ethernet settings at launch to the new ones and relaunch
"""
r = rospy.Rate(10)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) <
3.0) and (False == self.param_server_initialized):
r.sleep()
if (False == self.param_server_initialized):
rospy.logerr(
"Parameter server not found, you must pass an initial yaml in the launch! exiting..."
)
return
"""
Create the thread to run RMP communication
"""
interface = rospy.get_param('~interface', 'eth')
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
if ('eth' == interface):
self.comm = IoEthThread((os.environ['SEGWAY_IP_ADDRESS'],
int(os.environ['SEGWAY_IP_PORT_NUM'])),
self.tx_queue_,
self.rx_queue_,
max_packet_size=1248)
elif ('usb' == interface):
self.comm = IoUsbThread('ttyACM0',
self.tx_queue_,
self.rx_queue_,
max_packet_size=1248)
if (False == self.comm.link_up):
rospy.logerr("Could not open socket for RMP...")
self.comm.close()
return
"""
Initialize the publishers and subscribers for the node
"""
self.faultlog_pub = rospy.Publisher('/segway/feedback/faultlog',
Faultlog,
queue_size=10,
latch=True)
rospy.Subscriber("/segway/cmd_vel", Twist,
self._add_motion_command_to_queue)
rospy.Subscriber("/segway/gp_command", ConfigCmd,
self._add_config_command_to_queue)
rospy.Subscriber("/move_base/TrajectoryPlannerROS/parameter_updates",
Config, self._update_move_base_params)
rospy.Subscriber("/move_base/DWAPlannerROS/parameter_updates", Config,
self._update_move_base_params)
"""
Start the receive handler thread
"""
self.terminate_mutex = threading.RLock()
self.last_rsp_rcvd = rospy.Time.now().to_sec()
self._rcv_thread = threading.Thread(target=self._run)
self._rcv_thread.start()
"""
Start streaming continuous data
"""
rospy.loginfo("Stopping the data stream")
if (False == self._continuous_data(False)):
rospy.logerr("Could not stop RMP communication stream")
self.__del__()
return
"""
Extract the faultlog at startup
"""
self.flush_rcvd_data = False
rospy.loginfo("Extracting the faultlog")
self.extracting_faultlog = True
if (False == self._extract_faultlog()):
rospy.logerr("Could not get retrieve RMP faultlog")
self.__del__()
return
"""
Start streaming continuous data
"""
rospy.loginfo("Starting the data stream")
if (False == self._continuous_data(True)):
rospy.logerr("Could not start RMP communication stream")
self.__del__()
return
self.start_frequency_samp = True
"""
Ensure that the platform is the right one, we don't want to allow the wrong platform because
some things depend on knowing which platform is operating
"""
if (self.platform != PLATFORM_IDS[(
self.rmp_data.status.platform_identifier & 0x1F)]):
rospy.logerr(
"Platform ID is not correct!!! Platform reports %(1)s, user set %(2)s..."
% {
"1":
PLATFORM_IDS[(self.rmp_data.status.platform_identifier
& 0x1F)],
"2":
self.platform
})
self.__del__()
return
"""
Force the configuration to update the first time to ensure that the variables are set to
the correct values on the machine
"""
if (False == self._initial_param_force_update()):
rospy.logerr(
"Initial configuration parameteters my not be valid, please check them in the yaml file"
)
rospy.logerr(
"The ethernet address must be set to the present address at startup, to change it:"
)
rospy.logerr(
"start the machine; change the address using rqt_reconfigure; shutdown; update the yaml and restart"
)
self.__del__()
return
rospy.loginfo("Segway Driver is up and running")
"""
Indicate the driver is up with motor audio
"""
cmds = [
GENERAL_PURPOSE_CMD_ID,
[
GENERAL_PURPOSE_CMD_SET_AUDIO_COMMAND,
MOTOR_AUDIO_PLAY_EXIT_ALARM_SONG
]
]
self._add_command_to_queue(cmds)
def __init__(self, movo_ip='10.66.171.5'):
"""
Variables to track communication frequency for debugging
"""
self.summer = 0
self.samp = 0
self.avg_freq = 0.0
self.start_frequency_samp = False
self.need_to_terminate = False
self.flush_rcvd_data = True
self.update_base_local_planner = False
self.last_move_base_update = rospy.get_time()
"""
Initialize the publishers for MOVO
"""
self.movo_data = MOVO_DATA()
"""
Start the thread for the linear actuator commands
"""
self._linear = LinearActuator(movo_ip)
if (False == self._linear.init_success):
rospy.logerr(
"Could not initialize the linear actuator interface! exiting..."
)
return
"""
Initialize faultlog related items
"""
self.is_init = True
self.extracting_faultlog = False
"""
Initialize the dynamic reconfigure server for MOVO
"""
self.param_server_initialized = False
self.dyn_reconfigure_srv = Server(movoConfig,
self._dyn_reconfig_callback)
"""
Wait for the parameter server to set the configs and then set the IP address from that.
Note that this must be the current ethernet settings of the platform. If you want to change it
set the ethernet settings at launch to the current ethernet settings, power up, change them, power down, set the
the ethernet settings at launch to the new ones and relaunch
"""
r = rospy.Rate(10)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) <
3.0) and (False == self.param_server_initialized):
r.sleep()
if (False == self.param_server_initialized):
rospy.logerr(
"Parameter server not found, you must pass an initial yaml in the launch! exiting..."
)
return
"""
Create the thread to run MOVO communication
"""
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
self.comm = IoEthThread((movo_ip, 8080),
self.tx_queue_,
self.rx_queue_,
max_packet_size=1248)
if (False == self.comm.link_up):
rospy.logerr("Could not open socket for MOVO...")
self.comm.close()
return
"""
Initialize the publishers and subscribers for the node
"""
self.s = [0] * 4
self.faultlog_pub = rospy.Publisher('/movo/feedback/faultlog',
Faultlog,
queue_size=10,
latch=True)
self.s[0] = rospy.Subscriber("/movo/cmd_vel", Twist,
self._add_motion_command_to_queue)
self.s[1] = rospy.Subscriber("/movo/gp_command", ConfigCmd,
self._add_config_command_to_queue)
self.s[2] = rospy.Subscriber(
"/move_base/DWAPlannerROS/parameter_updates", Config,
self._update_move_base_params)
self.s[3] = rospy.Subscriber("/movo/motion_test_cmd", MotionTestCmd,
self._add_motion_test_command_to_queue)
"""
Start the receive handler thread
"""
self.terminate_mutex = threading.RLock()
self.last_rsp_rcvd = rospy.Time.now().to_sec()
self._rcv_thread = threading.Thread(target=self._run)
self._rcv_thread.start()
"""
Start streaming continuous data
"""
rospy.loginfo("Stopping the data stream")
if (False == self._continuous_data(False)):
rospy.logerr("Could not stop MOVO communication stream")
self.Shutdown()
return
"""
Extract the faultlog at startup
"""
self.flush_rcvd_data = False
rospy.loginfo("Extracting the faultlog")
self.extracting_faultlog = True
if (False == self._extract_faultlog()):
rospy.logerr("Could not get retrieve MOVO faultlog")
self.Shutdown()
return
"""
Start streaming continuous data
"""
rospy.loginfo("Starting the data stream")
if (False == self._continuous_data(True)):
rospy.logerr("Could not start MOVO communication stream")
self.Shutdown()
return
self.start_frequency_samp = True
"""
Force the configuration to update the first time to ensure that the variables are set to
the correct values on the machine
"""
if (False == self._initial_param_force_update()):
rospy.logerr(
"Initial configuration parameteters my not be valid, please check them in the yaml file"
)
rospy.logerr(
"The ethernet address must be set to the present address at startup, to change it:"
)
rospy.logerr(
"start the machine; change the address using rqt_reconfigure; shutdown; update the yaml and restart"
)
self.Shutdown()
return
rospy.loginfo("Movo Driver is up and running")
def __init__(self, movo_ip='10.66.171.5'):
self.init_success = False
"""
Create the thread to run MOVO Linear actuator command interface
"""
self._cmd_buffer = multiprocessing.Queue()
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
self.comm = IoEthThread((movo_ip, 6237),
self.tx_queue_,
self.rx_queue_,
max_packet_size=KINOVA_ACTUATOR_RSP_SIZE_BYTES)
if (False == self.comm.link_up):
rospy.logerr("Could not open socket for MOVO pan_tilt...exiting")
self.Shutdown()
return
"""
Initialize the publishers and subscribers for the node
"""
self.cmd_data = PanTiltCmd()
self._last_cmd = JointTrajectoryPoint()
self._init_cmds = True
self.s = rospy.Subscriber("/movo/head/cmd", PanTiltCmd,
self._add_motion_command_to_queue)
self._jcc = rospy.Subscriber("/movo/head_controller/command",
JointTrajectory,
self._add_traj_command_to_queue)
self.actuator_data = PanTiltFdbk()
self.actuator_pub = rospy.Publisher("/movo/head/data",
PanTiltFdbk,
queue_size=10)
self.js = JointState()
self.js_pub = rospy.Publisher("/movo/head/joint_states",
JointState,
queue_size=10)
self._jcs = JointTrajectoryControllerState()
self._jcs_pub = rospy.Publisher("/movo/head_controller/state",
JointTrajectoryControllerState,
queue_size=10)
self._jc_srv = rospy.Service('/movo/head_controller/query_state',
QueryTrajectoryState,
self._handle_state_query)
"""
Start the receive handler thread
"""
self.need_to_terminate = False
self.terminate_mutex = threading.RLock()
self.last_rsp_rcvd = rospy.get_time()
self._rcv_thread = threading.Thread(target=self._run)
self._rcv_thread.start()
self._t1 = rospy.Timer(rospy.Duration(0.01),
self._update_command_queue)
"""
Start streaming continuous data
"""
rospy.loginfo("Stopping the data stream")
if (False == self._continuous_data(False)):
rospy.logerr("Could not stop MOVO pan_tilt communication stream")
self.Shutdown()
return
"""
Start streaming continuous data
"""
rospy.loginfo("Starting the data stream")
if (False == self._continuous_data(True)):
rospy.logerr("Could not start MOVO pan_tilt communication stream")
self.Shutdown()
return
rospy.loginfo("Movo Pan-Tilt Head Driver is up and running")
self.init_success = True
def __init__(self):
"""
Variables to track communication frequency for debugging
"""
self.summer=0
self.samp = 0
self.avg_freq = 0.0
self.start_frequency_samp = False
self.need_to_terminate = False
self.flush_rcvd_data=True
self.update_base_local_planner = False
self.parameter_server_is_updating = False
self.last_move_base_update = rospy.Time.now().to_sec()
"""
Initialize the publishers for VECTOR
"""
self.vector_data = VECTOR_DATA()
"""
Start the thread for the linear actuator commands
"""
self._linear = LinearActuator()
if (False == self._linear.init_success):
rospy.logerr("Could not initialize the linear actuator interface! exiting...")
return
"""
Initialize faultlog related items
"""
self.is_init = True
self.extracting_faultlog = False
"""
Initialize the dynamic reconfigure server for VECTOR
"""
self.param_server_initialized = False
self.dyn_reconfigure_srv = Server(vectorConfig, self._dyn_reconfig_callback)
"""
Wait for the parameter server to set the configs and then set the IP address from that.
Note that this must be the current ethernet settings of the platform. If you want to change it
set the ethernet settings at launch to the current ethernet settings, power up, change them, power down, set the
the ethernet settings at launch to the new ones and relaunch
"""
r = rospy.Rate(10)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (False == self.param_server_initialized):
r.sleep()
if (False == self.param_server_initialized):
rospy.logerr("Parameter server not found, you must pass an initial yaml in the launch! exiting...")
return
"""
Create the thread to run VECTOR communication
"""
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
self.comm = IoEthThread((os.environ['VECTOR_IP_ADDRESS'],int(os.environ['VECTOR_IP_PORT_NUM'])),
self.tx_queue_,
self.rx_queue_,
max_packet_size=1248)
if (False == self.comm.link_up):
rospy.logerr("Could not open socket for VECTOR...")
self.comm.close()
return
"""
Initialize the publishers and subscribers for the node
"""
self.faultlog_pub = rospy.Publisher('/vector/feedback/faultlog', Faultlog, queue_size=10,latch=True)
rospy.Subscriber("/vector/cmd_vel", Twist, self._add_motion_command_to_queue)
rospy.Subscriber("/vector/gp_command",ConfigCmd,self._add_config_command_to_queue)
rospy.Subscriber("/move_base/DWAPlannerROS/parameter_updates",Config,self._update_move_base_params)
"""
Start the receive handler thread
"""
self.terminate_mutex = threading.RLock()
self.last_rsp_rcvd = rospy.Time.now().to_sec()
self._rcv_thread = threading.Thread(target = self._run)
self._rcv_thread.start()
"""
Start streaming continuous data
"""
rospy.loginfo("Stopping the data stream")
if (False == self._continuous_data(False)):
rospy.logerr("Could not stop VECTOR communication stream")
self.Shutdown()
return
"""
Extract the faultlog at startup
"""
self.flush_rcvd_data=False
rospy.loginfo("Extracting the faultlog")
self.extracting_faultlog = True
if (False == self._extract_faultlog()):
rospy.logerr("Could not get retrieve VECTOR faultlog")
self.Shutdown()
return
"""
Start streaming continuous data
"""
rospy.loginfo("Starting the data stream")
if (False == self._continuous_data(True)):
rospy.logerr("Could not start VECTOR communication stream")
self.Shutdown()
return
self.start_frequency_samp = True
"""
Force the configuration to update the first time to ensure that the variables are set to
the correct values on the machine
"""
if (False == self._force_machine_configuration(True)):
rospy.logerr("Initial configuration parameteters my not be valid, please check them in the yaml file")
rospy.logerr("The ethernet address must be set to the present address at startup, to change it:")
rospy.logerr("start the machine; change the address using rqt_reconfigure; shutdown; update the yaml and restart")
self.Shutdown()
return
"""
Uncomment the line below to always unlock gain tuning and force the upload based on the parameters in the yaml file
you must have the key to do this because it can be dangerous if one sets unstable gains. 0x00000000 needs to be replaced
by the key
"""
#self._unlock_gain_tuning(0x00000000)
#self._force_pid_configuration(True)
"""
Finally update the values for dynamic reconfigure with the ones reported by the machine
"""
new_config = dict({"x_vel_limit_mps" : self.vector_data.config_param.machcfg.x_vel_limit_mps,
"y_vel_limit_mps" : self.vector_data.config_param.machcfg.y_vel_limit_mps,
"accel_limit_mps2" : self.vector_data.config_param.machcfg.accel_limit_mps2,
"dtz_decel_limit_mps2": self.vector_data.config_param.machcfg.dtz_decel_limit_mps2,
"yaw_rate_limit_rps" : self.vector_data.config_param.machcfg.yaw_rate_limit_rps,
"wheel_diameter_m": self.vector_data.config_param.machcfg.wheel_diameter_m,
"wheel_base_length_m": self.vector_data.config_param.machcfg.wheelbase_length_m,
"wheel_track_width_m": self.vector_data.config_param.machcfg.wheel_track_width_m,
"gear_ratio" : self.vector_data.config_param.machcfg.gear_ratio,
"motion_while_charging" : ((self.vector_data.config_param.machcfg.config_bitmap >> DISABLE_AC_PRESENT_CSI_SHIFT) & 1) ^ 1,
"motion_ctl_input_filter" : (self.vector_data.config_param.machcfg.config_bitmap >> MOTION_MAPPING_FILTER_SHIFT) & VALID_MOTION_MAPPING_FILTER_MASK,
"p_gain_rps_per_rps" : self.vector_data.config_param.ctlconfig.p_gain_rps_per_rps,
"i_gain_rps_per_rad" : self.vector_data.config_param.ctlconfig.i_gain_rps_per_rad,
"d_gain_rps_per_rps2" : self.vector_data.config_param.ctlconfig.d_gain_rps_per_rps2,
"fdfwd_gain_rps_per_motor_rps" : self.vector_data.config_param.ctlconfig.fdfwd_gain_rps_per_motor_rps,
"p_error_limit_rps" : self.vector_data.config_param.ctlconfig.p_error_limit_rps,
"i_error_limit_rad" : self.vector_data.config_param.ctlconfig.i_error_limit_rad,
"i_error_drain_rate_rad_per_frame" : self.vector_data.config_param.ctlconfig.i_error_drain_rate_rad_per_frame,
"input_target_limit_rps" : self.vector_data.config_param.ctlconfig.input_target_limit_rps,
"output_limit_rps" : self.vector_data.config_param.ctlconfig.input_target_limit_rps})
self.dyn_reconfigure_srv.update_configuration(new_config)
rospy.loginfo("Vector Driver is up and running")