Merge branch 'main' of github.com:dcaiti-robot/dcaiti_ws
This commit is contained in:
Tim Korjakow
commit
0f25098175
|
|
@ -1,4 +1,6 @@
|
|||
slam_toolbox:
|
||||
ros__parameters:
|
||||
base_frame: base_link
|
||||
map_update_interval: 1.0
|
||||
map_update_interval: 1.0
|
||||
scan_topic: /scan_no_nan
|
||||
odom_topic: /odom_scaled
|
||||
|
|
|
|||
|
|
@ -0,0 +1,65 @@
|
|||
import rclpy
|
||||
from rclpy.node import Node
|
||||
|
||||
from geometry_msgs.msg import Twist
|
||||
from std_msgs.msg import String
|
||||
from nav_msgs.msg import Odometry
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
SCALE_FACTOR = 20.0
|
||||
|
||||
|
||||
|
||||
class TwistPublisher(Node):
|
||||
def __init__(
|
||||
self,
|
||||
scale_factor=20.0
|
||||
):
|
||||
super().__init__("twist_publisher")
|
||||
|
||||
self.scale_factor = scale_factor
|
||||
|
||||
self.publisher_ = self.create_publisher(
|
||||
Odometry, "/odom_scaled", 1
|
||||
)
|
||||
|
||||
self.create_subscription(Odometry, "/odom", self.odom_callback, 1)
|
||||
|
||||
def odom_callback(self, odom_message:Odometry):
|
||||
odom_message.twist.twist.linear.x *= self.scale_factor
|
||||
odom_message.twist.twist.linear.y *= self.scale_factor
|
||||
odom_message.twist.twist.linear.z *= self.scale_factor
|
||||
odom_message.twist.twist.angular.x *= self.scale_factor
|
||||
odom_message.twist.twist.angular.y *= self.scale_factor
|
||||
odom_message.twist.twist.angular.z *= self.scale_factor
|
||||
odom_message.pose.pose.position.x *= self.scale_factor
|
||||
odom_message.pose.pose.position.y *= self.scale_factor
|
||||
odom_message.pose.pose.position.z *= self.scale_factor
|
||||
odom_message.pose.pose.orientation.x *= self.scale_factor
|
||||
odom_message.pose.pose.orientation.y *= self.scale_factor
|
||||
odom_message.pose.pose.orientation.z *= self.scale_factor
|
||||
|
||||
self.publisher_.publish(odom_message)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
minimal_publisher = TwistPublisher(
|
||||
SCALE_FACTOR
|
||||
)
|
||||
|
||||
rclpy.spin(minimal_publisher)
|
||||
|
||||
minimal_publisher.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
@ -7,8 +7,8 @@ from sensor_msgs.msg import Joy
|
|||
|
||||
import numpy as np
|
||||
|
||||
SCALE_LINEAR_VELOCITY = 0.1
|
||||
SCALE_ANGULAR_VELOCITY = 0.1
|
||||
SCALE_LINEAR_VELOCITY = 0.2
|
||||
SCALE_ANGULAR_VELOCITY = 0.7
|
||||
PUBLISH_FREQUENCY_HZ = 30
|
||||
|
||||
SMOOTH_WEIGHTS = np.array([1, 2, 4, 10, 20])
|
||||
|
|
|
|||
|
|
@ -1,19 +1,14 @@
|
|||
digraph G {
|
||||
"odom" -> "base_footprint"[label=" Broadcaster: default_authority\nAverage rate: 29.645\nBuffer length: 4.284\nMost recent transform: 109.56\nOldest transform: 105.276\n"];
|
||||
"base_footprint" -> "base_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"camera_link" -> "camera_depth_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"base_link" -> "camera_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"camera_depth_frame" -> "camera_depth_optical_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"camera_link" -> "camera_rgb_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"camera_rgb_frame" -> "camera_rgb_optical_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"base_link" -> "caster_back_left_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"base_link" -> "caster_back_right_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"base_link" -> "imu_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"base_link" -> "base_scan"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"base_link" -> "wheel_left_link"[label=" Broadcaster: default_authority\nAverage rate: 17.882\nBuffer length: 4.25\nMost recent transform: 109.56\nOldest transform: 105.31\n"];
|
||||
"base_link" -> "wheel_right_link"[label=" Broadcaster: default_authority\nAverage rate: 17.882\nBuffer length: 4.25\nMost recent transform: 109.56\nOldest transform: 105.31\n"];
|
||||
"base_link" -> "velodyne"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"base_link" -> "base_footprint"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"chassis" -> "camera_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"base_link" -> "chassis"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"camera_link" -> "camera_link_optical"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"chassis" -> "caster_wheel_back"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"chassis" -> "caster_wheel_front"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"chassis" -> "laser_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
edge [style=invis];
|
||||
subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
|
||||
"Recorded at time: 1688900208.2025208"[ shape=plaintext ] ;
|
||||
}->"odom";
|
||||
"Recorded at time: 1688907915.1191485"[ shape=plaintext ] ;
|
||||
}->"base_link";
|
||||
}
|
||||
Binary file not shown.
|
|
@ -24,6 +24,27 @@ def generate_launch_description():
|
|||
'slam_params_file': slam_params}.items()
|
||||
)
|
||||
|
||||
node_fix_lidar = Node(
|
||||
package="dcaitirobot",
|
||||
executable="fixlidar",
|
||||
name="fixlidar",
|
||||
)
|
||||
|
||||
node_scale_odom = Node(
|
||||
package="dcaitirobot",
|
||||
executable="scaleodom",
|
||||
name="scaleodom",
|
||||
)
|
||||
|
||||
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, tfs1, tfs2, node_fix_lidar, node_scale_odom
|
||||
])
|
||||
|
|
@ -28,7 +28,8 @@ setup(
|
|||
'twistcalc = dcaitirobot.twist:main',
|
||||
'serial_comms = dcaitirobot.serial_comms:main',
|
||||
'joy = dcaitirobot.joy:main',
|
||||
"fixlidar = dcaitirobot.handle_nan_scan:main"
|
||||
"fixlidar = dcaitirobot.handle_nan_scan:main",
|
||||
"scaleodom = dcaitirobot.scale_odom:main"
|
||||
],
|
||||
},
|
||||
)
|
||||
|
|
|
|||
Loading…
Reference in New Issue