├── README.md
├── Tf_Move.cs
├── joy_sub_raspi_move.py
├── launch
    ├── .gitkeep
    ├── mapper_raspi.launch
    └── mapper_raspi_navi.launch
├── navi_raspi_move.py
└── param
    ├── .gitkeep
    ├── costmap_common_params.yaml
    ├── dwa_local_planner_params.yaml
    ├── global_costmap_params.yaml
    └── local_costmap_params.yaml


/README.md:
--------------------------------------------------------------------------------
 1 | # RaspberryPi-RobotCar
 2 |   
 3 | Raspberry Piにて作成したロボットカーにおいて  
 4 | 作成した部分のソースコードです。  
 5 | 
 6 | ## 概要（ProtoPediaに記載）
 7 | [Raspberry Piロボットカー作成](https://protopedia.net/prototype/3058 "Raspberry Piロボットカー作成")  
 8 |   
 9 | 
10 | ### ■コントローラで遠隔操作用
11 | **joy_sub_raspi_move.py**  
12 | Raspberry Piのモータをコントローラ（ジョイスティック）から  
13 | 遠隔操作するPythonプログラム  
14 | （ROSでは、sudoでPythonを実行できないためRPi.GPIOは使えない。  
15 | このため「pigpio」を使用しGPIOからモーターを動かす）  
16 | 「sudo pigpiod」してROSで動かす  
17 | 
18 | [![](https://img.youtube.com/vi/KuJdXyhc9Lg/0.jpg)](https://www.youtube.com/watch?v=KuJdXyhc9Lg)
19 | <p>
20 |   
21 | ### ■Uniry用
22 | **Tf_Move.cs**  
23 | Unityで位置と角度のデータからオブジェクトを動かすスクリプト  
24 | 
25 | [![](https://img.youtube.com/vi/D4HmZw1sfcI/0.jpg)](https://www.youtube.com/watch?v=D4HmZw1sfcI)
26 | <p>
27 | 
28 | ### ■Navigationで自動走行用
29 | **/launch**  
30 | move_baseを起動するlanchファイル  
31 | **/param**  
32 | パラメータ設定のyamlファイル   
33 |  **navi_raspi_move.py**  
34 | Raspberry Piのモータをトピックcmd_velから  
35 | 自動走行するPythonプログラム  
36 | （ROSでは、sudoでPythonを実行できないためRPi.GPIOは使えない。  
37 | このため「pigpio」を使用しGPIOからモーターを動かす）  
38 | 「sudo pigpiod」してROSで動かす  
39 | ＊後退できないので壁にぶつかると動かなくなります。。  
40 | 
41 | [![](https://img.youtube.com/vi/EbqeJYNXlME/0.jpg)](https://www.youtube.com/watch?v=EbqeJYNXlME)
42 | <p>
43 | 
44 | 


--------------------------------------------------------------------------------
/Tf_Move.cs:
--------------------------------------------------------------------------------
 1 | using System.Collections;
 2 | using System.Collections.Generic;
 3 | using UnityEngine;
 4 | using Unity.Robotics.ROSTCPConnector;
 5 | using TFMessageMsg = RosMessageTypes.Tf2.TFMessageMsg;
 6 | 
 7 | public class Tf_Move : MonoBehaviour
 8 | {
 9 |     ROSConnection ros;
10 |     public string frame_id;
11 |     public float translation_x;
12 |     public float translation_y;
13 |     public float rotation_z;
14 |     public float rotation_w;
15 | 
16 |     // Start is called before the first frame update
17 |     void Start()
18 |     {
19 |         ros = ROSConnection.GetOrCreateInstance();
20 |         ros.Subscribe<TFMessageMsg>("tf", OnSubscribe); 
21 |     }
22 | 
23 |     // Update is called once per frame
24 |     void Update()
25 |     {
26 |         gameObject.transform.position = new Vector3(translation_y, 0.1f, translation_x);
27 |         gameObject.transform.rotation = new Quaternion(0, rotation_z, 0, rotation_w);
28 |     }
29 | 
30 |     void OnSubscribe(TFMessageMsg msg)
31 |     {
32 |         frame_id = msg.transforms[0].header.frame_id;
33 |         if (frame_id=="map"){
34 |             translation_x = (float)msg.transforms[1].transform.translation.x;
35 |             translation_y = (float)msg.transforms[1].transform.translation.y * -1;
36 |             rotation_z = (float)msg.transforms[1].transform.rotation.z * -1;
37 |             rotation_w = (float)msg.transforms[1].transform.rotation.w;
38 |         }
39 |         
40 |     }
41 | }
42 | 


--------------------------------------------------------------------------------
/joy_sub_raspi_move.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python3
 2 | import rospy
 3 | from sensor_msgs.msg import Joy
 4 | import pigpio
 5 | 
 6 | pi = pigpio.pi()
 7 | pi.set_mode(22, pigpio.OUTPUT) 
 8 | pi.set_mode(23, pigpio.OUTPUT) 
 9 | pi.set_mode(24, pigpio.OUTPUT) 
10 | pi.set_mode(25, pigpio.OUTPUT) 
11 | pi.set_PWM_frequency(22, 100)
12 | pi.set_PWM_frequency(23, 100)
13 | pi.set_PWM_frequency(24, 100)
14 | pi.set_PWM_frequency(25, 100)
15 | 
16 | duty_forward = 150
17 | duty_direction = duty_forward + 5
18 | duty_max =255
19 | 
20 | def callback(joy_msg):
21 |     global duty_forward, duty_direction, duty_max
22 |     axes =[int(joy_msg.axes[0]),  int(joy_msg.axes[1])]
23 |     buttonsA = int(joy_msg.buttons[0])
24 |     buttonsB = int(joy_msg.buttons[1])
25 |     print(axes, buttonsA, buttonsB, duty_forward, duty_direction)
26 |     if axes == [0, 1]:
27 |         print('forward')
28 |         duty = duty_forward
29 |         pi.set_PWM_dutycycle(22, duty)
30 |         pi.set_PWM_dutycycle(23, 0)
31 |         pi.set_PWM_dutycycle(24, duty)
32 |         pi.set_PWM_dutycycle(25, 0)
33 | 
34 |     elif axes == [0, -1]:
35 |         print('back')
36 |         duty = duty_forward
37 |         pi.set_PWM_dutycycle(22, 0)
38 |         pi.set_PWM_dutycycle(23, duty)
39 |         pi.set_PWM_dutycycle(24, 0)
40 |         pi.set_PWM_dutycycle(25, duty)
41 | 
42 |     elif axes == [-1, 0]:
43 |         print('right')
44 |         duty = duty_direction
45 |         pi.set_PWM_dutycycle(22, 0)
46 |         pi.set_PWM_dutycycle(23, 0)
47 |         pi.set_PWM_dutycycle(24, duty)
48 |         pi.set_PWM_dutycycle(25, 0)
49 | 
50 |     elif axes == [1, 0]:
51 |         print('left')
52 |         duty = duty_direction
53 |         pi.set_PWM_dutycycle(22, duty)
54 |         pi.set_PWM_dutycycle(23, 0)
55 |         pi.set_PWM_dutycycle(24, 0)
56 |         pi.set_PWM_dutycycle(25, 0)
57 | 
58 |     elif buttonsA == 1:
59 |         if duty_forward <= duty_max - 10:
60 |             duty_forward = duty_forward + 10
61 |             duty_direction = duty_forward + 5
62 |         print('duty : ', duty_forward)
63 |     elif buttonsB == 1:
64 |         if duty_forward >= 5:
65 |             duty_forward = duty_forward - 10
66 |             duty_direction = duty_forward + 5
67 |         print('duty : ', duty_forward)
68 | 
69 |     else:
70 |         print('stop')
71 |         pi.set_PWM_dutycycle(22, 0)
72 |         pi.set_PWM_dutycycle(23, 0)
73 |         pi.set_PWM_dutycycle(24, 0)
74 |         pi.set_PWM_dutycycle(25, 0)
75 | 
76 | if __name__ == '__main__':
77 |     rospy.init_node('joy_sub_raspi_move', anonymous=True)
78 |     rospy.Subscriber("joy", Joy, callback)
79 |     rospy.spin()
80 | 


--------------------------------------------------------------------------------
/launch/.gitkeep:
--------------------------------------------------------------------------------
1 | 
2 | 


--------------------------------------------------------------------------------
/launch/mapper_raspi.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <!-- <param name="robot_description"
 3 |     textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />
 4 | 
 5 |   <node name="robot_state_publisher" pkg="robot_state_publisher"
 6 |     type="robot_state_publisher" /> 
 7 |     
 8 |     <include file="$(find ydlidar_ros)/launch/X2L.launch" /> -->
 9 | 
10 |   <node name="cartographer_node" pkg="cartographer_ros"
11 |       type="cartographer_node" args="
12 |           -configuration_directory $(find carto_mapper)/config
13 |           -configuration_basename x2.lua"
14 |       output="screen">
15 |   </node>
16 | 
17 |   <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
18 |       type="cartographer_occupancy_grid_node" args="-resolution 0.05"
19 |       output="screen" />
20 |   
21 |   <node name="rviz" pkg="rviz" type="rviz" args="-d $(find carto_mapper)/mapconfig.rviz"/>
22 | </launch>
23 | 


--------------------------------------------------------------------------------
/launch/mapper_raspi_navi.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <!-- <param name="robot_description"
 3 |     textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />
 4 | 
 5 |   <node name="robot_state_publisher" pkg="robot_state_publisher"
 6 |     type="robot_state_publisher" /> 
 7 |     
 8 |     <include file="$(find ydlidar_ros)/launch/X2L.launch" /> -->
 9 | 
10 |   <node name="cartographer_node" pkg="cartographer_ros"
11 |       type="cartographer_node" args="
12 |           -configuration_directory $(find carto_mapper)/config
13 |           -configuration_basename x2.lua"
14 |       output="screen">
15 |   </node>
16 | 
17 |   <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
18 |       type="cartographer_occupancy_grid_node" args="-resolution 0.05"
19 |       output="screen" />
20 |   
21 |   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
22 |     <rosparam file="$(find carto_mapper)/param/costmap_common_params.yaml" command="load" ns="global_costmap"/>
23 |     <rosparam file="$(find carto_mapper)/param/costmap_common_params.yaml" command="load" ns="local_costmap"/>
24 |     <rosparam file="$(find carto_mapper)/param/local_costmap_params.yaml" command="load"/>
25 |     <rosparam file="$(find carto_mapper)/param/global_costmap_params.yaml" command="load"/>
26 |     <rosparam file="$(find carto_mapper)/param/base_local_planner_params.yaml" command="load" />
27 |   </node>
28 |   
29 |   <node name="rviz" pkg="rviz" type="rviz" args="-d $(find carto_mapper)/mapconfig.rviz"/>
30 | </launch>
31 | 


--------------------------------------------------------------------------------
/navi_raspi_move.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python3
 2 | import rospy
 3 | from geometry_msgs.msg import Twist
 4 | import pigpio
 5 | 
 6 | pi = pigpio.pi()
 7 | pi.set_mode(22, pigpio.OUTPUT) 
 8 | pi.set_mode(23, pigpio.OUTPUT) 
 9 | pi.set_mode(24, pigpio.OUTPUT) 
10 | pi.set_mode(25, pigpio.OUTPUT) 
11 | pi.set_PWM_frequency(22, 100)
12 | pi.set_PWM_frequency(23, 100)
13 | pi.set_PWM_frequency(24, 100)
14 | pi.set_PWM_frequency(25, 100)
15 | 
16 | print('start')
17 | pi.set_PWM_dutycycle(22, 0)
18 | pi.set_PWM_dutycycle(23, 0)
19 | pi.set_PWM_dutycycle(24, 0)
20 | pi.set_PWM_dutycycle(25, 0)
21 | 
22 | duty_forward = 130
23 | duty_direction =  160
24 | rate_x = 10
25 | rate_z = 5
26 | 
27 | def callback(msg):
28 |     linear_x = round(msg.linear.x, 3)
29 |     print(linear_x)
30 |     if linear_x <0 :
31 |         linear_x = linear_x * -1
32 |     angular_z = round(msg.angular.z, 3)
33 | 
34 |     r_moter = int(duty_forward * linear_x * rate_x + duty_direction * angular_z * rate_z)
35 |     l_moter = int(duty_forward * linear_x * rate_x - duty_direction * angular_z * rate_z)
36 | 
37 |     r_moter_23 = 0
38 |     l_moter_25 = 0
39 |     if r_moter > 0 :
40 |         if r_moter > duty_direction :
41 |             r_moter_22 = duty_direction
42 |         else:
43 |             r_moter_22 = r_moter
44 |     else:
45 |         r_moter_22 = 0
46 |     
47 |     if l_moter > 0 :
48 |         if l_moter > duty_direction :
49 |             l_moter_24 = duty_direction
50 |         else:
51 |             l_moter_24 = l_moter
52 |     else:
53 |         l_moter_24 = 0
54 | 
55 |     print(linear_x, angular_z)
56 |     print(r_moter_22, l_moter_24)
57 | 
58 |     pi.set_PWM_dutycycle(22, r_moter_22)
59 |     pi.set_PWM_dutycycle(23, r_moter_23)
60 |     pi.set_PWM_dutycycle(24, l_moter_24)
61 |     pi.set_PWM_dutycycle(25, l_moter_25)
62 |         
63 | if __name__ == '__main__':
64 |     rospy.init_node('navi_raspi_move', anonymous=True)
65 |     rospy.Subscriber("cmd_vel", Twist, callback)
66 |     rospy.spin()
67 | 


--------------------------------------------------------------------------------
/param/.gitkeep:
--------------------------------------------------------------------------------
1 | 
2 | 


--------------------------------------------------------------------------------
/param/costmap_common_params.yaml:
--------------------------------------------------------------------------------
 1 | obstacle_range: 0.5
 2 | raytrace_range: 2.0
 3 | #footprint: [[x0, y0], [x1, y1], ... [xn, yn]]
 4 | #robot_radius: ir_of_robot
 5 | inflation_radius: 0.55
 6 | 
 7 | observation_sources: laser_scan_sensor
 8 | 
 9 | laser_scan_sensor: {sensor_frame: laser_frame, data_type: LaserScan, topic: scan, marking: true, clearing: true}
10 | 
11 | #point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true, clearing: true}
12 | 


--------------------------------------------------------------------------------
/param/dwa_local_planner_params.yaml:
--------------------------------------------------------------------------------
 1 | DWAPlannerROS:
 2 | 
 3 | # Robot Configuration Parameters
 4 |   max_vel_x: 0.15
 5 |   min_vel_x: -0.15
 6 | 
 7 |   max_vel_y: 0.0
 8 |   min_vel_y: 0.0
 9 | 
10 | # The velocity when robot is moving in a straight line
11 |   max_vel_trans:  0.22
12 |   min_vel_trans:  0.11
13 | 
14 |   max_vel_theta: 2.75
15 |   min_vel_theta: 1.37
16 | 
17 |   acc_lim_x: 2.5
18 |   acc_lim_y: 0.0
19 |   acc_lim_theta: 3.2
20 | 
21 | # Goal Tolerance Parametes
22 |   xy_goal_tolerance: 0.5
23 |   yaw_goal_tolerance: 3.14
24 |   latch_xy_goal_tolerance: false
25 | 
26 | # Forward Simulation Parameters
27 |   sim_time: 4.0
28 |   vx_samples: 20
29 |   vy_samples: 0
30 |   vth_samples: 40
31 |   controller_frequency: 10.0
32 | 
33 | # Trajectory Scoring Parameters
34 |   path_distance_bias: 32.0
35 |   goal_distance_bias: 20.0
36 |   occdist_scale: 0.02
37 |   forward_point_distance: 0.325
38 |   stop_time_buffer: 0.2
39 |   scaling_speed: 0.25
40 |   max_scaling_factor: 0.2
41 | 
42 | # Oscillation Prevention Parameters
43 |   oscillation_reset_dist: 0.05
44 | 
45 | # Debugging
46 |   publish_traj_pc : true
47 |   publish_cost_grid_pc: true
48 | 


--------------------------------------------------------------------------------
/param/global_costmap_params.yaml:
--------------------------------------------------------------------------------
1 | global_costmap:
2 |    global_frame: map
3 |    robot_base_frame: base_footprint
4 |    update_frequency: 1.0
5 |    publish_frequency: 0.5
6 |    static_map: true
7 |    transform_tolerance: 0.5
8 | 


--------------------------------------------------------------------------------
/param/local_costmap_params.yaml:
--------------------------------------------------------------------------------
 1 | local_costmap:
 2 |    global_frame: map
 3 |    robot_base_frame: base_footprint
 4 |    update_frequency: 10.0
 5 |    publish_frequency: 5.0
 6 |    static_map: false
 7 |    rolling_window: true
 8 |    width: 6.0
 9 |    height: 6.0
10 |    resolution: 0.05
11 |    transform_tolerance: 0.5
12 | 


--------------------------------------------------------------------------------