t.korjakow/ft24_control
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

twist adjustments

Browse Source
This commit is contained in:
Your Name 2023-07-14 13:52:30 +02:00
parent 2cae6005f9
commit ff8d3211ff
8 changed files with 114 additions and 91 deletions
src
dcaiti_navigation
config
slam.yml
launch
start_nav2.py
params
nav2_params.yaml
dcaitirobot
dcaitirobot
joy.pytwist.py
embedded/src/communication
UARTCom.cpp
launch
launch_slam.py
m-explore-ros2

2
src/dcaiti_navigation/config/slam.yml

@ -2,5 +2,5 @@ slam_toolbox:
ros__parameters:
base_frame: base_link
map_update_interval: 1.0
scan_topic: /scan
scan_topic: /scan_no_nan
odom_topic: /diff_cont/odom

43
src/dcaiti_navigation/launch/start_nav2.py

@ -11,6 +11,8 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetE
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, LaunchConfiguration, PythonExpression
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node, SetRemap
from launch.actions import GroupAction
def generate_launch_description():
@ -57,7 +59,7 @@ def generate_launch_description():
declare_use_sim_time_cmd = DeclareLaunchArgument(
name='use_sim_time',
default_value='True',
default_value='False',
description='Use simulation (Gazebo) clock if true')
slam_params = os.path.join(
@ -67,10 +69,19 @@ def generate_launch_description():
)
# Launch the ROS 2 Navigation Stack
start_ros2_navigation_cmd = IncludeLaunchDescription(
start_ros2_navigation_cmd = GroupAction(
actions=[
SetRemap(src='/cmd_vel',dst='/cmd_vel_nav'),
IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('nav2_bringup'),'launch','navigation_launch.py')]))
get_package_share_directory('nav2_bringup'),'launch','navigation_launch.py')]),
launch_arguments={
'use_sim_time': 'false',
'-r': '/cmd_vel:=/cmd_vel_nav'
}.items()
)
]
)
@ -79,16 +90,33 @@ def generate_launch_description():
get_package_share_directory('slam_toolbox'),'launch','online_async_launch.py'
)]),
launch_arguments={
'use_sim_time': 'true',
'use_sim_time': 'false',
'slam_params_file': slam_params}.items()
)
node_fix_lidar = Node(
package="dcaitirobot",
executable="fixlidar",
name="fixlidar",
)
tfs1 = Node(package = "tf2_ros",
executable = "static_transform_publisher",
arguments = ["0", "0", "0" ,"0", "0", "0", "base_link", "velodyne"])
tfs2 = Node(package = "tf2_ros",
executable = "static_transform_publisher",
arguments = ["0", "0", "0" ,"0", "0", "0", "base_link", "base_footprint"])
exploration = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('explore_lite'),'launch','explore.launch.py'
)]),
launch_arguments={
'use_sim_time': 'true'}.items()
'use_sim_time': 'false'}.items()
)
# Create the launch description and populate
@ -101,6 +129,9 @@ def generate_launch_description():
declare_use_sim_time_cmd,
start_ros2_navigation_cmd,
slam_toolbox,
node_fix_lidar,
tfs1,
tfs2,
exploration,
])

46
src/dcaiti_navigation/params/nav2_params.yaml

@ -1,6 +1,6 @@
amcl:
ros__parameters:
use_sim_time: True
use_sim_time: False
alpha1: 0.002
alpha2: 0.002
alpha3: 0.002
@ -43,15 +43,15 @@ amcl:
amcl_map_client:
ros__parameters:
use_sim_time: True
use_sim_time: False
amcl_rclcpp_node:
ros__parameters:
use_sim_time: True
use_sim_time: False
bt_navigator:
ros__parameters:
use_sim_time: True
use_sim_time: False
global_frame: map
robot_base_frame: base_link
odom_topic: /diff_cont/odom #/wheel/odometry
@ -99,11 +99,11 @@ bt_navigator:
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: True
use_sim_time: False
controller_server:
ros__parameters:
use_sim_time: True
use_sim_time: False
controller_frequency: 2.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5
@ -174,7 +174,7 @@ controller_server:
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
use_sim_time: False
local_costmap:
local_costmap:
@ -183,7 +183,7 @@ local_costmap:
publish_frequency: 2.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: True
use_sim_time: False
rolling_window: true
width: 10
height: 10
@ -205,7 +205,7 @@ local_costmap:
mark_threshold: 0
observation_sources: scan
scan:
topic: /scan
topic: /scan_no_nan
max_obstacle_height: 2.0
clearing: True
marking: True
@ -219,10 +219,10 @@ local_costmap:
always_send_full_costmap: True
local_costmap_client:
ros__parameters:
use_sim_time: True
use_sim_time: False
local_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
use_sim_time: False
global_costmap:
global_costmap:
@ -231,7 +231,7 @@ global_costmap:
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
use_sim_time: False
robot_radius: 0.21
resolution: 0.05
track_unknown_space: true
@ -241,7 +241,7 @@ global_costmap:
enabled: True
observation_sources: scan
scan:
topic: /scan
topic: /scan_no_nan
max_obstacle_height: 2.0
clearing: True
marking: True
@ -260,19 +260,19 @@ global_costmap:
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
use_sim_time: True
use_sim_time: False
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
use_sim_time: False
map_server:
ros__parameters:
use_sim_time: True
use_sim_time: False
yaml_filename: "turtlebot3_world.yaml"
map_saver:
ros__parameters:
use_sim_time: True
use_sim_time: False
save_map_timeout: 5.0
free_thresh_default: 0.25
occupied_thresh_default: 0.65
@ -281,7 +281,7 @@ map_saver:
planner_server:
ros__parameters:
expected_planner_frequency: 2.0
use_sim_time: True
use_sim_time: False
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
@ -291,7 +291,7 @@ planner_server:
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: True
use_sim_time: False
recoveries_server:
ros__parameters:
@ -308,7 +308,7 @@ recoveries_server:
global_frame: odom
robot_base_frame: base_link
transform_timeout: 0.1
use_sim_time: true
use_sim_time: False
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
@ -316,7 +316,7 @@ recoveries_server:
robot_state_publisher:
ros__parameters:
use_sim_time: True
use_sim_time: False
waypoint_follower:
ros__parameters:
@ -341,8 +341,8 @@ slam_toolbox:
base_frame: base_link
map_update_interval: 0.1
scan_topic: /scan
odom_topic: /diff_cont/odom
scan_topic: /scan_no_nan
odom_topic: /odom
# if you'd like to immediately start continuing a map at a given pose
# or at the dock, but they are mutually exclusive, if pose is given

54
src/dcaitirobot/dcaitirobot/joy.py

@ -207,53 +207,11 @@ class JoystickController(Joystick):
self.__axis_update()
def __button_update(self):
print(
"button: {} \tvalue: {}".format(
self.update_id, self.getButtonState(self.update_id)
)
)
pass
def __axis_update(self):
"""Uses the left and right joysticks to produce a direction vector and a rotation vector.
On a traditional mecanum robot, it can locomote in all directions with no rotation. This is
why they are separate. The vectors are in polar coordinates.
"""
print(f"{self.update_id} {self.getAxisState(self.update_id)}")
# x = self.getAxisState(JSEvent.AXIS_LEFT_STICK_HORIZONTAL)
# # down and right is positive, so flip y axis
# y = -self.getAxisState(JSEvent.AXIS_LEFT_STICK_VERTICAL)
# # TODO: define SHRT_MAX (32767) as as large as the radius can get, so scale appropriately?
# # interestingly enough, we don't have a circular portion of the plane to work with, we can
# # get the largest vectors in the 45 degree direction. Think about how this will effect
# # scaling the vectors' magnitude
# # TODO: Think about headless - rotation being a difference from the current direction
# # or actually what it would take... This is actually a part of the *robots* locomotion
# # code, not the joystick. Do headless stuff there.
# # by headless I mean - there is no sense of front - the current direction of locomotion is
# # forwards.
# # TODO: Think about what kind of coordinate transformations would be useful...
# d = self.__to_polar(x, y)
# rotate_x = self.getAxisState(JSEvent.AXIS_RIGHT_STICK_HORIZONTAL)
# rotate_y = -self.getAxisState(JSEvent.AXIS_RIGHT_STICK_VERTICAL)
# r = self.__to_polar(rotate_x, rotate_y)
# self.direction_vector = d
# self.rotation_vector = r
# # self.difference_vector = (d[0] - r[0], d[1] - r[1])
# print('direction: ({0[0]:.2f}, {0[1]:.2f})'.format(self.direction_vector))
# print('rotation: ({0[0]:.2f}, {0[1]:.2f})'.format(self.rotation_vector))
# print('difference: ({0[0]:.2f}, {0[1]:.2f})\n'.format(self.difference_vector))
def __to_polar(self, x, y):
# since atan2 knows both y and x, it yields the angle in the proper quadrant
return sqrt(x**2 + y**2), degrees(atan2(y, x))
pass
class JoyNode(Node):
def __init__(
self,
@ -287,6 +245,7 @@ class JoyNode(Node):
def run(self):
msg = Joy()
msg.axes = [0.0, 0.0]
msg.buttons = [0]
with JoystickController("/dev/input/js0") as controller:
while rclpy.ok():
controller.Update()
@ -295,11 +254,16 @@ class JoyNode(Node):
if controller.hasUpdate():
hor = controller.getAxisState(JSEvent.AXIS_LEFT_STICK_HORIZONTAL)
ver = controller.getAxisState(JSEvent.AXIS_LEFT_STICK_VERTICAL)
button_change_mode = controller.getButtonState(JSEvent.BUTTON_CIRCLE) # it says circle but it actually is dpad up
self.get_logger().info(str(button_change_mode))
hor /= 2**15
ver /= 2**15
msg.axes[JSEvent.AXIS_LEFT_STICK_HORIZONTAL] = hor
msg.axes[JSEvent.AXIS_LEFT_STICK_VERTICAL] = ver
msg.buttons[0] = button_change_mode
self.pub.publish(msg)

35
src/dcaitirobot/dcaitirobot/twist.py

@ -24,6 +24,8 @@ class TwistPublisher(Node):
):
super().__init__("twist_publisher")
self.mode = "manual"
self.linear_window = np.zeros(smooth_weights.shape[0])
self.angular_window = np.zeros(smooth_weights.shape[0])
self.linear_recent = 0.0
@ -38,9 +40,9 @@ class TwistPublisher(Node):
self.linear_scale = linear_scale
self.angular_scale = angular_scale
self.publisher_ = self.create_publisher(
Twist, "/diff_cont/cmd_vel_unstamped", 1
)
# self.publisher_ = self.create_publisher(
# Twist, "/diff_cont/cmd_vel_unstamped", 1
# )
self.publisher_2 = self.create_publisher(Twist, "/cmd_vel_joystick", 1)
@ -52,6 +54,11 @@ class TwistPublisher(Node):
self.linear_recent = -y * SCALE_LINEAR_VELOCITY
self.angular_recent = x * SCALE_ANGULAR_VELOCITY
if joy_message.buttons[0]:
self.mode = "manual" if self.mode == "auto" else "auto"
self.get_logger().info(f"Switching to {self.mode!r} mode")
def update_smooth_window(self):
self.linear_window = np.append(self.linear_window, self.linear_recent)[
-self.window_size :
@ -65,6 +72,10 @@ class TwistPublisher(Node):
self.angular_out = np.average(self.angular_window, weights=self.weights)
def timer_callback(self):
if self.mode != "manual":
return
msg = Twist()
self.update_smooth_window()
@ -72,16 +83,16 @@ class TwistPublisher(Node):
msg.linear.x = self.linear_out
msg.angular.z = self.angular_out
self.publisher_.publish(msg)
# self.publisher_.publish(msg)
self.publisher_2.publish(msg)
self.get_logger().info(
"Linear_window: {}, Linear_out: {}, Angular_window: {}, Angular_out: {}".format(
self.linear_window,
self.linear_out,
self.angular_window,
self.angular_out,
)
)
# self.get_logger().info(
# "Linear_window: {}, Linear_out: {}, Angular_window: {}, Angular_out: {}".format(
# self.linear_window,
# self.linear_out,
# self.angular_window,
# self.angular_out,
# )
# )
def main(args=None):

2
src/dcaitirobot/embedded/src/communication/UARTCom.cpp

@ -171,7 +171,7 @@ void handleRequest (uint8_t* d)
break;
}
case DRIVE_ENCODER: {
UValue::sendBothEncoderValues(-wheelLeft.getGearedSpeedRPM(), wheelRight.getGearedSpeedRPM(), wheelLeft.getPosition(), wheelRight.getPosition());
UValue::sendBothEncoderValues(wheelRight.getGearedSpeedRPM(), -wheelLeft.getGearedSpeedRPM(), wheelRight.getPosition(), -wheelLeft.getPosition());
break;
}
case PID_PARAMETER: {

21
src/dcaitirobot/launch/launch_slam.py

@ -10,7 +10,8 @@ from ament_index_python import get_package_share_directory
def generate_launch_description():
slam_params = os.path.join(
str(Path(__file__).absolute().parent.parent), # <-- Replace with your package name
str(Path(__file__).absolute().parent.parent.parent), # <-- Replace with your package name
'dcaiti_navigation',
'config',
'slam.yml'
)
@ -24,6 +25,22 @@ def generate_launch_description():
'slam_params_file': slam_params}.items()
)
node_fix_lidar = Node(
package="dcaitirobot",
executable="fixlidar",
name="fixlidar",
)
tfs1 = Node(package = "tf2_ros",
executable = "static_transform_publisher",
arguments = ["0", "0", "0" ,"0", "0", "0", "base_link", "velodyne"])
tfs2 = Node(package = "tf2_ros",
executable = "static_transform_publisher",
arguments = ["0", "0", "0" ,"0", "0", "0", "base_link", "base_footprint"])
return LaunchDescription([
slam_toolbox
slam_toolbox, node_fix_lidar, tfs1, tfs2
])

2
src/m-explore-ros2

@ -1 +1 @@
Subproject commit 81fdebe979ddf142065cc276989124437976978b
Subproject commit e40e857b6af6e64f82514efdcd2cb9844ea77dc4