From 9d916a95d3d7ddcc867e2d8a5e3b5c8e7aa761e0 Mon Sep 17 00:00:00 2001 From: hojunlim Date: Fri, 15 Dec 2023 11:04:27 +0900 Subject: [PATCH] * ld08_driver delete * ekf_node topic remmaping * U2D2, Yahboom Expasion Board Port fix * verify cartographer node activation on rivz but odometry link point incorrect --- src/.vscode/colcon.env | 75 ------------------ .../eutobot_bringup/launch/robot.launch.py | 12 +-- src/ld08_driver | 1 - .../launch/yahboomcar_bringup_X3_launch.py | 32 ++++---- .../yahboomcar_bringup/Mcnamu_driver_X3.py | 8 +- .../Mcnamu_driver_X3.cpython-310.pyc | Bin 4655 -> 4653 bytes .../yahboomcar_bringup/calibrate_linear_X3.py | 45 +++++------ 7 files changed, 43 insertions(+), 130 deletions(-) delete mode 100644 src/.vscode/colcon.env delete mode 160000 src/ld08_driver diff --git a/src/.vscode/colcon.env b/src/.vscode/colcon.env deleted file mode 100644 index 68bbe89..0000000 --- a/src/.vscode/colcon.env +++ /dev/null @@ -1,75 +0,0 @@ -SHELL=/bin/bash -SESSION_MANAGER=local/hjmatt-linux:@/tmp/.ICE-unix/2257,unix/hjmatt-linux:/tmp/.ICE-unix/2257 -QT_ACCESSIBILITY=1 -COLORTERM=truecolor -XDG_CONFIG_DIRS=/etc/xdg/xdg-ubuntu:/etc/xdg -XDG_MENU_PREFIX=gnome- -GNOME_DESKTOP_SESSION_ID=this-is-deprecated -TERMINATOR_DBUS_PATH=/net/tenshu/Terminator2 -LC_ADDRESS=ko_KR.UTF-8 -GNOME_SHELL_SESSION_MODE=ubuntu -LC_NAME=ko_KR.UTF-8 -SSH_AUTH_SOCK=/run/user/1000/keyring/ssh -ELECTRON_RUN_AS_NODE=1 -TERMINATOR_UUID=urn:uuid:1b1c9186-aec3-4494-ab5a-f51a9555abf6 -XMODIFIERS=@im=ibus -DESKTOP_SESSION=ubuntu -LC_MONETARY=ko_KR.UTF-8 -SSH_AGENT_PID=2219 -VSCODE_AMD_ENTRYPOINT=vs/workbench/api/node/extensionHostProcess -GTK_MODULES=gail:atk-bridge -PWD=/home/hjmatt/task8/13.ros/SRDK/mnt_target/workspace/src -XDG_SESSION_DESKTOP=ubuntu -LOGNAME=hjmatt -XDG_SESSION_TYPE=x11 -GPG_AGENT_INFO=/run/user/1000/gnupg/S.gpg-agent:0:1 -VSCODE_CODE_CACHE_PATH=/home/hjmatt/.config/Code/CachedData/1a5daa3a0231a0fbba4f14db7ec463cf99d7768e -_=/usr/bin/env -XAUTHORITY=/run/user/1000/gdm/Xauthority -GJS_DEBUG_TOPICS=JS ERROR;JS LOG -WINDOWPATH=2 -HOME=/home/hjmatt -USERNAME=hjmatt -IM_CONFIG_PHASE=1 -LC_PAPER=ko_KR.UTF-8 -LANG=en_US.UTF-8 -LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36: -XDG_CURRENT_DESKTOP=Unity -VSCODE_IPC_HOOK=/run/user/1000/vscode-7e50df75-1.84-main.sock -VTE_VERSION=6003 -VSCODE_CLI=1 -INVOCATION_ID=67cab6c134a244dba56513793b4cab58 -TERMINATOR_DBUS_NAME=net.tenshu.Terminator21a9d5db22c73a993ff0b42f64b396873 -MANAGERPID=2041 -CHROME_DESKTOP=code-url-handler.desktop -GJS_DEBUG_OUTPUT=stderr -LESSCLOSE=/usr/bin/lesspipe %s %s -XDG_SESSION_CLASS=user -TERM=xterm-256color -LC_IDENTIFICATION=ko_KR.UTF-8 -LESSOPEN=| /usr/bin/lesspipe %s -USER=hjmatt -DISPLAY=:0 -VSCODE_PID=609928 -SHLVL=1 -LC_TELEPHONE=ko_KR.UTF-8 -QT_IM_MODULE=ibus -LC_MEASUREMENT=ko_KR.UTF-8 -VSCODE_CWD=/home/hjmatt/task8/13.ros/SRDK/mnt_target/workspace/src -VSCODE_CRASH_REPORTER_PROCESS_TYPE=extensionHost -XDG_RUNTIME_DIR=/run/user/1000 -LC_TIME=ko_KR.UTF-8 -ELECTRON_NO_ATTACH_CONSOLE=1 -JOURNAL_STREAM=8:55273 -XDG_DATA_DIRS=/usr/share/ubuntu:/usr/local/share/:/usr/share/ -GDK_BACKEND=x11 -PATH=/home/hjmatt/.local/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin -GDMSESSION=ubuntu -ORIGINAL_XDG_CURRENT_DESKTOP=ubuntu:GNOME -DBUS_SESSION_BUS_ADDRESS=unix:path=/run/user/1000/bus -VSCODE_NLS_CONFIG={"locale":"en-us","osLocale":"en-us","availableLanguages":{},"_languagePackSupport":true} -GIO_LAUNCHED_DESKTOP_FILE_PID=2710 -GIO_LAUNCHED_DESKTOP_FILE=/usr/share/applications/terminator.desktop -VSCODE_HANDLES_UNCAUGHT_ERRORS=true -LC_NUMERIC=ko_KR.UTF-8 -OLDPWD=/home/hjmatt/task8/13.ros/SRDK/mnt_target/workspace diff --git a/src/eutobot/eutobot_bringup/launch/robot.launch.py b/src/eutobot/eutobot_bringup/launch/robot.launch.py index 03cb4b4..cce498a 100644 --- a/src/eutobot/eutobot_bringup/launch/robot.launch.py +++ b/src/eutobot/eutobot_bringup/launch/robot.launch.py @@ -68,9 +68,9 @@ def generate_launch_description(): # YAHBOOM - # yahboom_pkg_dir = LaunchConfiguration( - # 'yahboom_pkg_dir', - # default=os.path.join(get_package_share_directory('yahboomcar_bringup'), 'launch')) + yahboom_pkg_dir = LaunchConfiguration( + 'yahboom_pkg_dir', + default=os.path.join(get_package_share_directory('yahboomcar_bringup'), 'launch')) return LaunchDescription([ @@ -95,9 +95,9 @@ def generate_launch_description(): launch_arguments={'port': '/dev/ttySAC6', 'frame_id': 'base_scan'}.items(), ), - # IncludeLaunchDescription( - # PythonLaunchDescriptionSource([yahboom_pkg_dir, "/yahboomcar_bringup_X3_launch.py"]), - # ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([yahboom_pkg_dir, "/yahboomcar_bringup_X3_launch.py"]), + ), Node( package='eutobot_node', diff --git a/src/ld08_driver b/src/ld08_driver deleted file mode 160000 index f63c92d..0000000 --- a/src/ld08_driver +++ /dev/null @@ -1 +0,0 @@ -Subproject commit f63c92d178dab1d54cee0f0ed143a2b706c17487 diff --git a/src/yahboomcar_bringup/launch/yahboomcar_bringup_X3_launch.py b/src/yahboomcar_bringup/launch/yahboomcar_bringup_X3_launch.py index 75695ce..c792dcd 100644 --- a/src/yahboomcar_bringup/launch/yahboomcar_bringup_X3_launch.py +++ b/src/yahboomcar_bringup/launch/yahboomcar_bringup_X3_launch.py @@ -83,20 +83,19 @@ def generate_launch_description(): parameters=[imu_filter_config] ) - ekf_node = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('robot_localization'), 'launch'), - '/ekf.launch.py']) - ) - - yahboom_joy_node = Node( - package='yahboomcar_ctrl', - executable='yahboom_joy_X3', - ) - - yahboom_keyboard_node = Node( - package='yahboomcar_ctrl', - executable='yahboom_keyboard', + ekf_filter_node = Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node', + output='screen', + parameters=[ + os.path.join(get_package_share_directory("robot_localization"), 'params', 'ekf.yaml') + ], + remappings=[ + ("/example/imu", "/imu/data"), + ("/example/odom", "/odom_raw"), + ("/odometry/filtered", "/odom"), + ] ) return LaunchDescription([ @@ -111,7 +110,6 @@ def generate_launch_description(): driver_node, base_node, imu_filter_node, - ekf_node, - # yahboom_keyboard_node - yahboom_joy_node + ekf_filter_node + # yahboom_joy_node ]) diff --git a/src/yahboomcar_bringup/yahboomcar_bringup/Mcnamu_driver_X3.py b/src/yahboomcar_bringup/yahboomcar_bringup/Mcnamu_driver_X3.py index 9c17d3a..b03e57e 100644 --- a/src/yahboomcar_bringup/yahboomcar_bringup/Mcnamu_driver_X3.py +++ b/src/yahboomcar_bringup/yahboomcar_bringup/Mcnamu_driver_X3.py @@ -74,10 +74,8 @@ class yahboomcar_driver(Node): self.car.create_receive_threading() #callback function def cmd_vel_callback(self,msg): - # 小车运动控制，订阅者回调函数 # Car motion control, subscriber callback function if not isinstance(msg, Twist): return - # 下发线速度和角速度 # Issue linear vel and angular vel vx = msg.linear.x*1.0 #vy = msg.linear.y/1000.0*180.0/3.1416 #Radian system @@ -90,7 +88,7 @@ class yahboomcar_driver(Node): #rospy.loginfo("nav_use_rot:{}".format(self.nav_use_rotvel)) #print(self.nav_use_rotvel) def RGBLightcallback(self,msg): - # 流水灯控制，服务端回调函数 RGBLight control + # RGBLight control if not isinstance(msg, Int32): return # print ("RGBLight: ", msg.data) for i in range(3): self.car.set_colorful_effect(msg.data, 6, parm=1) @@ -132,7 +130,6 @@ class yahboomcar_driver(Node): '''print("vx: ",vx) print("vy: ",vy) print("angular: ",angular)''' - # 发布陀螺仪的数据 # Publish gyroscope data imu.header.stamp = time_stamp.to_msg() imu.header.frame_id = self.imu_link @@ -149,7 +146,6 @@ class yahboomcar_driver(Node): mag.magnetic_field.y = my*1.0 mag.magnetic_field.z = mz*1.0 - # 将小车当前的线速度和角速度发布出去 # Publish the current linear vel and angular vel of the car twist.linear.x = vx *1.0 twist.linear.y = vy *1.0 @@ -170,5 +166,3 @@ def main(): driver = yahboomcar_driver('driver_node') rclpy.spin(driver) -'''if __name__ == '__main__': - main()''' diff --git a/src/yahboomcar_bringup/yahboomcar_bringup/__pycache__/Mcnamu_driver_X3.cpython-310.pyc b/src/yahboomcar_bringup/yahboomcar_bringup/__pycache__/Mcnamu_driver_X3.cpython-310.pyc index 775056d97d78ba0be44fe6709f7915a648f60044..f5e2afe48fe3ebfa838a2dcfd11e5e40fbf4756f 100644 GIT binary patch delta 111 zcmZ3lvQ~vJpO=@50SF2gRi`$JZ{!nWVKmsR%F@ol%*4Yqc|FHl#;D12Io~nHZ|>o` x&BB;IIfLI)nuUvz2N^QK`OK5|@GoTKn;a+5$GBwkTLBS9Mvci#LYhoGTmZF>7$5)u delta 113 zcmZ3hvR;KRpO=@50SIoJR;D&dY~&MTVKm&V%F@ol%*?|)c|FHl#+b=-Io~lRZtmf_ z&BB;DIfLI)nw5)@2N^QK`7D$7@GoQ(m>eh2$GBqiTLBS9My<(ALYgcbd@QU0=!6(M diff --git a/src/yahboomcar_bringup/yahboomcar_bringup/calibrate_linear_X3.py b/src/yahboomcar_bringup/yahboomcar_bringup/calibrate_linear_X3.py index d27276e..688f560 100644 --- a/src/yahboomcar_bringup/yahboomcar_bringup/calibrate_linear_X3.py +++ b/src/yahboomcar_bringup/yahboomcar_bringup/calibrate_linear_X3.py @@ -20,32 +20,32 @@ class CalibrateLinear(Node): #declare_parameter self.declare_parameter('rate',20.0) self.rate = self.get_parameter('rate').get_parameter_value().double_value - + self.declare_parameter('test_distance',1.0) self.test_distance = self.get_parameter('test_distance').get_parameter_value().double_value - + self.declare_parameter('speed',0.5) self.speed = self.get_parameter('speed').get_parameter_value().double_value - + self.declare_parameter('tolerance',0.03) self.tolerance = self.get_parameter('tolerance').get_parameter_value().double_value - + self.declare_parameter('odom_linear_scale_correction',1.0) self.odom_linear_scale_correction = self.get_parameter('odom_linear_scale_correction').get_parameter_value().double_value - + self.declare_parameter('start_test',False) - - + + self.declare_parameter('direction',True) self.direction = self.get_parameter('direction').get_parameter_value().bool_value - + self.declare_parameter('base_frame','base_footprint') self.base_frame = self.get_parameter('base_frame').get_parameter_value().string_value - + self.declare_parameter('odom_frame','odom') self.odom_frame = self.get_parameter('odom_frame').get_parameter_value().string_value - - + + #init the tf listener self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) @@ -54,15 +54,13 @@ class CalibrateLinear(Node): self.position = Point() self.x_start = self.position.x self.y_start = self.position.y - + print ("finish init work") now = rclpy.time.Time() - #trans = self.tf_buffer.lookup_transform(self.odom_frame,self.base_frame,now,timeout=Duration(seconds = 10.0)) - #create timer + #trans = self.tf_buffer.lookup_transform(self.odom_frame,self.base_frame,now,timeout=Duration(seconds = 10.0)) + #create timer self.timer = self.create_timer(0.05, self.on_timer) - - def on_timer(self): move_cmd = Twist() #self.get_param() @@ -95,7 +93,7 @@ class CalibrateLinear(Node): self.start_test = rclpy.parameter.Parameter('start_test',rclpy.Parameter.Type.BOOL,False) all_new_parameters = [self.start_test] self.set_parameters(all_new_parameters) - + print("done") else: if self.direction: @@ -114,8 +112,8 @@ class CalibrateLinear(Node): print("self.x_start: ",self.x_start) print("self.y_start:",self.y_start) self.cmd_vel.publish(Twist()) - #self.cmd_vel.publish(Twist() ) - + #self.cmd_vel.publish(Twist() ) + def get_param(self): #self.start_test = self.get_parameter('start_test').get_parameter_value().bool_value self.rate = self.get_parameter('rate').get_parameter_value().double_value @@ -123,18 +121,17 @@ class CalibrateLinear(Node): self.direction = self.get_parameter('direction').get_parameter_value().bool_value self.tolerance = self.get_parameter('tolerance').get_parameter_value().double_value self.speed = self.get_parameter('speed').get_parameter_value().double_value - - + def get_position(self): try: now = rclpy.time.Time() - trans = self.tf_buffer.lookup_transform(self.odom_frame,self.base_frame,now) - return trans + trans = self.tf_buffer.lookup_transform(self.odom_frame,self.base_frame,now) + return trans except (LookupException, ConnectivityException, ExtrapolationException): self.get_logger().info('transform not ready') raise return - + def main(): rclpy.init() class_calibratelinear = CalibrateLinear("calibrate_linear")