#### 実行
```
roscore
roslaunch rplidar_ros rplidar.launch
roslaunch hector_slam_launch tutorial.launch
roslaunch navigation2d_example move_base.launch
rosrun map_server map_saver -f mymap
```
#### 結果
```
WARNING: ignoring defunct <master /> tag
RLException: while processing /home/hattori/catkin_ws/src/navigation2d_example/launch/hector_hokuyo.launch:
while processing hector_geotiff_launch/launch/geotiff_mapper.launch:
Invalid roslaunch XML syntax: [Errno 2] No such file or directory: 'hector_geotiff_launch/launch/geotiff_mapper.launch'
The traceback for the exception was written to the log file
```
`hector_geotiff_launch/launch/geotiff_mapper.launch`
`/home/hattori/catkin_ws/src/hector_slam/hector_geotiff_launch/launch/geotiff_mapper.launch`
```
hattori@hattori-raspi:~/catkin_ws$ rospack find hector_geo
tiff_launch
/home/hattori/catkin_ws/src/hector_slam/hector_geotiff_launch
hattori@hattori-raspi:~/catkin_ws$ rospack find hector_geotiff
/home/hattori/catkin_ws/src/hector_slam/hector_geotiff
```
https://github.com/tu-darmstadt-ros-pkg/hector_slam
```
$ rostopic list
/clicked_point
/initialpose
/map
/map_metadata
/map_updates
/move_base_simple/goal
/poseupdate
/rosout
/rosout_agg
/scan
/slam_cloud
/slam_out_pose
/syscommand
/tf
/tf_static
/trajectory
```
---
# [Cartographerでnavigationを行おう！](https://qiita.com/MMM-lab/items/14e94f9d41ccca4e1c61)
## Install
https://qiita.com/Decwest/items/ac1a701a2217dd05e1fb
より
```
sudo apt update
sudo apt install -y python3-wstool python3-rosdep ninja-build stow
```
```
# sudo apt install ros-noetic-cartographer-ros
sudo apt install ros-noetic-ddynamic-reconfigure
sudo apt install ros-noetic-tf2-sensor-msgs
sudo apt install ros-noetic-move-base
sudo apt install -y ros-noetic-gmapping ros-noetic-amcl ros-noetic-map-server
sudo apt install ros-noetic-dwa-local-planner
sudo apt install -y ros-noetic-robot-localization
```
E: Unable to locate package ros-noetic-cartographer-ros
```
# https://qiita.com/Decwest/items/ac1a701a2217dd05e1fb
source /opt/ros/noetic/setup.bash
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin build
source devel/setup.bash
wstool init src
wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall
wstool update -t src
```
https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html
```
sudo apt-get update
sudo apt-get install -y python3-wstool python3-rosdep ninja-build stow
mkdir catkin_ws
cd catkin_ws
$ pwd
/home/hattori/projects/catkin_ws
wstool init src
wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall
wstool update -t src
sudo rosdep init
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y
src/cartographer/scripts/install_abseil.sh
# sudo apt-get remove ros-${ROS_DISTRO}-abseil-cpp
catkin_make_isolated --install --use-ninja
```
## cartographerの動作確認(地図生成)
```
cd ~/projects/catkin_ws/src
git clone https://github.com/Andrew-rw/gbot_core.git
cd ..
catkin_make
# @catkin_ws
source ~/catkin_ws/devel/setup.bash
roslaunch gbot_core gbot.launch
source devel/setup.bash
roslaunch gbot_core visualization.launch
```
###
$ ll ~/projects/
total 16
drwxrwxr-x 4 hattori hattori 4096 Mar 20 20:56 ./
drwxr-xr-x 23 hattori hattori 4096 Mar 20 20:53 ../
drwxrwxr-x 6 hattori hattori 4096 Mar 22 23:19 catkin_ws/
drwxrwxr-x 3 hattori hattori 4096 Mar 20 20:50 mobile-trash-can/
```
$ catkin_make
Base path: /home/hattori/projects/catkin_ws
Source space: /home/hattori/projects/catkin_ws/src
Build space: /home/hattori/projects/catkin_ws/build
Devel space: /home/hattori/projects/catkin_ws/devel
Install space: /home/hattori/projects/catkin_ws/install
####
#### Running command: "cmake /home/hattori/projects/catkin_ws/src -DCATKIN_DEVEL_PREFIX=/home/hattori/projects/catkin_ws/devel -DCMAKE_INSTALL_PREFIX=/home/hattori/projects/catkin_ws/install -G Unix Makefiles" in "/home/hattori/projects/catkin_ws/build"
####
-- Using CATKIN_DEVEL_PREFIX: /home/hattori/projects/catkin_ws/devel
-- Using CMAKE_PREFIX_PATH: /home/hattori/catkin_ws/devel;/opt/ros/noetic
-- This workspace overlays: /home/hattori/catkin_ws/devel;/opt/ros/noetic
-- Found PythonInterp: /usr/bin/python3 (found suitable version "3.8.10", minimum required is "3")
-- Using PYTHON_EXECUTABLE: /usr/bin/python3
-- Using Debian Python package layout
-- Using empy: /usr/lib/python3/dist-packages/em.py
-- Using CATKIN_ENABLE_TESTING: ON
-- Call enable_testing()
-- Using CATKIN_TEST_RESULTS_DIR: /home/hattori/projects/catkin_ws/build/test_results
-- Forcing gtest/gmock from source, though one was otherwise available.
-- Found gtest sources under '/usr/src/googletest': gtests will be built
-- Found gmock sources under '/usr/src/googletest': gmock will be built
-- Found PythonInterp: /usr/bin/python3 (found version "3.8.10")
-- Using Python nosetests: /usr/bin/nosetests3
-- catkin 0.8.10
-- BUILD_SHARED_LIBS is on
-- BUILD_SHARED_LIBS is on
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- ~~ traversing 5 packages in topological order:
-- ~~ - cartographer (plain cmake)
-- ~~ - cartographer_ros_msgs
-- ~~ - gbot_core
-- ~~ - cartographer_ros
-- ~~ - cartographer_rviz
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
CMake Error at /opt/ros/noetic/share/catkin/cmake/catkin_workspace.cmake:100 (message):
This workspace contains non-catkin packages in it, and catkin cannot build
a non-homogeneous workspace without isolation. Try the
'catkin_make_isolated' command instead.
Call Stack (most recent call first):
CMakeLists.txt:69 (catkin_workspace)
-- Configuring incomplete, errors occurred!
See also "/home/hattori/projects/catkin_ws/build/CMakeFiles/CMakeOutput.log".
See also "/home/hattori/projects/catkin_ws/build/CMakeFiles/CMakeError.log".
Invoking "cmake" failed
```
```
catkin_make_isolated
git clone https://github.com/TUKUBA-CHALLENGE/cartgrapher_navigation.git
```
---
```
$ catkin_make_isolated
Base path: /home/hattori/projects/catkin_ws
Source space: /home/hattori/projects/catkin_ws/src
Build space: /home/hattori/projects/catkin_ws/build_isolated
Devel space: /home/hattori/projects/catkin_ws/devel_isolated
Install space: /home/hattori/projects/catkin_ws/install_isolated
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
~~ traversing 6 packages in topological order:
~~ - cartgrapher_navigation
~~ - cartographer (plain cmake)
~~ - cartographer_ros_msgs
~~ - cartographer_ros
~~ - cartographer_rviz
~~ - gbot_core
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
The packages or cmake arguments have changed, forcing cmake invocation
==> Processing catkin package: 'cartgrapher_navigation'
==> Creating build directory: 'build_isolated/cartgrapher_navigation'
==> cmake /home/hattori/projects/catkin_ws/src/cartgrapher_navigation -DCATKIN_DEVEL_PREFIX=/home/hattori/projects/catkin_ws/devel_isolated/cartgrapher_navigation -DCMAKE_INSTALL_PREFIX=/home/hattori/projects/catkin_ws/install_isolated -G Unix Makefiles in '/home/hattori/projects/catkin_ws/build_isolated/cartgrapher_navigation'
-- The C compiler identification is GNU 9.4.0
-- The CXX compiler identification is GNU 9.4.0
-- Check for working C compiler: /usr/bin/cc
-- Check for working C compiler: /usr/bin/cc -- works
-- Detecting C compiler ABI info
-- Detecting C compiler ABI info - done
-- Detecting C compile features
-- Detecting C compile features - done
-- Check for working CXX compiler: /usr/bin/c++
-- Check for working CXX compiler: /usr/bin/c++ -- works
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Detecting CXX compile features
-- Detecting CXX compile features - done
-- Using CATKIN_DEVEL_PREFIX: /home/hattori/projects/catkin_ws/devel_isolated/cartgrapher_navigation
-- Using CMAKE_PREFIX_PATH: /home/hattori/catkin_ws/devel;/opt/ros/noetic
-- This workspace overlays: /home/hattori/catkin_ws/devel;/opt/ros/noetic
-- Found PythonInterp: /usr/bin/python3 (found suitable version "3.8.10", minimum required is "3")
-- Using PYTHON_EXECUTABLE: /usr/bin/python3
-- Using Debian Python package layout
-- Found PY_em: /usr/lib/python3/dist-packages/em.py
-- Using empy: /usr/lib/python3/dist-packages/em.py
-- Using CATKIN_ENABLE_TESTING: ON
-- Call enable_testing()
-- Using CATKIN_TEST_RESULTS_DIR: /home/hattori/projects/catkin_ws/build_isolated/cartgrapher_navigation/test_results
-- Forcing gtest/gmock from source, though one was otherwise available.
-- Found gtest sources under '/usr/src/googletest': gtests will be built
-- Found gmock sources under '/usr/src/googletest': gmock will be built
-- Found PythonInterp: /usr/bin/python3 (found version "3.8.10")
-- Found Threads: TRUE
-- Using Python nosetests: /usr/bin/nosetests3
-- catkin 0.8.10
-- BUILD_SHARED_LIBS is on
-- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
-- Could NOT find cartographer_ros (missing: cartographer_ros_DIR)
-- Could not find the required component 'cartographer_ros'. The following CMake error indicates that you either need to install the package with the same name or change your environment so that it can be found.
CMake Error at /opt/ros/noetic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
Could not find a package configuration file provided by "cartographer_ros"
with any of the following names:
cartographer_rosConfig.cmake
cartographer_ros-config.cmake
Add the installation prefix of "cartographer_ros" to CMAKE_PREFIX_PATH or
set "cartographer_ros_DIR" to a directory containing one of the above
files. If "cartographer_ros" provides a separate development package or
SDK, be sure it has been installed.
Call Stack (most recent call first):
CMakeLists.txt:4 (find_package)
-- Configuring incomplete, errors occurred!
See also "/home/hattori/projects/catkin_ws/build_isolated/cartgrapher_navigation/CMakeFiles/CMakeOutput.log".
See also "/home/hattori/projects/catkin_ws/build_isolated/cartgrapher_navigation/CMakeFiles/CMakeError.log".
<== Failed to process package 'cartgrapher_navigation':
Command '['cmake', '/home/hattori/projects/catkin_ws/src/cartgrapher_navigation', '-DCATKIN_DEVEL_PREFIX=/home/hattori/projects/catkin_ws/devel_isolated/cartgrapher_navigation', '-DCMAKE_INSTALL_PREFIX=/home/hattori/projects/catkin_ws/install_isolated', '-G', 'Unix Makefiles']' returned non-zero exit status 1.
Reproduce this error by running:
==> cd /home/hattori/projects/catkin_ws/build_isolated/cartgrapher_navigation && cmake /home/hattori/projects/catkin_ws/src/cartgrapher_navigation -DCATKIN_DEVEL_PREFIX=/home/hattori/projects/catkin_ws/devel_isolated/cartgrapher_navigation -DCMAKE_INSTALL_PREFIX=/home/hattori/projects/catkin_ws/install_isolated -G 'Unix Makefiles'
Command failed, exiting.
```
catkin_make_isolated --install --use-ninja
```
$ echo $ROS_PACKAGE_PATH
/home/hattori/projects/catkin_ws/src/cartographer_ros/cartographer_rviz:/home/hattori/projects/catkin_ws/install_isolated/share:/home/hattori/catkin_ws/src:/opt/ros/noetic/share
```
export ROS_PACKAGE_PATH="/home/hattori/projects/catkin_ws/src/cartographer_ros/cartographer_ros:$ROS_PACKAGE_PATH"
docker build -f Dockerfile.noetic -t myros .
# catkin build
https://catkin-tools.readthedocs.io/en/latest/installing.html
## Install
https://github.com/TUKUBA-CHALLENGE/cartgrapher_navigation.git
```
mkdir ~/catkin_build_ws
# ワークスペース初期化
source /opt/ros/noetic/setup.bash
mkdir -p ~/catkin_build_ws/src
cd ~/catkin_build_ws
catkin build
source devel/setup.bash
# cartographerのclone
wstool init src
wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall
wstool update -t src
# 依存関係の解決
rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y
# abceil-cpp libraryのclone, build
src/cartographer/scripts/install_abseil.sh
# catkin build
catkin build
source devel/setup.bash
# 動作確認: 2D backpack demoをダウンロードしてlaunch
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag
roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag
```
### gbot
```
cd ~/catkin_build_ws/src
git clone https://github.com/Andrew-rw/gbot_core.git
cd ..
catkin build
# launchファイルの実行
source ~/catkin_build_ws/devel/setup.bash
roslaunch gbot_core gbot.launch
# rvizでの可視化
source ~/catkin_build_ws/devel/setup.bash
roslaunch gbot_core visualization.launch
```
```
git clone https://github.com/TUKUBA-CHALLENGE/cartgrapher_navigation.git
cd ~/catkin_build_ws
catkin build
```
# arduino
https://qiita.com/MMM-lab/items/80d69403678a71a30fdf
gbot.launch
```
[ WARN] [1647974940.834105416]: W0323 03:49:00.000000 21987 node.cc:881] Expected topic "scan" (trajectory 0) (resolved topic "/scan") but no publisher is currently active.
[ WARN] [1647974940.834652411]: W0323 03:49:00.000000 21987 node.cc:890] Currently available topics are: /rosout_agg,/rosout,/tf,/tf_static,/submap_list,/map,/trajectory_node_list,/landmark_poses_list,/constraint_list,/scan_matched_points2,
```
visualization.launch
```
WARN] [1647975108.819563556]: Could not compute submap fading: "map" passed to lookupTransform argument target_frame does not exist.
[ WARN] [1647975109.897174746]: Could not compute submap fading: "map" passed to lookupTransform argument target_frame does not exist.
[ WARN] [1647975110.979456327]: Could not compute submap fading: "map" passed to lookupTransform argument target_frame does not exist.
[ WARN] [1647975112.067587789]: Could not compute submap fading: "map" passed to lookupTransform argument target_frame does not exist.
[ WARN] [1647975113.105551350]: Could not compute submap fading: "map" passed to lookupTransform argument target_frame does not exist.
[ WARN] [1647975114.156619664]: Could not compute submap fading: "map" passed to lookupTransform argument target_frame does not exist.
[ WARN] [1647975115.214141559]: Could not compute submap fading: "map" passed to lookupTransform argument target_frame does not exist.
```
https://github.com/Andrew-rw/gbot_core/blob/main/launch/gbot.launch
```
$ rosnode list
/cartographer_node
/cartographer_occupancy_grid_node
/robot_state_publisher
/rosout
/show_rviz
```
```
$ roslaunch gbot_core gbot.launch
... logging to /home/hattori/.ros/log/b9662d9e-aa12-11ec-83e1-85b322337fe6/roslaunch-hattori-raspi-25247.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
started roslaunch server http://hattori-raspi:35105/
SUMMARY
========
PARAMETERS
* /robot_description: <robot name="head...
* /rosdistro: noetic
* /rosversion: 1.15.14
* /rplidarNode/angle_compensate: True
* /rplidarNode/frame_id: laser
* /rplidarNode/inverted: False
* /rplidarNode/serial_baudrate: 115200
* /rplidarNode/serial_port: /dev/ttyUSB0
NODES
/
cartographer_node (cartographer_ros/cartographer_node)
cartographer_occupancy_grid_node (cartographer_ros/cartographer_occupancy_grid_node)
robot_state_publisher (robot_state_publisher/robot_state_publisher)
rplidarNode (rplidar_ros/rplidarNode)
ROS_MASTER_URI=http://localhost:11311
process[robot_state_publisher-1]: started with pid [25306]
ERROR: cannot launch node of type [rplidar_ros/rplidarNode]: rplidar_ros
ROS path [0]=/opt/ros/noetic/share/ros
ROS path [1]=/home/hattori/catkin_build_ws/src/cartgrapher_navigation
ROS path [2]=/home/hattori/catkin_build_ws/src/cartographer
ROS path [3]=/home/hattori/catkin_build_ws/src/cartographer_ros/cartographer_ros_msgs
ROS path [4]=/home/hattori/catkin_build_ws/src/cartographer_ros/cartographer_ros
ROS path [5]=/home/hattori/catkin_build_ws/src/cartographer_ros/cartographer_rviz
ROS path [6]=/home/hattori/catkin_build_ws/src/gbot_core
ROS path [7]=/opt/ros/noetic/share
process[cartographer_node-3]: started with pid [25307]
process[cartographer_occupancy_grid_node-4]: started with pid [25309]
[ INFO] [1647975799.018256558]: I0323 04:03:19.000000 25307 configuration_file_resolver.cc:41] Found '/home/hattori/catkin_build_ws/src/gbot_core/configuration_files/gbot_lidar_2d.lua' for 'gbot_lidar_2d.lua'.
[ INFO] [1647975799.033663217]: I0323 04:03:19.000000 25307 configuration_file_resolver.cc:41] Found '/home/hattori/catkin_build_ws/devel/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'.
[ INFO] [1647975799.035012484]: I0323 04:03:19.000000 25307 configuration_file_resolver.cc:41] Found '/home/hattori/catkin_build_ws/devel/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'.
[ INFO] [1647975799.036192549]: I0323 04:03:19.000000 25307 configuration_file_resolver.cc:41] Found '/home/hattori/catkin_build_ws/devel/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'.
[ INFO] [1647975799.037061153]: I0323 04:03:19.000000 25307 configuration_file_resolver.cc:41] Found '/home/hattori/catkin_build_ws/devel/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'.
[ INFO] [1647975799.038053183]: I0323 04:03:19.000000 25307 configuration_file_resolver.cc:41] Found '/home/hattori/catkin_build_ws/devel/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'.
[ INFO] [1647975799.038951750]: I0323 04:03:19.000000 25307 configuration_file_resolver.cc:41] Found '/home/hattori/catkin_build_ws/devel/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'.
[ INFO] [1647975799.039824151]: I0323 04:03:19.000000 25307 configuration_file_resolver.cc:41] Found '/home/hattori/catkin_build_ws/devel/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
[ INFO] [1647975799.042650925]: I0323 04:03:19.000000 25307 configuration_file_resolver.cc:41] Found '/home/hattori/catkin_build_ws/devel/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
[ INFO] [1647975799.043907897]: I0323 04:03:19.000000 25307 configuration_file_resolver.cc:41] Found '/home/hattori/catkin_build_ws/devel/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
[ INFO] [1647975799.044300450]: I0323 04:03:19.000000 25307 configuration_file_resolver.cc:41] Found '/home/hattori/catkin_build_ws/devel/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
[ INFO] [1647975799.113749470]: I0323 04:03:19.000000 25307 map_builder_bridge.cc:135] Added trajectory with ID '0'.
[ WARN] [1647975802.138880367]: W0323 04:03:22.000000 25307 node.cc:881] Expected topic "scan" (trajectory 0) (resolved topic "/scan") but no publisher is currently active.
[ WARN] [1647975802.139081069]: W0323 04:03:22.000000 25307 node.cc:890] Currently available topics are: /rosout_agg,/rosout,/tf,/tf_static,/map,/submap_list,/trajectory_node_list,/landmark_poses_list,/constraint_list,/scan_matched_points2,
^C[cartographer_occupancy_grid_node-4] killing on exit
[robot_state_publisher-1] killing on exit
[cartographer_node-3] killing on exit
Optimizing: Done.
Optimizing: Done.
shutting down processing monitor...
... shutting down processing monitor complete
done
```
https://qiita.com/MENDY/items/0089b0f52acf23b7d3f1
```
catkin_create_pkg motor rospy std_msgs
catkin build
# 1
roscore
# 2
rosrun motor right_motor.py
# 3
rosrun motor left_motor.py
rostopic pub motor/twist/cmd_vel geometry_msgs/Twist '[1.0, 0.0, 0.0]' '[0.0, 0.0, 0.0]'
```
```python=
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Int64
class Twist2int64():
def __init__(self):
self.command_left = Int64()
self.command_right = Int64()
self.received_twist = None
rospy.init_node('Twist2int64')
rospy.Subscriber('motor/twist/cmd_vel', Twist, self.callback)
self.pub_right = rospy.Publisher('right_motor/cmd_vel', Int64, queue_size=10)#name, topic_type, size
self.pub_left = rospy.Publisher('left_motor/cmd_vel', Int64, queue_size=10)#name, topic_type, size
def main_twist2int64(self):
rospy.spin()
def callback(self, message):
self.received_twist = message #input data
self.command_right, self.command_left = self.twist2rpm(self.received_twist)
self.pub_right.publish(self.command_right)
self.pub_left.publish(self.command_left)
def twist2rpm(self, received_data):#convert to speed
#(m/s, rad/s)
wheeles_size = 0.075#wheel size
axle_length = 0.35#axle_size(2d)
v = received_data.linear.x#(m/s)
omega = received_data.angular.z#(rad/s)
v_r = (omega*axle_length + 2*v)/2
v_l = (omega*axle_length - 2*v)/(-2)
v_r = v_r/(wheeles_size * 2 * 3.14) #wheel_speed(1/s)
v_l = v_l/(wheeles_size * 2 * 3.14) #wheel_speed(1/s)
r_rpm = 60 * v_r * 19 #gear rate
l_rpm = 60 * v_l * 19 #gear rate
return r_rpm, l_rpm
#Main Program
Convert = Twist2int64()
Convert.main_twist2int64()
#pub memo
#rostopic pub motor/twist/cmd_vel geometry_msgs/Twist '[1.0, 0.0, 0.0]' '[0.0, 0.0, 0.0]'
```
```python=
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import RPi.GPIO as GPIO#GPIOの設定
import time
import rospy
from std_msgs.msg import Int64
class motor_command():
def __init__(self):#初期化
#初期変数
self.RotationDirect = True #Forward
self.Stop = False #Forward
self.Enable = True
self.pin_num_PWM = 18#pinの名前
self.pin_num_RotationDirect = 15
self.pin_num_Enable = 14
self.pin_num_Stop = 23
self.freq = 50 # Hz (PWM のパルスを一秒間に 50 個生成)
#ノード立ち上げ（ROS）
rospy.init_node('maxon_test')
rospy.Subscriber('right_motor/cmd_vel', Int64, self.callback)
rospy.loginfo('Ready to receive motor command')
#初期設定（GPIO）
GPIO.setmode(GPIO.BCM)
# GPIOをデジタル出力に設定 GPIOのセットアップ
GPIO.setup(self.pin_num_PWM, GPIO.OUT) #PWM
GPIO.setup(self.pin_num_RotationDirect, GPIO.OUT) #rotation direct
GPIO.setup(self.pin_num_Enable, GPIO.OUT) #enable
GPIO.setup(self.pin_num_Stop, GPIO.OUT) #stop`
# PWM 設定
GPIO.output(self.pin_num_RotationDirect, GPIO.LOW)
GPIO.output(self.pin_num_Enable, GPIO.LOW)
GPIO.output(self.pin_num_Stop, GPIO.HIGH)
self.pwm = GPIO.PWM(self.pin_num_PWM, self.freq)
duty = 0.0 # デューティー比 0.0 で出力開始 (パルス内に占める HIGH 状態の時間が 0.0 %)
self.pwm.start(duty)
def motor_main(self):
rospy.spin()
#お片付け
GPIO.cleanup()
rospy.loginfo('回転終了')
self.pwm.stop()#終了
def callback(self, data):
self.rpm_command = data.data#rpmで取得
rospy.loginfo(self.rpm_command)
duty_command = self.rpm2duty(self.rpm_command)#rpm→duty比へ（制約考慮）
rospy.loginfo('motor command is '+ str(duty_command))
#PINいじり
if duty_command >= 0:#正逆回転
self.RotationDirect = True
else:
self.RotationDirect = False#逆回転
rospy.loginfo('Reverse')
duty_command = -duty_command
if self.RotationDirect:
GPIO.output(self.pin_num_RotationDirect,GPIO.HIGH)
else:
GPIO.output(self.pin_num_RotationDirect,GPIO.LOW)
if self.Stop:
GPIO.output(self.pin_num_Stop, GPIO.HIGH)
else:
GPIO.output(self.pin_num_Stop, GPIO.LOW)
if self.Enable:
GPIO.output(self.pin_num_Enable, GPIO.HIGH)
else:
GPIO.output(self.pin_num_Enable, GPIO.LOW)
rospy.loginfo('Change Command')
self.pwm.ChangeDutyCycle(duty_command)#回転量を変える
def rpm2duty(self, rpm):
max_rpm = 6000 #規定値90%
max_duty = 90
min_rpm = 0 #規定値10%
min_duty = 10
rpm_rate = (max_rpm-min_rpm)/(max_duty-min_duty)#1%に対するrpm
if rpm >= 0:
duty = rpm / rpm_rate + min_duty
else:
duty = rpm / rpm_rate - min_duty
#速度制約
if duty > 90:
rospy.loginfo('Speed limit!!')
duty = 90
elif duty < -90:
rospy.loginfo('Speed limit!!')
duty = -90
return duty
if __name__ == '__main__':#これがメイン実行file
Motor_Command = motor_command()
Motor_Command.motor_main()
#pubする場合の参考
#rostopic pub right_motor/cmd_vel std_msgs/Int64 "data: 10
```
https://github.com/Shunichi09/Qiita/tree/master/motor
Google Drive
Gist
Clipboard
Sign in with Wallet
