Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion ackermann_cmd_mux/launch/reconfigure.launch
Original file line number Diff line number Diff line change
Expand Up @@ -5,4 +5,4 @@
<node name="$(anon dynparam)" pkg="dynamic_reconfigure" type="dynparam" args="set_from_parameters nodelet_manager">
<param name="yaml_cfg_file" type="string" value="$(find ackermann_cmd_mux)/param/reconfigure.yaml" />
</node>
</launch>
</launch>
8 changes: 4 additions & 4 deletions ackermann_cmd_mux/launch/standalone.launch
Original file line number Diff line number Diff line change
@@ -1,20 +1,20 @@
<!--
Example standalone launcher for the ackermann command multiplexer.

For best results you would usually load this into the core control system
to avoid latency for reactive controllers. However for other apps, running
it standalone with a similar configuration will also work.
-->
<launch>
<arg name="nodelet_manager_name" default="nodelet_manager"/>
<arg name="config_file" default="$(find ackermann_cmd_mux)/param/example.yaml"/>

<!-- nodelet manager -->
<node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager"/>

<!-- ackermann command mulitplexer -->
<include file="$(find ackermann_cmd_mux)/launch/ackermann_cmd_mux.launch">
<arg name="nodelet_manager_name" value="$(arg nodelet_manager_name)"/>
<arg name="config_file" value="$(arg config_file)"/>
</include>
</launch>
</launch>
4 changes: 2 additions & 2 deletions ackermann_cmd_mux/param/example.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
# name: Source name
# topic: The topic that provides ackermann_cmd messages
# timeout: Time in seconds without incoming messages to consider this topic inactive
# priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT
# priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT
# short_desc: Short description (optional)

subscribers:
Expand Down Expand Up @@ -39,4 +39,4 @@ subscribers:
topic: "input/keyop"
timeout: 0.1
priority: 7
publisher: "output/ackermann_cmd"
publisher: "output/ackermann_cmd"
14 changes: 7 additions & 7 deletions ackermann_cmd_mux/src/throttle_interpolator.py
Original file line number Diff line number Diff line change
Expand Up @@ -32,14 +32,14 @@ def __init__(self):
# Variables
self.last_rpm = 0
self.desired_rpm = self.last_rpm

self.last_servo = rospy.get_param('/vesc/steering_angle_to_servo_offset')
self.desired_servo_position = self.last_servo

# Create topic subscribers and publishers
self.rpm_output = rospy.Publisher(self.rpm_output_topic, Float64,queue_size=1)
self.servo_output = rospy.Publisher(self.servo_output_topic, Float64,queue_size=1)

rospy.Subscriber(self.rpm_input_topic, Float64, self._process_throttle_command)
rospy.Subscriber(self.servo_input_topic, Float64, self._process_servo_command)

Expand All @@ -48,7 +48,7 @@ def __init__(self):

self.max_delta_rpm = abs(self.speed_to_erpm_gain * self.max_acceleration / self.throttle_smoother_rate)
rospy.Timer(rospy.Duration(1.0/self.max_delta_rpm), self._publish_throttle_command)

# run the node
self._run()

Expand All @@ -60,10 +60,10 @@ def _publish_throttle_command(self, evt):
desired_delta = self.desired_rpm-self.last_rpm
clipped_delta = max(min(desired_delta, self.max_delta_rpm), -self.max_delta_rpm)
smoothed_rpm = self.last_rpm + clipped_delta
self.last_rpm = smoothed_rpm
self.last_rpm = smoothed_rpm
# print self.desired_rpm, smoothed_rpm
self.rpm_output.publish(Float64(smoothed_rpm))

def _process_throttle_command(self,msg):
input_rpm = msg.data
# Do some sanity clipping
Expand All @@ -74,7 +74,7 @@ def _publish_servo_command(self, evt):
desired_delta = self.desired_servo_position-self.last_servo
clipped_delta = max(min(desired_delta, self.max_delta_servo), -self.max_delta_servo)
smoothed_servo = self.last_servo + clipped_delta
self.last_servo = smoothed_servo
self.last_servo = smoothed_servo
self.servo_output.publish(Float64(smoothed_servo))

def _process_servo_command(self,msg):
Expand All @@ -84,7 +84,7 @@ def _process_servo_command(self,msg):
# set the target servo position
self.desired_servo_position = input_servo

# Boilerplate node spin up.
# Boilerplate node spin up.
if __name__ == '__main__':
try:
rospy.init_node('Throttle_Interpolator')
Expand Down