cmd_vel and odom integration for the sensorless jetbot

CMakeLists.txt

@@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS
   rospy
   std_msgs
   sensor_msgs
+  controller_manager  
 )
 
 ## System dependencies are found with CMake's conventions
@@ -118,7 +119,7 @@ catkin_package(
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 include_directories(
-# include
+  include
   ${catkin_INCLUDE_DIRS}
   ${CUDA_INCLUDE_DIRS}
 )
@@ -130,6 +131,10 @@ include_directories(/usr/include/gstreamer-1.0 /usr/include/glib-2.0 /usr/includ
 add_executable(jetbot_camera src/jetbot_camera.cpp src/image_converter.cpp)
 target_link_libraries(jetbot_camera ${catkin_LIBRARIES} jetson-utils)
 
+# motor controller
+add_executable(jetbot_controller src/motor2927_controller.cpp src/adafruitmotorhat.cpp src/adafruitdcmotor.cpp src/pwm.cpp)
+target_link_libraries(jetbot_controller ${catkin_LIBRARIES})
+
 ## Declare a C++ library
 # add_library(${PROJECT_NAME}
 #   src/${PROJECT_NAME}/jetbot_ros.cpp

config/diff_drive.yaml

@@ -0,0 +1,50 @@
+mobile_base_controller:
+  type        : "diff_drive_controller/DiffDriveController"
+  left_wheel  : 'robot_left_wheel_hinge'
+  right_wheel : 'robot_right_wheel_hinge'
+  publish_rate: 50.0               # default: 50
+  pose_covariance_diagonal : [0.001, .001, 1000000., 1000000., 1000000., 1000.]
+  twist_covariance_diagonal: [0.001, .001, 1000000., 1000000., 1000000., 1000.]
+
+  # Wheel separation and diameter. These are both optional.
+  # diff_drive_controller will attempt to read either one or both from the
+  # URDF if not specified as a parameter
+  wheel_separation : 0.1025
+  wheel_radius : 0.03
+
+  # Wheel separation and radius multipliers
+  wheel_separation_multiplier: 4.0 # default: 1.0
+  wheel_radius_multiplier    : 0.075 # default: 1.0
+
+  # Odometry to commands calibration, used for dead reckoning
+  v_multiplier : 0.0125
+  w_multiplier : 0.05
+
+  # Velocity commands timeout [s], default 0.5
+  cmd_vel_timeout: 0.25
+
+  enable_odom_tf: true
+
+  # Base frame_id
+  base_frame_id: robot__chassis #default: base_link
+
+  # Velocity and acceleration limits
+  # Whenever a min_* is unspecified, default to -max_*
+  linear:
+    x:
+      has_velocity_limits    : true
+      max_velocity           : 0.5  # m/s
+      min_velocity           : -0.5 # m/s
+      has_acceleration_limits: true
+      max_acceleration       : 1.0  # m/s^2
+      min_acceleration       : -1.0 # m/s^2
+      has_jerk_limits        : true
+      max_jerk               : 5.0  # m/s^3
+  angular:
+    z:
+      has_velocity_limits    : true
+      max_velocity           : 1.7  # rad/s
+      has_acceleration_limits: true
+      max_acceleration       : 1.5  # rad/s^2
+      has_jerk_limits        : true
+      max_jerk               : 2.5  # rad/s^3

launch/jetbot.launch

@@ -0,0 +1,31 @@
+<launch>
+
+  <arg name="model" default="$(find jetbot_ros)/model/jetbot.urdf"/>
+  <arg name="gui" default="true"/>
+  <arg name="rvizconfig" default="$(find jetbot_ros)/rviz/urdf.rviz" />
+
+  <node pkg="tf2_ros" type="static_transform_publisher" name="laserlink_broadcaster" args="0.05 0 0.03 0 0 0 robot__chassis base_laser" />
+  <node pkg="tf2_ros" type="static_transform_publisher" name="chassis_broadcaster" args="0 0 0 0 0 0 base_link robot__chassis" />
+  <node pkg="tf2_ros" type="static_transform_publisher" name="odom_broadcaster" args="0 0 0 0 0 0 odom map" /> 
+
+  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)"/>
+  <param name="use_gui" value="gui"/>
+
+  <node name="laser_node" pkg="urg_node" type="urg_node" >
+    <param name="serial_port" value="/dev/ttyACM0"/>
+    <param name="frame_id" value="base_laser"/>
+  </node>
+
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+  <!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" /> -->
+
+  <!-- <node name="map_server" pkg="map_server" type="map_server" args="$(find jetbot_ros)/map/test_map.yaml"/> -->
+  <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="map_base_broadcaster" args="0 0 0 0 0 0 map base_link" /> -->
+
+  <node name="motor_controller" pkg="jetbot_ros" type="jetbot_controller" output="screen"/> <!--output="screen"/> -->
+
+  <rosparam file="$(find jetbot_ros)/config/diff_drive.yaml" command="load" />
+  <node name="jetbot_controller_manager" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="mobile_base_controller"/>
+
+</launch>

model/jetbot.urdf

@@ -0,0 +1,94 @@
+<?xml version="1.0" ?>
+<robot name="robot">
+  <joint name="robot_left_wheel_hinge" type="continuous">
+    <parent link="robot__chassis"/>
+    <child link="robot__left_wheel"/>
+    <origin rpy="0       0       1.57079" xyz="0.03     0.05125  0.03"/>
+    <axis xyz="1  0  0"/>
+    <limit effort="0" lower="0" upper="0" velocity="0"/>
+  </joint>
+  <joint name="robot_right_wheel_hinge" type="continuous">
+    <parent link="robot__chassis"/>
+    <child link="robot__right_wheel"/>
+    <origin rpy="0       0       1.57079" xyz="0.03   -0.0595  0.03"/>
+    <axis xyz="1  0  0"/>
+    <limit effort="0" lower="0" upper="0" velocity="0"/>
+  </joint>
+  <joint name="robot__camera_joint" type="fixed">
+    <parent link="robot__chassis"/>
+    <child link="robot__camera_link"/>
+    <origin rpy="0    0.25  0" xyz="0.055   0      0.0857"/>
+    <axis xyz="0.96891  0       0.2474"/>
+    <limit effort="0" lower="0" upper="0" velocity="0"/>
+  </joint>
+  <link name="robot__chassis">
+    <inertial>
+      <mass value="0"/>
+      <origin rpy="0  0  0" xyz="0  0  0"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <collision name="robot__collision">
+      <origin rpy="1.57079  0       1.57079" xyz="0     0     0.043"/>
+      <geometry>
+        <mesh filename="package://jetbot_ros/gazebo/jetbot/meshes/JetBot-v3-Chassis.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+    <visual name="robot__visual">
+      <origin rpy="1.57079  0       1.57079" xyz="0     0     0.043"/>
+      <geometry>
+        <mesh filename="package://jetbot_ros/gazebo/jetbot/meshes/JetBot-v3-Chassis.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+  </link>
+  <link name="robot__left_wheel">
+    <inertial>
+      <mass value="0"/>
+      <origin rpy="0  0  0" xyz="0  0  0"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <collision name="robot__collision">
+      <origin rpy="0  0  0" xyz="0  0  0"/>
+      <geometry>
+        <mesh filename="package://jetbot_ros/gazebo/jetbot/meshes/JetBot-v3-Wheel.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+    <visual name="robot__visual">
+      <origin rpy="0  0  0" xyz="0  0  0"/>
+      <geometry>
+        <mesh filename="package://jetbot_ros/gazebo/jetbot/meshes/JetBot-v3-Wheel.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+  </link>
+  <link name="robot__right_wheel">
+    <inertial>
+      <mass value="0"/>
+      <origin rpy="0  0  0" xyz="0  0  0"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <collision name="robot__collision">
+      <origin rpy="0  0  0" xyz="0  0  0"/>
+      <geometry>
+        <mesh filename="package://jetbot_ros/gazebo/jetbot/meshes/JetBot-v3-Wheel.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+    <visual name="robot__visual">
+      <origin rpy="0  0  0" xyz="0  0  0"/>
+      <geometry>
+        <mesh filename="package://jetbot_ros/gazebo/jetbot/meshes/JetBot-v3-Wheel.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+  </link>
+  <link name="robot__camera_link">
+    <inertial>
+      <mass value="0.1"/>
+      <origin rpy="0  0  0" xyz="0  0  0"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual name="robot__visual">
+      <origin rpy="0  0  0" xyz="0  0  0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+  </link>
+</robot>

src/adafruitdcmotor.cpp

@@ -0,0 +1,99 @@
+/**
+ *  adafruitdcmotor.cpp
+ *
+ *  MIT License
+ *
+ *  Copyright (c) 2018, Tom Clarke
+ *
+ *  Permission is hereby granted, free of charge, to any person obtaining a copy
+ *  of this software and associated documentation files (the "Software"), to deal
+ *  in the Software without restriction, including without limitation the rights
+ *  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ *  copies of the Software, and to permit persons to whom the Software is
+ *  furnished to do so, subject to the following conditions:
+ *
+ *  The above copyright notice and this permission notice shall be included in all
+ *  copies or substantial portions of the Software.
+ *
+ *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ *  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ *  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ *  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ *  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ *  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *  SOFTWARE.
+ */
+
+#include "adafruitdcmotor.h"
+
+#include <iostream>
+#include <algorithm>
+
+AdafruitDCMotor::AdafruitDCMotor (PWM& pwm, int index)
+    : controller (pwm)
+{
+    switch (index)
+    {
+        case 0:
+            pwmPin = 8;
+            in2Pin = 9;
+            in1Pin = 10;
+            break;
+        case 1:
+            pwmPin = 13;
+            in2Pin = 12;
+            in1Pin = 11;
+            break;
+        case 2:
+            pwmPin = 2;
+            in2Pin = 3;
+            in1Pin = 4;
+            break;
+        case 3:
+            pwmPin = 7;
+            in2Pin = 6;
+            in1Pin = 5;
+            break;
+        default:
+            std::cerr << "Motor index out-of-range. Must be between 1 and 4 inclusive." << std::endl;
+            break;
+    }
+}
+
+void AdafruitDCMotor::run (Command command)
+{
+    switch (command)
+    {
+        case kForward:
+            setPin (in1Pin, true);
+            setPin (in2Pin, false);
+            break;
+        case kBackward:
+            setPin (in1Pin, false);
+            setPin (in2Pin, true);
+            break;
+        case kRelease:
+            setPin (in1Pin, false);
+            setPin (in2Pin, false);
+            break;
+        default:
+            break;
+    }
+}
+
+void AdafruitDCMotor::setSpeed (int speed)
+{
+    speed = std::max (0, std::min (speed, 255));
+    controller.setChannel (pwmPin, 0, speed * 16);
+}
+
+void AdafruitDCMotor::setPin (int pin, bool enabled)
+{
+    if (pin < 0 || pin > 15)
+    {
+        std::cerr << "Failed to set PWM pin " << pin << ". Must be between 0 and 15 inclusive." << std::endl;
+        return;
+    }
+
+    controller.setChannel (pin, enabled ? 4096 : 0, enabled ? 0 : 4096);
+}

src/adafruitdcmotor.h

@@ -0,0 +1,59 @@
+/**
+ *  adafruitdcmotor.h
+ *
+ *  MIT License
+ *
+ *  Copyright (c) 2018, Tom Clarke
+ *
+ *  Permission is hereby granted, free of charge, to any person obtaining a copy
+ *  of this software and associated documentation files (the "Software"), to deal
+ *  in the Software without restriction, including without limitation the rights
+ *  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ *  copies of the Software, and to permit persons to whom the Software is
+ *  furnished to do so, subject to the following conditions:
+ *
+ *  The above copyright notice and this permission notice shall be included in all
+ *  copies or substantial portions of the Software.
+ *
+ *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ *  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ *  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ *  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ *  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ *  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *  SOFTWARE.
+ */
+
+#pragma once
+
+#include "pwm.h"
+
+class AdafruitDCMotor
+{
+public:
+    enum Command
+    {
+        kForward = 1,
+        kBackward = 2,
+        kBrake = 3,
+        kRelease = 4,
+    };
+
+    AdafruitDCMotor (PWM& pwm, int index);
+
+    /** Makes the motor perform an action.
+     *  @see Commands
+     */
+    void run (Command command);
+
+    /** Sets the speed of the motor.
+     *  Expects a value between 0 and 255 inclusive.
+     */
+    void setSpeed (int speed);
+
+private:
+    void setPin (int pin, bool enabled);
+
+    PWM& controller;
+    int pwmPin = 0, in1Pin = 0, in2Pin = 0;
+};