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
class LinearActuator(object):
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 Shutdown(self):
rospy.loginfo("Shutting down the linear actuator command driver...")
self.comm.Close()
self.tx_queue_.close()
self.rx_queue_.close()
def UpdateVelLimit(self, new_limit):
cmds = [
LINEAR_ACTUATOR_VELOCITY_LIMIT_CMD_ID,
[convert_float_to_u32(new_limit)]
]
self._add_command_to_queue(cmds)
def _add_command_to_queue(self, command):
"""
Create a byte array with the CRC from the command
"""
cmd_bytes = generate_cmd_bytes(command)
"""
Send it
"""
self.tx_queue_.put(cmd_bytes)
def _add_motion_command_to_queue(self, command):
"""
Add the command to the queue, platform does command limiting and mapping
"""
cmds = [
LINEAR_ACTUATOR_POSITION_CMD_ID,
[convert_float_to_u32(command.desired_position_m)]
]
self._add_command_to_queue(cmds)
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")
class MovoDriver:
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 Shutdown(self):
with self.terminate_mutex:
self.need_to_terminate = True
rospy.loginfo(
"Movo Driver has called the Shutdown method, terminating")
for i in range(len(self.s)):
self.s[i].unregister()
self.faultlog_pub.unregister()
self.movo_data.Shutdown()
self._linear.Shutdown()
self.comm.Close()
self.tx_queue_.close()
self.rx_queue_.close()
def _run(self):
while not self.need_to_terminate:
"""
Run until signaled to stop
Perform the actions defined based on the flags passed out
"""
result = select.select([self.rx_queue_._reader], [], [], 0.02)
if len(result[0]) > 0:
data = result[0][0].recv()
with self.terminate_mutex:
if not self.need_to_terminate:
self._handle_rsp(data)
def _add_command_to_queue(self, command):
"""
Create a byte array with the CRC from the command
"""
cmd_bytes = generate_cmd_bytes(command)
"""
Send it
"""
self.tx_queue_.put(cmd_bytes)
def _update_rcv_frq(self):
if (True == self.start_frequency_samp):
self.samp += 1
self.summer += 1.0 / (rospy.Time.now().to_sec() -
self.last_rsp_rcvd)
self.avg_freq = self.summer / self.samp
self.last_rsp_rcvd = rospy.Time.now().to_sec()
def _handle_rsp(self, data_bytes):
if (True == self.flush_rcvd_data) or (True == rospy.is_shutdown()):
return
if (self.extracting_faultlog):
valid_data = validate_response(
data_bytes, ((NUMBER_OF_FAULTLOG_WORDS + 1) * 4))
else:
valid_data = validate_response(
data_bytes, ((NUMBER_OF_MOVO_RSP_WORDS + 1) * 4))
if (False == valid_data):
rospy.logerr("bad movo data packet")
return
rsp_data = array.array('I', data_bytes.tostring()).tolist()
rsp_data = rsp_data[:(len(rsp_data) - 1)]
if (self.extracting_faultlog):
self.extracting_faultlog = False
faultlog_msg = Faultlog()
faultlog_msg.data = rsp_data
self.faultlog_pub.publish(faultlog_msg)
else:
header_stamp = self.movo_data.status.parse(
rsp_data[START_STATUS_BLOCK:END_STATUS_BLOCK])
wheel_circum = self.movo_data.config_param.parse(
rsp_data[START_APP_CONFIG_BLOCK:END_FRAM_CONFIG_BLOCK],
header_stamp)
self.movo_data.auxiliary_power.parse(
rsp_data[START_BATTERY_DATA_BLOCK:END_BATTERY_DATA_BLOCK],
header_stamp)
self.movo_data.propulsion.parse(
rsp_data[
START_PROPULSION_DATA_BLOCK:END_PROPULSION_DATA_BLOCK],
header_stamp)
self.movo_data.dynamics.parse(
rsp_data[START_DYNAMICS_DATA_BLOCK:END_DYNAMICS_DATA_BLOCK],
header_stamp, wheel_circum)
self.movo_data.imu.parse_data(
rsp_data[START_IMU_DATA_BLOCK:END_IMU_DATA_BLOCK],
header_stamp)
self._update_rcv_frq()
rospy.logdebug("feedback received from movo")
def _add_motion_command_to_queue(self, command):
"""
Add the command to the queue, platform does command limiting and mapping
"""
cmds = [
MOTION_CMD_ID,
[
convert_float_to_u32(command.linear.x),
convert_float_to_u32(command.linear.y),
convert_float_to_u32(command.angular.z)
]
]
self._add_command_to_queue(cmds)
def _add_config_command_to_queue(self, command):
try:
cmds = [
GENERAL_PURPOSE_CMD_ID,
[command_ids[command.gp_cmd], command.gp_param]
]
self._add_command_to_queue(cmds)
except:
rospy.logerr("Config param failed, it is probably not known")
return
def _add_motion_test_command_to_queue(self, command):
test = command.test_type & ~MOTION_TEST_TYPE_MASK
if (0 != test):
rospy.logerr("Bad test command see system_defines.py for details")
cmds = [
MOTION_TEST_CMD_ID,
[
command.test_type, command.duration_sec,
convert_float_to_u32(command.magnitude)
]
]
rospy.loginfo("MOTION_TEST IS GOING TO BE SENT!!!!!!!!!!!!!!")
self._add_command_to_queue(cmds)
def _dyn_reconfig_callback(self, config, level):
"""
The first time through we want to ignore the values because they are just defaults from the ROS
parameter server which has no knowledge of the platform being used
"""
if (True == self.is_init):
self.is_init = False
return config
"""
Create the configuration bitmap from the appropriate variables
"""
config_bitmap = ((
(config.motion_while_charging ^ 1) << DISABLE_AC_PRESENT_CSI_SHIFT)
| (config.motion_ctl_input_filter <<
MOTION_MAPPING_FILTER_SHIFT))
"""
Define the configuration parameters for all the platforms
"""
self.valid_config_cmd = [
LOAD_MACH_CONFIG_CMD_ID,
[
convert_float_to_u32(config.x_vel_limit_mps),
convert_float_to_u32(config.y_vel_limit_mps),
convert_float_to_u32(config.accel_limit_mps2),
convert_float_to_u32(config.decel_limit_mps2),
convert_float_to_u32(config.dtz_decel_limit_mps2),
convert_float_to_u32(config.yaw_rate_limit_rps),
convert_float_to_u32(config.yaw_accel_limit_rps2),
convert_float_to_u32(config.wheel_diameter_m),
convert_float_to_u32(config.wheel_base_length_m),
convert_float_to_u32(config.wheel_track_width_m),
convert_float_to_u32(config.gear_ratio), config_bitmap
]
]
rospy.loginfo("Reconfigure Requested!")
rospy.loginfo("x_vel_limit_mps: %f" % config.x_vel_limit_mps)
rospy.loginfo("y_vel_limit_mps: %f" % config.y_vel_limit_mps)
rospy.loginfo("accel_limit_mps2: %f" % config.accel_limit_mps2)
rospy.loginfo("decel_limit_mps2: %f" % config.decel_limit_mps2)
rospy.loginfo("dtz_decel_limit_mps2: %f" %
config.dtz_decel_limit_mps2)
rospy.loginfo("yaw_rate_limit_rps: %f" %
config.yaw_rate_limit_rps)
rospy.loginfo("yaw_accel_limit_rps2: %f" %
config.yaw_accel_limit_rps2)
rospy.loginfo("wheel_diameter_m: %f" % config.wheel_diameter_m)
rospy.loginfo("wheel_base_length_m: %f" %
config.wheel_base_length_m)
rospy.loginfo("wheel_track_width_m: %f" %
config.wheel_track_width_m)
rospy.loginfo("gear_ratio: %f" % config.gear_ratio)
rospy.loginfo("motion_while_charging: %u" %
config.motion_while_charging)
rospy.loginfo("motion_ctl_input_filter: %u" %
config.motion_ctl_input_filter)
rospy.loginfo("strafe_correction_factor: %u" %
config.strafe_correction_factor)
rospy.loginfo("yaw_correction_factor: %u" %
config.yaw_correction_factor)
"""
The teleop limits are always the minimum of the actual machine limit and the ones set for teleop
"""
config.teleop_x_vel_limit_mps = minimum_f(
config.teleop_x_vel_limit_mps, config.x_vel_limit_mps)
config.teleop_y_vel_limit_mps = minimum_f(
config.teleop_y_vel_limit_mps, config.y_vel_limit_mps)
config.teleop_accel_limit_mps2 = minimum_f(
config.teleop_accel_limit_mps2, config.accel_limit_mps2)
config.teleop_yaw_rate_limit_rps = minimum_f(
config.teleop_yaw_rate_limit_rps, config.yaw_rate_limit_rps)
config.teleop_yaw_accel_limit_rps2 = minimum_f(
config.teleop_yaw_accel_limit_rps2, config.yaw_accel_limit_rps2)
config.teleop_linear_actuator_vel_limit = minimum_f(
config.teleop_linear_actuator_vel_limit,
config.linear_actuator_vel_limit_mps)
"""
Set the teleop configuration in the feedback
"""
self.movo_data.config_param.SetTeleopConfig([
config.teleop_x_vel_limit_mps, config.teleop_y_vel_limit_mps,
config.teleop_accel_limit_mps2, config.teleop_yaw_rate_limit_rps,
config.teleop_yaw_accel_limit_rps2, config.teleop_arm_vel_limit,
config.teleop_pan_tilt_vel_limit,
config.teleop_linear_actuator_vel_limit
])
"""
Update the linear actuator velocity limit
"""
self._linear.UpdateVelLimit(config.linear_actuator_vel_limit_mps)
if self.param_server_initialized:
if ((1 << 4) == (level & (1 << 4))):
rospy.sleep(0.1)
cmds = [
GENERAL_PURPOSE_CMD_ID,
[6,
convert_float_to_u32(config.strafe_correction_factor)]
]
self._add_command_to_queue(cmds)
rospy.sleep(0.1)
if ((1 << 5) == (level & (1 << 5))):
rospy.sleep(0.1)
cmds = [
GENERAL_PURPOSE_CMD_ID,
[7, convert_float_to_u32(config.yaw_correction_factor)]
]
self._add_command_to_queue(cmds)
rospy.sleep(0.1)
"""
Check and see if we need to store the parameters in NVM before we try, although the NVM is F-RAM
with unlimited read/write, uneccessarily setting the parameters only introduces risk for error
"""
if self.param_server_initialized:
load_params = False
for i in range(NUMBER_OF_CONFIG_PARAM_VARIABLES):
if (self.movo_data.config_param.configuration_feedback[i] !=
self.valid_config_cmd[1][i]):
load_params = True
if (True == load_params):
self._add_command_to_queue(self.valid_config_cmd)
rospy.loginfo("Sent config update command")
self.param_server_initialized = True
self.valid_config = config
self.update_base_local_planner = True
self._update_move_base_params(None)
return config
def _update_move_base_params(self, config):
"""
If parameter updates have not been called in the last 5 seconds allow the
subscriber callback to set them
"""
if ((rospy.Time.now().to_sec() - self.last_move_base_update) > 5.0):
self.update_base_local_planner = True
if self.update_base_local_planner:
self.update_base_local_planner = False
self.last_move_base_update = rospy.Time.now().to_sec()
try:
dyn_reconfigure_client = Client("/move_base/DWAPlannerROS",
timeout=1.0)
changes = dict()
changes['acc_lim_x'] = maximum_f(
self.valid_config.accel_limit_mps2,
self.valid_config.decel_limit_mps2)
changes['acc_lim_y'] = maximum_f(
self.valid_config.accel_limit_mps2,
self.valid_config.decel_limit_mps2)
changes['acc_lim_th'] = self.valid_config.yaw_accel_limit_rps2
changes['max_vel_x'] = self.valid_config.x_vel_limit_mps
changes['max_vel_y'] = self.valid_config.y_vel_limit_mps
changes['max_rot_vel'] = self.valid_config.yaw_rate_limit_rps
dyn_reconfigure_client.update_configuration(changes)
dyn_reconfigure_client.close()
except:
pass
rospy.loginfo(
"Movo Driver updated move_base parameters to match machine parameters"
)
def _continuous_data(self, start_cont):
set_continuous = [
GENERAL_PURPOSE_CMD_ID,
[GENERAL_PURPOSE_CMD_SEND_CONTINUOUS_DATA, start_cont]
]
ret = False
if (True == start_cont):
r = rospy.Rate(10)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) <
3.0) and (True == self.movo_data.status.init):
self._add_command_to_queue(set_continuous)
r.sleep()
ret = not self.movo_data.status.init
else:
r = rospy.Rate(5)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) <
3.0) and (False == ret):
self._add_command_to_queue(set_continuous)
rospy.sleep(0.6)
if ((rospy.Time.now().to_sec() - self.last_rsp_rcvd) > 0.5):
ret = True
r.sleep()
self.movo_data.status.init = True
return ret
def _extract_faultlog(self):
r = rospy.Rate(2)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) <
3.0) and (True == self.extracting_faultlog):
self._add_command_to_queue([
GENERAL_PURPOSE_CMD_ID, [GENERAL_PURPOSE_CMD_SEND_FAULTLOG, 0]
])
r.sleep()
return not self.extracting_faultlog
def _initial_param_force_update(self):
"""
Load all the parameters on the machine at startup; first check if they match, if they do continue.
Otherwise load them and check again.
"""
r = rospy.Rate(2)
start_time = rospy.get_time()
params_loaded = False
while ((rospy.get_time() - start_time) < 3.0) and (False
== params_loaded):
load_params = False
for i in range(NUMBER_OF_CONFIG_PARAM_VARIABLES):
if (self.movo_data.config_param.configuration_feedback[i] !=
self.valid_config_cmd[1][i]):
load_params = True
if (True == load_params):
self._add_command_to_queue(self.valid_config_cmd)
r.sleep()
else:
params_loaded = True
return params_loaded
class SegwayDriver:
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()
self._subs = []
"""
Make sure we have a valid platform
"""
self.platform = rospy.get_param('~platform', "RMP_110")
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
"""
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
self.comm = IoEthThread(('10.66.171.5', 8080),
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)
self._subs.append(
rospy.Subscriber("/segway/cmd_vel", Twist,
self._add_motion_command_to_queue))
self._subs.append(
rospy.Subscriber("/segway/gp_command", ConfigCmd,
self._add_config_command_to_queue))
self._subs.append(
rospy.Subscriber(
"/move_base/TrajectoryPlannerROS/parameter_updates", Config,
self._update_move_base_params))
self._subs.append(
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.Shutdown()
return
"""
Start the comm stream for Super Aux Power system. This
will just idle if there is no Super Aux attached
"""
self._super_aux = SuperAuxDriver()
"""
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.Shutdown()
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.Shutdown()
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 & 0x0F)]):
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.Shutdown()
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.Shutdown()
return
rospy.loginfo("Segway Driver is up and running")
cmds = [
GENERAL_PURPOSE_CMD_ID,
[GENERAL_PURPOSE_CMD_RESET_INTEGRATORS, 0x000000FF]
]
self._add_command_to_queue(cmds)
"""
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 Shutdown(self):
rospy.logerr("Segway Driver has called for shutdown, terminating")
try:
self._super_aux.Shutdown()
except:
pass
with self.terminate_mutex:
self.need_to_terminate = True
assert (self._rcv_thread)
self._rcv_thread.join()
self.rmp_data.unregister_topics()
self.comm.Close()
self.tx_queue_.close()
self.rx_queue_.close()
self.faultlog_pub.unregister()
for sub in self._subs:
sub.unregister()
def _run(self):
while not rospy.is_shutdown():
with self.terminate_mutex:
if (self.need_to_terminate):
break
"""
Run until signaled to stop
Perform the actions defined based on the flags passed out
"""
result = select.select([self.rx_queue_._reader], [], [], 0.5)
if len(result[0]) > 0:
try:
data = result[0][0].recv()
self._handle_rsp(data)
except:
rospy.logdebug("select did not return interface data")
self.comm.Close()
self.tx_queue_.close()
self.rx_queue_.close()
def _add_command_to_queue(self, command):
"""
Create a byte array with the CRC from the command
"""
cmd_bytes = generate_cmd_bytes(command)
"""
Send it
"""
self.tx_queue_.put(cmd_bytes)
def _update_rcv_frq(self):
if (True == self.start_frequency_samp):
self.samp += 1
self.summer += 1.0 / (rospy.Time.now().to_sec() -
self.last_rsp_rcvd)
self.avg_freq = self.summer / self.samp
self.last_rsp_rcvd = rospy.Time.now().to_sec()
def _handle_rsp(self, data_bytes):
self._update_rcv_frq()
if (True == self.flush_rcvd_data):
return
valid_data, rsp_data = validate_response(data_bytes)
if (False == valid_data):
rospy.logerr("bad RMP packet")
return
if (self.extracting_faultlog):
if (len(rsp_data) == NUMBER_OF_FAULTLOG_WORDS):
self.extracting_faultlog = False
faultlog_msg = Faultlog()
faultlog_msg.data = rsp_data
self.faultlog_pub.publish(faultlog_msg)
elif (len(rsp_data) == NUMBER_OF_RMP_RSP_WORDS):
self.rmp_data.status.parse(
rsp_data[START_STATUS_BLOCK:END_STATUS_BLOCK])
self.rmp_data.auxiliary_power.parse(
rsp_data[START_AUX_POWER_BLOCK:END_AUX_POWER_BLOCK])
self.rmp_data.propulsion.parse(rsp_data[
START_PROPULSION_POWER_BLOCK:END_PROPULSION_POWER_BLOCK])
self.rmp_data.dynamics.parse(
rsp_data[START_DYNAMICS_BLOCK:END_DYNAMICS_BLOCK])
self.rmp_data.config_param.parse(
rsp_data[START_CONFIG_BLOCK:END_CONFIG_BLOCK])
self.rmp_data.imu.parse_data(
rsp_data[START_IMU_BLOCK:END_IMU_BLOCK])
rospy.logdebug("feedback received from rmp")
def _add_motion_command_to_queue(self, command):
"""
Add the command to the queue, platform does command limiting and mapping
"""
cmds = [
MOTION_CMD_ID,
[
convert_float_to_u32(command.linear.x),
convert_float_to_u32(command.linear.y),
convert_float_to_u32(command.angular.z)
]
]
self._add_command_to_queue(cmds)
def _add_config_command_to_queue(self, command):
try:
cmds = [
GENERAL_PURPOSE_CMD_ID,
[command_ids[command.gp_cmd], command.gp_param]
]
self._add_command_to_queue(cmds)
except:
rospy.logerr("Config param failed, it is probably not known")
return
def _dyn_reconfig_callback(self, config, level):
"""
The first time through we want to ignore the values because they are just defaults from the ROS
parameter server which has no knowledge of the platform being used
"""
if (True == self.is_init):
self.is_init = False
return config
"""
Create the configuration bitmap from the appropriate variables
"""
config_bitmap = (
((config.enable_audio ^ 1) << AUDIO_SILENCE_REQUEST_SHIFT) |
((config.motion_while_charging ^ 1) << DISABLE_AC_PRESENT_CSI_SHIFT
) | (config.balace_gains << BALANCE_GAIN_SCHEDULE_SHIFT) |
(config.balance_mode_enabled << BALANCE_MODE_LOCKOUT_SHIFT) |
(config.vel_ctl_input_filter << VEL_CTL_FILTER_SHIFT) |
(config.yaw_ctl_input_filter << YAW_CTL_FILTER_SHIFT) |
(config.imu_ahs_correction_enabled << ENABLE_IMU_AHS_CORR_SHIFT))
"""
Define the configuration parameters for all the platforms
"""
self.valid_config_cmd = [
LOAD_MACH_CONFIG_CMD_ID,
[
convert_float_to_u32(config.vel_limit_mps),
convert_float_to_u32(config.accel_limit_mps2),
convert_float_to_u32(config.decel_limit_mps2),
convert_float_to_u32(config.dtz_decel_limit_mps2),
convert_float_to_u32(config.yaw_rate_limit_rps),
convert_float_to_u32(config.yaw_accel_limit_rps2),
convert_float_to_u32(config.lateral_accel_limit_mps2),
convert_float_to_u32(config.tire_rolling_diameter_m),
convert_float_to_u32(config.wheel_base_length_m),
convert_float_to_u32(config.wheel_track_width_m),
convert_float_to_u32(config.omni_yaw_correction_factor),
convert_float_to_u32(config.omni_straffe_correction_factor),
convert_float_to_u32(config.gear_ratio), config_bitmap
]
]
rospy.loginfo("Reconfigure Requested!")
rospy.loginfo("vel_limit_mps: %f" %
config.vel_limit_mps)
rospy.loginfo("accel_limit_mps2: %f" %
config.accel_limit_mps2)
rospy.loginfo("decel_limit_mps2: %f" %
config.decel_limit_mps2)
rospy.loginfo("dtz_decel_limit_mps2: %f" %
config.dtz_decel_limit_mps2)
rospy.loginfo("yaw_rate_limit_rps: %f" %
config.yaw_rate_limit_rps)
rospy.loginfo("yaw_accel_limit_rps2: %f" %
config.yaw_accel_limit_rps2)
rospy.loginfo("lateral_accel_limit_mps2: %f" %
config.lateral_accel_limit_mps2)
rospy.loginfo("tire_rolling_diameter_m: %f" %
config.tire_rolling_diameter_m)
rospy.loginfo("wheel_base_length_m: %f" %
config.wheel_base_length_m)
rospy.loginfo("wheel_track_width_m: %f" %
config.wheel_track_width_m)
rospy.loginfo("omni_yaw_correction_factor: %f" %
config.omni_yaw_correction_factor)
rospy.loginfo("omni_straffe_correction_factor: %f" %
config.omni_yaw_correction_factor)
rospy.loginfo("gear_ratio: %f" %
config.gear_ratio)
rospy.loginfo("enable_audio: %u" %
config.enable_audio)
rospy.loginfo("motion_while_charging: %u" %
config.motion_while_charging)
rospy.loginfo("balance_mode_enabled: %u" %
config.balance_mode_enabled)
rospy.loginfo("balance_gain_schedule: %u" %
config.balace_gains)
rospy.loginfo("vel_ctl_input_filter: %u" %
config.vel_ctl_input_filter)
rospy.loginfo("yaw_ctl_input_filter: %u" %
config.yaw_ctl_input_filter)
rospy.loginfo("imu_ahs_correction_enabled: %u" %
config.imu_ahs_correction_enabled)
"""
The teleop limits are always the minimum of the actual machine limit and the ones set for teleop
"""
config.teleop_vel_limit_mps = minimum_f(config.teleop_vel_limit_mps,
config.vel_limit_mps)
config.teleop_accel_limit_mps2 = minimum_f(
config.teleop_accel_limit_mps2, config.accel_limit_mps2)
config.teleop_yaw_rate_limit_rps = minimum_f(
config.teleop_yaw_rate_limit_rps, config.yaw_rate_limit_rps)
config.teleop_yaw_accel_limit_rps2 = minimum_f(
config.teleop_yaw_accel_limit_rps2,
config.teleop_yaw_accel_limit_rps2)
"""
Set the teleop configuration in the feedback
"""
self.rmp_data.config_param.SetTeleopConfig([
config.teleop_vel_limit_mps, config.teleop_accel_limit_mps2,
config.teleop_yaw_rate_limit_rps,
config.teleop_yaw_accel_limit_rps2
])
"""
Check and see if we need to store the parameters in NVM before we try, although the NVM is F-RAM
with unlimited read/write, uneccessarily setting the parameters only introduces risk for error
"""
if self.param_server_initialized:
load_params = False
for i in range(NUMBER_OF_CONFIG_PARAM_VARIABLES - 4):
if (self.rmp_data.config_param.configuration_feedback[i] !=
self.valid_config_cmd[1][i]):
load_params = True
if (True == load_params):
self._add_command_to_queue(self.valid_config_cmd)
rospy.loginfo("Sent config update command")
"""
Just update the peak torque as it is not a persistant command
"""
if ((1 << 19) == ((1 << 19) & level)):
rospy.loginfo("level is %u" % level)
cmd = [
GENERAL_PURPOSE_CMD_ID,
[
GENERAL_PURPOSE_CMD_SET_TORQUE_LIMIT,
convert_float_to_u32(config.torqe_limit / 100.0)
]
]
self._add_command_to_queue(cmd)
self.param_server_initialized = True
self.valid_config = config
self.update_base_local_planner = True
self._update_move_base_params(None)
return config
def _update_move_base_params(self, config):
"""
If parameter updates have not been called in the last 5 seconds allow the
subscriber callback to set them
"""
if ((rospy.Time.now().to_sec() - self.last_move_base_update) > 5.0):
self.update_base_local_planner = True
if self.update_base_local_planner:
self.update_base_local_planner = False
self.last_move_base_update = rospy.Time.now().to_sec()
try:
dyn_reconfigure_client = Client(
"/move_base/TrajectoryPlannerROS", timeout=1.0)
changes = dict()
changes['acc_lim_x'] = minimum_f(
self.valid_config.accel_limit_mps2,
self.valid_config.decel_limit_mps2)
changes[
'acc_lim_theta'] = self.valid_config.yaw_accel_limit_rps2
changes['max_vel_x'] = self.valid_config.vel_limit_mps
changes['max_vel_theta'] = self.valid_config.yaw_rate_limit_rps
changes[
'min_vel_theta'] = -self.valid_config.yaw_rate_limit_rps
dyn_reconfigure_client.update_configuration(changes)
dyn_reconfigure_client.close()
except:
pass
try:
dyn_reconfigure_client = Client("/move_base/DWAPlannerROS",
timeout=1.0)
changes = dict()
changes['acc_lim_x'] = minimum_f(
self.valid_config.accel_limit_mps2,
self.valid_config.decel_limit_mps2)
changes['acc_lim_th'] = self.valid_config.yaw_accel_limit_rps2
changes['max_vel_x'] = self.valid_config.vel_limit_mps
changes['max_rot_vel'] = self.valid_config.yaw_rate_limit_rps
dyn_reconfigure_client.update_configuration(changes)
dyn_reconfigure_client.close()
except:
pass
rospy.loginfo(
"Segway Driver updated move_base parameters to match machine parameters"
)
def _continuous_data(self, start_cont):
set_continuous = [
GENERAL_PURPOSE_CMD_ID,
[GENERAL_PURPOSE_CMD_SEND_CONTINUOUS_DATA, start_cont]
]
ret = False
if (True == start_cont):
r = rospy.Rate(10)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) <
3.0) and (True == self.rmp_data.status.init):
self._add_command_to_queue(set_continuous)
r.sleep()
ret = not self.rmp_data.status.init
else:
r = rospy.Rate(5)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) <
3.0) and (False == ret):
self._add_command_to_queue(set_continuous)
if ((rospy.Time.now().to_sec() - self.last_rsp_rcvd) > 0.1):
ret = True
r.sleep()
self.rmp_data.status.init = True
return ret
def _extract_faultlog(self):
r = rospy.Rate(2)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) <
3.0) and (True == self.extracting_faultlog):
self._add_command_to_queue([
GENERAL_PURPOSE_CMD_ID,
[GENERAL_PURPOSE_CMD_SEND_SP_FAULTLOG, 0]
])
r.sleep()
return not self.extracting_faultlog
def _initial_param_force_update(self):
"""
Load all the parameters on the machine at startup; first check if they match, if they do continue.
Otherwise load them and check again.
"""
r = rospy.Rate(2)
start_time = rospy.Time.now().to_sec()
load_params = True
while ((rospy.Time.now().to_sec() - start_time) <
3.0) and (True == load_params):
load_params = False
for i in range(NUMBER_OF_CONFIG_PARAM_VARIABLES - 4):
if (self.rmp_data.config_param.configuration_feedback[i] !=
self.valid_config_cmd[1][i]):
load_params = True
if (True == load_params):
self._add_command_to_queue(self.valid_config_cmd)
r.sleep()
return not load_params
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
class PanTiltIO(object):
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 Shutdown(self):
with self.terminate_mutex:
self.need_to_terminate = True
rospy.loginfo(
"Movo pan_tilt has called the Shutdown method, terminating")
self.js_pub.unregister()
self.actuator_pub.unregister()
self.s.unregister()
self._jcs_pub.unregister()
self._jcc.unregister()
self._jc_srv.shutdown()
self.comm.Close()
self.tx_queue_.close()
self.rx_queue_.close()
def _run(self):
while not self.need_to_terminate:
"""
Run until signaled to stop
Perform the actions defined based on the flags passed out
"""
result = select.select([self.rx_queue_._reader], [], [], 0.02)
if len(result[0]) > 0:
data = result[0][0].recv()
with self.terminate_mutex:
if not self.need_to_terminate:
self._handle_rsp(data)
def _handle_state_query(self, req):
tmp = QueryTrajectoryStateResponse()
tmp.name = ['pan_joint', 'tilt_joint']
tmp.position = [
self.actuator_data.pan.pos_rad, self.actuator_data.tilt.pos_rad
]
tmp.velocity = [
self.actuator_data.pan.vel_rps, self.actuator_data.tilt.vel_rps
]
tmp.acceleration = [0.0, 0.0]
return tmp
def _update_command_queue(self, event):
"""
Send it
"""
if not self._cmd_buffer.empty():
tmp = self._cmd_buffer.get_nowait()
cmds = [
KINOVA_ACTUATOR_CMD_ID,
[
convert_float_to_u32(tmp.positions[0]),
convert_float_to_u32(tmp.velocities[0]),
convert_float_to_u32(tmp.accelerations[0]),
convert_float_to_u32(tmp.positions[1]),
convert_float_to_u32(tmp.velocities[1]),
convert_float_to_u32(tmp.accelerations[1])
]
]
self._last_cmd = tmp
cmd_bytes = generate_cmd_bytes(cmds)
self.tx_queue_.put(cmd_bytes)
def _add_motion_command_to_queue(self, command):
tmp = JointTrajectory()
tmp.header = command.header
tmp.joint_names = ['pan_joint', 'pan_joint']
tmp.points = [JointTrajectoryPoint()]
tmp.points[0].positions = [
command.pan_cmd.pos_rad, command.tilt_cmd.pos_rad
]
tmp.points[0].velocities = [
command.pan_cmd.vel_rps, command.tilt_cmd.vel_rps
]
tmp.points[0].accelerations = [
command.pan_cmd.acc_rps2, command.tilt_cmd.acc_rps2
]
self._cmd_buffer.put(tmp.points[0])
def _add_traj_command_to_queue(self, msg):
for pnt in msg.points:
self._cmd_buffer.put(pnt)
def _add_config_command_to_queue(self, gp_cmd, gp_param):
try:
cmds = [EIB_GENERAL_CMD_ID, [gp_cmd, gp_param]]
cmd_bytes = generate_cmd_bytes(cmds)
self.tx_queue_.put(cmd_bytes)
except:
rospy.logerr("Config param failed, it is probably not known")
return
def _handle_rsp(self, data_bytes):
valid_data = validate_response(data_bytes,
KINOVA_ACTUATOR_RSP_SIZE_BYTES)
if (False == valid_data):
self.last_rsp_rcvd = rospy.get_time()
rospy.logerr("bad movo pan_tilt data packet")
return
self.last_rsp_rcvd = rospy.get_time()
data_bytes = data_bytes[2:]
rsp_data = array.array('I', data_bytes.tostring()).tolist()
rsp_data = rsp_data[:(len(rsp_data) - 1)]
rsp = [convert_u32_to_float(i) for i in rsp_data]
self.actuator_data.header.stamp = rospy.get_rostime()
self.actuator_data.header.seq += 1
for i in range(2):
if (i == 0):
self.actuator_data.pan.current = (rsp[10 * i])
self.actuator_data.pan.pos_rad = (rsp[10 * i + 1])
self.actuator_data.pan.vel_rps = (rsp[10 * i + 2])
self.actuator_data.pan.torque_nm = (rsp[10 * i + 3])
self.actuator_data.pan.pwm = (rsp[10 * i + 4])
self.actuator_data.pan.encoder_rad = (rsp[10 * i + 5])
self.actuator_data.pan.accel.x = (rsp[10 * i + 6])
self.actuator_data.pan.accel.y = (rsp[10 * i + 7])
self.actuator_data.pan.accel.z = (rsp[10 * i + 8])
self.actuator_data.pan.temperature_degC = (rsp[10 * i + 9])
else:
self.actuator_data.tilt.current = (rsp[10 * i])
self.actuator_data.tilt.pos_rad = (rsp[10 * i + 1])
self.actuator_data.tilt.vel_rps = (rsp[10 * i + 2])
self.actuator_data.tilt.torque_nm = (rsp[10 * i + 3])
self.actuator_data.tilt.pwm = (rsp[10 * i + 4])
self.actuator_data.tilt.encoder_rad = (rsp[10 * i + 5])
self.actuator_data.tilt.accel.x = (rsp[10 * i + 6])
self.actuator_data.tilt.accel.y = (rsp[10 * i + 7])
self.actuator_data.tilt.accel.z = (rsp[10 * i + 8])
self.actuator_data.tilt.temperature_degC = (rsp[10 * i + 9])
if not rospy.is_shutdown():
self.actuator_pub.publish(self.actuator_data)
self.js.header.stamp = self.actuator_data.header.stamp
self.js.name = ['pan_joint', 'tilt_joint']
self.js.position = [
self.actuator_data.pan.pos_rad, self.actuator_data.tilt.pos_rad
]
self.js.velocity = [
self.actuator_data.pan.vel_rps, self.actuator_data.tilt.vel_rps
]
self.js.effort = [
self.actuator_data.pan.torque_nm,
self.actuator_data.tilt.torque_nm
]
self.js_pub.publish(self.js)
self._jcs.header.stamp = self.actuator_data.header.stamp
self._jcs.joint_names = ['pan_joint', 'tilt_joint']
self._jcs.desired = self._last_cmd
self._jcs.actual.positions = self.js.position
self._jcs.actual.velocities = self.js.velocity
self._jcs.actual.accelerations = [0.0, 0.0]
try:
self._jcs.error.positions = map(operator.sub,
self._jcs.desired.positions,
self._jcs.actual.positions)
self._jcs.error.velocities = map(operator.sub,
self._jcs.desired.velocities,
self._jcs.actual.velocities)
except:
self._jcs.error.positions = [0.0] * 2
self._jcs.error.velocities = [0.0] * 2
self._jcs.desired = self._jcs.actual
self._last_cmd = self._jcs.actual
self._jcs.error.accelerations = [0.0, 0.0]
self._jcs_pub.publish(self._jcs)
rospy.logdebug("feedback received from movo")
def _continuous_data(self, start_cont):
ret = False
if (True == start_cont):
r = rospy.Rate(10)
start_time = rospy.get_time()
while ((rospy.get_time() - start_time) < 3.0) and (False == ret):
self._add_config_command_to_queue(0, 1)
r.sleep()
if ((rospy.get_time() - self.last_rsp_rcvd) < 0.05):
ret = True
else:
start_time = rospy.get_time()
while ((rospy.get_time() - start_time) < 3.0) and (False == ret):
self._add_config_command_to_queue(0, 0)
rospy.sleep(0.6)
if ((rospy.get_time() - self.last_rsp_rcvd) > 0.5):
ret = True
return ret
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")
class VectorDriver:
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")
def Shutdown(self):
with self.terminate_mutex:
self.need_to_terminate = True
rospy.loginfo("Vector Driver has called the Shutdown method, terminating")
self._linear.Shutdown()
self.comm.Close()
self.tx_queue_.close()
self.rx_queue_.close()
def _run(self):
while not self.need_to_terminate:
"""
Run until signaled to stop
Perform the actions defined based on the flags passed out
"""
result = select.select([self.rx_queue_._reader],[],[],0.02)
if len(result[0]) > 0:
data = result[0][0].recv()
with self.terminate_mutex:
if not self.need_to_terminate:
self._handle_rsp(data)
def _add_command_to_queue(self,command):
"""
Create a byte array with the CRC from the command
"""
cmd_bytes = generate_cmd_bytes(command)
"""
Send it
"""
self.tx_queue_.put(cmd_bytes)
def _update_rcv_frq(self):
if (True == self.start_frequency_samp):
self.samp+=1
self.summer+=1.0/(rospy.Time.now().to_sec() - self.last_rsp_rcvd)
self.avg_freq = self.summer/self.samp
self.last_rsp_rcvd = rospy.Time.now().to_sec()
def _handle_rsp(self,data_bytes):
self._update_rcv_frq()
if (True == self.flush_rcvd_data):
return
valid_data,rsp_data = validate_response(data_bytes)
if (False == valid_data):
rospy.logerr("bad vector data packet")
return
if (self.extracting_faultlog):
if (len(rsp_data) == NUMBER_OF_FAULTLOG_WORDS):
self.extracting_faultlog = False
faultlog_msg = Faultlog()
faultlog_msg.data = rsp_data
self.faultlog_pub.publish(faultlog_msg)
elif (len(rsp_data) == NUMBER_OF_VECTOR_RSP_WORDS):
header_stamp = self.vector_data.status.parse(rsp_data[START_STATUS_BLOCK:END_STATUS_BLOCK])
wheel_circum = self.vector_data.config_param.parse(rsp_data[START_APP_CONFIG_BLOCK:END_FRAM_CONFIG_BLOCK],header_stamp)
self.vector_data.auxiliary_power.parse(rsp_data[START_BATTERY_DATA_BLOCK:END_BATTERY_DATA_BLOCK],header_stamp)
self.vector_data.propulsion.parse(rsp_data[START_PROPULSION_DATA_BLOCK:END_PROPULSION_DATA_BLOCK],header_stamp)
self.vector_data.dynamics.parse(rsp_data[START_DYNAMICS_DATA_BLOCK:END_DYNAMICS_DATA_BLOCK],header_stamp,wheel_circum)
self.vector_data.imu.parse_data(rsp_data[START_IMU_DATA_BLOCK:END_IMU_DATA_BLOCK],header_stamp)
rospy.logdebug("feedback received from vector")
def _add_motion_command_to_queue(self,command):
"""
Add the command to the queue, platform does command limiting and mapping
"""
cmds = [MOTION_CMD_ID,[convert_float_to_u32(command.linear.x),
convert_float_to_u32(command.linear.y),
convert_float_to_u32(command.angular.z)]]
if (False == self.parameter_server_is_updating):
self._add_command_to_queue(cmds)
def _add_config_command_to_queue(self,command):
try:
cmds = [GENERAL_PURPOSE_CMD_ID,[command_ids[command.gp_cmd],command.gp_param]]
self._add_command_to_queue(cmds)
except:
rospy.logerr("Config param failed, it is probably not known")
return
def _dyn_reconfig_callback(self,config,level):
"""
Note: for some reason the commands collide (queue gets garbled) if the
motion commands and reconfiguration are sent at the same time; so
we just gate the motion commands when reconfiguring which should be OK in general
since most result in zero velocity on the embedded side. Need to dig in further
to understand why this is happening
"""
self.parameter_server_is_updating = True
rospy.sleep(0.1)
"""
The first time through we want to ignore the values because they are just defaults from the ROS
parameter server which has no knowledge of the platform being used
"""
if (True == self.is_init):
self.is_init = False
return config
"""
Create the configuration bitmap from the appropriate variables
"""
config_bitmap = (((config.motion_while_charging^1) << DISABLE_AC_PRESENT_CSI_SHIFT)|
(config.motion_ctl_input_filter << MOTION_MAPPING_FILTER_SHIFT))
"""
Define the configuration parameters for all the platforms
"""
self.valid_config_cmd = [LOAD_MACH_CONFIG_CMD_ID,
[convert_float_to_u32(config.x_vel_limit_mps),
convert_float_to_u32(config.y_vel_limit_mps),
convert_float_to_u32(config.accel_limit_mps2),
convert_float_to_u32(config.decel_limit_mps2),
convert_float_to_u32(config.dtz_decel_limit_mps2),
convert_float_to_u32(config.yaw_rate_limit_rps),
convert_float_to_u32(config.yaw_accel_limit_rps2),
convert_float_to_u32(config.wheel_diameter_m),
convert_float_to_u32(config.wheel_base_length_m),
convert_float_to_u32(config.wheel_track_width_m),
convert_float_to_u32(config.gear_ratio),
config_bitmap]]
"""
The teleop limits are always the minimum of the actual machine limit and the ones set for teleop
"""
config.teleop_x_vel_limit_mps = minimum_f(config.teleop_x_vel_limit_mps, config.x_vel_limit_mps)
config.teleop_y_vel_limit_mps = minimum_f(config.teleop_y_vel_limit_mps, config.x_vel_limit_mps)
config.teleop_accel_limit_mps2 = minimum_f(config.teleop_accel_limit_mps2, config.accel_limit_mps2)
config.teleop_yaw_rate_limit_rps = minimum_f(config.teleop_yaw_rate_limit_rps, config.yaw_rate_limit_rps)
config.teleop_yaw_accel_limit_rps2 = minimum_f(config.teleop_yaw_accel_limit_rps2, config.teleop_yaw_accel_limit_rps2)
"""
Set the teleop configuration in the feedback
"""
self.vector_data.config_param.SetTeleopConfig([config.teleop_x_vel_limit_mps,
config.teleop_y_vel_limit_mps,
config.teleop_accel_limit_mps2,
config.teleop_yaw_rate_limit_rps,
config.teleop_yaw_accel_limit_rps2])
"""
Define the configuration parameters for all the platforms
"""
self.valid_pid_cmd = [SET_PID_CONFIG_CMD_ID,
[convert_float_to_u32(config.p_gain_rps_per_rps),
convert_float_to_u32(config.i_gain_rps_per_rad),
convert_float_to_u32(config.d_gain_rps_per_rps2),
convert_float_to_u32(config.fdfwd_gain_rps_per_motor_rps),
convert_float_to_u32(config.p_error_limit_rps),
convert_float_to_u32(config.i_error_limit_rad),
convert_float_to_u32(config.d_error_limit_rps2),
convert_float_to_u32(config.i_error_drain_rate_rad_per_frame),
convert_float_to_u32(config.output_limit_rps),
convert_float_to_u32(config.input_target_limit_rps)]]
"""
Update the linear actuator velocity limit
"""
"""
Check and see if we need to store the parameters in NVM before we try, although the NVM is F-RAM
with unlimited read/write, uneccessarily setting the parameters only introduces risk for error
"""
load_config_params = False
if self.param_server_initialized:
if ((1<<7) == ((1<<7) & level)):
self._linear.UpdateVelLimit(config.linear_actuator_vel_limit_mps)
if ((1<<2) == ((1<<2) & level)):
self._force_machine_configuration()
load_config_params = True
config.x_vel_limit_mps = round(self.vector_data.config_param.machcfg.x_vel_limit_mps,3)
config.y_vel_limit_mps = round(self.vector_data.config_param.machcfg.y_vel_limit_mps,3)
config.accel_limit_mps2 = round(self.vector_data.config_param.machcfg.accel_limit_mps2,3)
config.decel_limit_mps2 = round(self.vector_data.config_param.machcfg.decel_limit_mps2,3)
config.dtz_decel_limit_mps2 = round(self.vector_data.config_param.machcfg.dtz_decel_limit_mps2,3)
config.yaw_rate_limit_rps = round(self.vector_data.config_param.machcfg.yaw_rate_limit_rps,3)
config.yaw_accel_limit_rps2 = round(self.vector_data.config_param.machcfg.yaw_accel_limit_rps2,3)
config.wheel_diameter_m = round(self.vector_data.config_param.machcfg.wheel_diameter_m,3)
config.wheel_base_length_m = round(self.vector_data.config_param.machcfg.wheelbase_length_m,3)
config.wheel_track_width_m = round(self.vector_data.config_param.machcfg.wheel_track_width_m,3)
config.gear_ratio = round(self.vector_data.config_param.machcfg.gear_ratio,3)
config.motion_while_charging = ((self.vector_data.config_param.machcfg.config_bitmap >> DISABLE_AC_PRESENT_CSI_SHIFT) & 1) ^ 1
config.motion_ctl_input_filter = (self.vector_data.config_param.machcfg.config_bitmap >> MOTION_MAPPING_FILTER_SHIFT) & VALID_MOTION_MAPPING_FILTER_MASK
if self.param_server_initialized:
if ((1<<5) == ((1<<5) & level)):
if self.vector_data.config_param.ctlconfig.control_tuning_unlocked:
self._force_pid_configuration()
if (True == config.set_default_gains):
cmds = [GENERAL_PURPOSE_CMD_ID,[2002,0]]
self._add_command_to_queue(cmds)
config.set_default_gains = False
rospy.sleep(0.1)
config.p_gain_rps_per_rps = round(self.vector_data.config_param.ctlconfig.p_gain_rps_per_rps,3)
config.i_gain_rps_per_rad = round(self.vector_data.config_param.ctlconfig.i_gain_rps_per_rad,3)
config.d_gain_rps_per_rps2 = round(self.vector_data.config_param.ctlconfig.d_gain_rps_per_rps2,3)
config.fdfwd_gain_rps_per_motor_rps = round(self.vector_data.config_param.ctlconfig.fdfwd_gain_rps_per_motor_rps,3)
config.p_error_limit_rps = round(self.vector_data.config_param.ctlconfig.p_error_limit_rps,3)
config.i_error_limit_rad = round(self.vector_data.config_param.ctlconfig.i_error_limit_rad,3)
config.d_error_limit_rps2 = round(self.vector_data.config_param.ctlconfig.d_error_limit_rps2,3)
config.i_error_drain_rate_rad_per_frame = round(self.vector_data.config_param.ctlconfig.i_error_drain_rate_rad_per_frame,5)
config.output_limit_rps = round(self.vector_data.config_param.ctlconfig.output_limit_rps,3)
config.input_target_limit_rps = round(self.vector_data.config_param.ctlconfig.input_target_limit_rps,3)
if (True == config.send_unlock_request):
try:
key = int(config.unlock_key,16)
self._unlock_gain_tuning(key)
except:
rospy.logerr("Invalid Key Value!!!! Must be a string in 32bit hex format")
config.send_unlock_request = False
self.valid_config = config
if (True == load_config_params) or (False == self.param_server_initialized):
self.update_base_local_planner = True
self._update_move_base_params()
self.param_server_initialized = True
self.parameter_server_is_updating = False
return config
def _update_move_base_params(self):
"""
If parameter updates have not been called in the last 5 seconds allow the
subscriber callback to set them
"""
if ((rospy.Time.now().to_sec()-self.last_move_base_update) > 5.0):
self.update_base_local_planner = True
if self.update_base_local_planner:
self.update_base_local_planner = False
self.last_move_base_update = rospy.Time.now().to_sec()
try:
dyn_reconfigure_client= Client("/move_base/DWAPlannerROS",timeout=1.0)
changes = dict()
changes['acc_lim_x'] = maximum_f(self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2)
changes['acc_lim_y'] = maximum_f(self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2)
changes['acc_lim_th'] = self.valid_config.yaw_accel_limit_rps2
changes['max_vel_x'] = self.valid_config.x_vel_limit_mps
changes['max_vel_y'] = self.valid_config.y_vel_limit_mps
changes['max_rot_vel'] = self.valid_config.yaw_rate_limit_rps
dyn_reconfigure_client.update_configuration(changes)
dyn_reconfigure_client.close()
except:
pass
rospy.loginfo("Vector Driver updated move_base parameters to match machine parameters")
def _unlock_gain_tuning(self,key):
r = rospy.Rate(10)
attempts = 0
while (False == self.vector_data.config_param.ctlconfig.control_tuning_unlocked) and (attempts < 3):
cmds = [GENERAL_PURPOSE_CMD_ID,[2001,key]]
self._add_command_to_queue(cmds)
attempts += 1
r.sleep()
if (True == self.vector_data.config_param.ctlconfig.control_tuning_unlocked):
rospy.loginfo("Controller tuning successfully unlocked")
else:
rospy.logerr("Failed to unlock controller tuning, the key is likely incorrect")
def _continuous_data(self,start_cont):
set_continuous = [GENERAL_PURPOSE_CMD_ID,[GENERAL_PURPOSE_CMD_SEND_CONTINUOUS_DATA,start_cont]]
ret = False
if (True == start_cont):
r = rospy.Rate(10)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (True == self.vector_data.status.init):
self._add_command_to_queue(set_continuous)
r.sleep()
ret = not self.vector_data.status.init
else:
r = rospy.Rate(5)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (False == ret):
self._add_command_to_queue(set_continuous)
rospy.sleep(0.6)
if ((rospy.Time.now().to_sec() - self.last_rsp_rcvd) > 0.5):
ret = True
r.sleep()
self.vector_data.status.init = True
return ret
def _extract_faultlog(self):
r = rospy.Rate(2)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (True == self.extracting_faultlog):
self._add_command_to_queue([GENERAL_PURPOSE_CMD_ID,[GENERAL_PURPOSE_CMD_SEND_FAULTLOG,0]])
r.sleep()
return not self.extracting_faultlog
def _force_machine_configuration(self,echo=False):
"""
Load all the parameters on the machine at startup; first check if they match, if they do continue.
Otherwise load them and check again.
"""
r = rospy.Rate(5)
start_time = rospy.get_time()
params_loaded = False
new_params = False
while ((rospy.get_time() - start_time) < 10.0) and (False == params_loaded):
load_params = False
for i in range(NUMBER_OF_CONFIG_PARAM_VARIABLES):
if (self.vector_data.config_param.configuration_feedback[i] != self.valid_config_cmd[1][i]):
load_params = True
if (True == load_params):
self._add_command_to_queue(self.valid_config_cmd)
new_params = True
r.sleep()
else:
params_loaded = True
if (new_params == True) or (echo == True):
rospy.loginfo("New Machine Parameters Loaded.......")
rospy.loginfo("x_vel_limit_mps: %.4f"%self.vector_data.config_param.machcfg.x_vel_limit_mps)
rospy.loginfo("y_vel_limit_mps: %.4f"%self.vector_data.config_param.machcfg.y_vel_limit_mps)
rospy.loginfo("accel_limit_mps2: %.4f"%self.vector_data.config_param.machcfg.accel_limit_mps2)
rospy.loginfo("decel_limit_mps2: %.4f"%self.vector_data.config_param.machcfg.decel_limit_mps2)
rospy.loginfo("dtz_decel_limit_mps2: %.4f"%self.vector_data.config_param.machcfg.dtz_decel_limit_mps2)
rospy.loginfo("yaw_rate_limit_rps: %.4f"%self.vector_data.config_param.machcfg.yaw_rate_limit_rps)
rospy.loginfo("yaw_accel_limit_rps2: %.4f"%self.vector_data.config_param.machcfg.yaw_accel_limit_rps2)
rospy.loginfo("wheel_diameter_m: %.4f"%self.vector_data.config_param.machcfg.wheel_diameter_m)
rospy.loginfo("wheel_base_length_m: %.4f"%self.vector_data.config_param.machcfg.wheelbase_length_m)
rospy.loginfo("wheel_track_width_m: %.4f"%self.vector_data.config_param.machcfg.wheel_track_width_m)
rospy.loginfo("gear_ratio: %.4f"%self.vector_data.config_param.machcfg.gear_ratio)
rospy.loginfo("motion_while_charging: %u"%(((self.vector_data.config_param.machcfg.config_bitmap >> DISABLE_AC_PRESENT_CSI_SHIFT) & 1) ^ 1))
rospy.loginfo("motion_ctl_input_filter: %u\n"%((self.vector_data.config_param.machcfg.config_bitmap >> MOTION_MAPPING_FILTER_SHIFT) & VALID_MOTION_MAPPING_FILTER_MASK))
elif not params_loaded:
rospy.logerr("Failed to load machine parameters!!!!!!!!!!")
return params_loaded
def _force_pid_configuration(self,echo=False):
r = rospy.Rate(5)
start_time = rospy.get_time()
params_loaded = False
new_params = False
while ((rospy.get_time() - start_time) < 10.0) and (False == params_loaded):
load_params = False
for i in range(16,(16+NUMBER_OF_PID_PARAM_VARIABLES)):
if (self.vector_data.config_param.configuration_feedback[i] != self.valid_pid_cmd[1][i-16]):
load_params = True
if (True == load_params):
self._add_command_to_queue(self.valid_pid_cmd)
r.sleep()
new_params = True
else:
params_loaded = True
if (new_params == True) or (echo == True):
rospy.loginfo("New PID Parameters Loaded.......")
rospy.loginfo("p_gain_rps_per_rps: %.4f"%self.vector_data.config_param.ctlconfig.p_gain_rps_per_rps)
rospy.loginfo("i_gain_rps_per_rps: %.4f"%self.vector_data.config_param.ctlconfig.i_gain_rps_per_rad)
rospy.loginfo("d_gain_rps_per_rps: %.4f"%self.vector_data.config_param.ctlconfig.d_gain_rps_per_rps2)
rospy.loginfo("fdfwd_gain_rps_per_motor_rps: %.4f"%self.vector_data.config_param.ctlconfig.fdfwd_gain_rps_per_motor_rps)
rospy.loginfo("p_error_limit_rps: %.4f"%self.vector_data.config_param.ctlconfig.p_error_limit_rps)
rospy.loginfo("i_error_limit_rps: %.4f"%self.vector_data.config_param.ctlconfig.i_error_limit_rad)
rospy.loginfo("d_error_limit_rps: %.4f"%self.vector_data.config_param.ctlconfig.d_error_limit_rps2)
rospy.loginfo("i_error_drain_rate_rad_per_frame: %.4f"%self.vector_data.config_param.ctlconfig.i_error_drain_rate_rad_per_frame)
rospy.loginfo("output_limit_rps: %.4f"%self.vector_data.config_param.ctlconfig.output_limit_rps)
rospy.loginfo("input_target_limit_rps: %.4f\n"%self.vector_data.config_param.ctlconfig.input_target_limit_rps)
elif not params_loaded:
rospy.logerr("Failed to load machine parameters!!!!!!!!!!")
return params_loaded
class SuperAuxDriver(object):
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 Shutdown(self):
with self.terminate_mutex:
self.need_to_terminate = True
rospy.loginfo("Super Aux interface has called the Shutdown method, terminating")
self.battery_pub.unregister()
self.comm.Close()
self.tx_queue_.close()
self.rx_queue_.close()
def _run(self):
while not self.need_to_terminate:
"""
Run until signaled to stop
Perform the actions defined based on the flags passed out
"""
result = select.select([self.rx_queue_._reader],[],[],0.02)
if len(result[0]) > 0:
data = result[0][0].recv()
if (SUPER_AUX_PACKET_SIZE == len(data)):
with self.terminate_mutex:
if not self.need_to_terminate:
self._handle_rsp(data)
def _handle_rsp(self,data_bytes):
valid_data,rsp_data = validate_response(data_bytes)
if (False == valid_data):
self.last_rsp_rcvd = rospy.get_time()
rospy.logerr("bad super aux data packet")
return
self.last_rsp_rcvd = rospy.get_time()
rsp = [convert_u32_to_float(i) for i in rsp_data]
rsp[7] = rsp_data[7]
self.battery_data.header.stamp = rospy.get_rostime()
self.battery_data.header.seq +=1
self.battery_data.battery_state_of_charge_percent = rsp[0]
self.battery_data.battery_current_A0pk = rsp[1]
self.battery_data.battery_voltage_V = rsp[2]
self.battery_data.max_cell_temp_degc = rsp[3]
self.battery_data.max_pcba_temp_degc = rsp[4]
self.battery_data.max_cell_voltage_V = rsp[5]
self.battery_data.min_cell_voltage_V = rsp[6]
self.battery_data.battery_interface_status_bits = rsp[7]
if not rospy.is_shutdown():
self.battery_pub.publish(self.battery_data)
rospy.logdebug("feedback received from super aux")
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()
"""
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._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("Vector Driver is up and running")
