Skip to content

GitLab

Commit 0fef89f8 authored by Günter Niemeyer's avatar Günter Niemeyer
Browse files

Added example package with wheelcontrol and odometry 

Added the example package to show my versions of the low level wheel
control and odometry nodes.  This also includes the launch files.  As
well as a timestamp fix to the pointcloud, without which you can not
drive the robot and see the pointcloud in the correct place.  Try
  roslaunch example odometry.launch
  roslaunch example odometry_with_pointcloud.launch
run together with the teleop
  rosrun shared_demo teleop.py
And more importantly, take a look at the files and let me know if you
have questions!
parent be8b7f65
Hide whitespace changes
Inline Side-by-side
Showing
with 1615 additions and 0 deletions
+1615 -0
example/CMakeLists.txt 0 → 100644
View file @ 0fef89f8
cmake_minimum_required(VERSION 3.0.2)
project(example)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
sensor_msgs
geometry_msgs
visualization_msgs
shared_demo
bot_description
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES example
# CATKIN_DEPENDS
# roscpp
# rospy
# std_msgs
# sensor_msgs
# geometry_msgs
# visualization_msgs
# shared_demo
# bot_description
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/example.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/example_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_example.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
example/launch/odometry.launch 0 → 100644
View file @ 0fef89f8
<launch>
<!-- Test the odometry node (and lower levels). -->
<!-- URDF parameter. -->
<arg name="urdf" default="bot.urdf"/>
<arg name="model" default="$(find bot_description)/urdf/$(arg urdf)"/>
<!-- Rviz configuration file parameter. -->
<arg name="cfg" default="$(find example)/rviz/odometry.rviz"/>
<!-- Load the URDF file into the robot_description parameter. -->
<param name="robot_description" textfile="$(arg model)"/>
<!-- Start the low-level wheelcontrol node -->
<node name="wheelcontrol"
pkg="example"
type="wheelcontrol.py"
output="screen"/>
<!-- Start the odometry node -->
<node name="odometry"
pkg="example"
type="odometry.py"
output="screen"/>
<!-- Start the teleop node SEPARATELY -->
<!--
<node name="teleop"
pkg="shared_demo"
type="teleop.py"
output="screen"/>
-->
<!-- Publish all the robot frames. -->
<node name="robot_state_publisher"
pkg="robot_state_publisher"
type="robot_state_publisher"/>
<!-- Run the rviz visualization, with the specified config file -->
<!-- Kill everything if this stops. -->
<node pkg="rviz"
type="rviz"
name="rviz"
args="-d $(arg cfg)"
output="screen"
required="true"/>
</launch>
example/launch/odometry_with_pointcloud.launch 0 → 100644
View file @ 0fef89f8
<launch>
<!-- Show the RealSense pointcloud on top of the odometry. -->
<!-- URDF parameter. -->
<arg name="urdf" default="bot.urdf"/>
<arg name="model" default="$(find bot_description)/urdf/$(arg urdf)"/>
<!-- Rviz configuration file parameter. -->
<arg name="cfg" default="$(find example)/rviz/pointcloud.rviz"/>
<!-- Load the URDF file into the robot_description parameter. -->
<param name="robot_description" textfile="$(arg model)"/>
<!-- Start the low-level wheelcontrol node -->
<node name="wheelcontrol"
pkg="example"
type="wheelcontrol.py"
output="screen"/>
<!-- Start the odometry node -->
<node name="odometry"
pkg="example"
type="odometry.py"
output="screen"/>
<!-- Start the teleop node SEPARATELY -->
<!--
<node name="teleop"
pkg="shared_demo"
type="teleop.py"
output="screen"/>
-->
<!-- Start the RealSense, also publishing a pointcloud! -->
<include file="$(find realsense2_camera)/launch/rs_camera.launch">
<arg name="depth_width" value="640"/>
<arg name="depth_height" value="480"/>
<arg name="depth_fps" value="6"/>
<arg name="color_width" value="640"/>
<arg name="color_height" value="480"/>
<arg name="color_fps" value="6"/>
<arg name="enable_pointcloud" value="true"/>
<arg name="allow_no_texture_points" value="true"/>
<!-- Replace RS2_STREAM_COLOR with RS_STREAM_ANY to -->
<!-- create the pointcloud without the RGB colors. -->
<arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
</include>
<!-- The following "fixes" the point cloud - it has bad timestamps, -->
<!-- which causes RVIZ to be unable to display the data properly -->
<!-- synchronized with a moving bot. Fix this by resetting the -->
<!-- timestamp and republishing on a new topic, which RVIZ renders! -->
<node name="fixpointcloud"
pkg="example"
type="fixpointcloudtimestamp.py"
output="screen"/>
<!-- Publish all the robot frames. -->
<node name="robot_state_publisher"
pkg="robot_state_publisher"
type="robot_state_publisher"/>
<!-- Run the rviz visualization, with the specified config file -->
<!-- Kill everything if this stops. -->
<node pkg="rviz"
type="rviz"
name="rviz"
args="-d $(arg cfg)"
output="screen"
required="true"/>
</launch>
example/package.xml 0 → 100644
View file @ 0fef89f8
<?xml version="1.0"?>
<package format="2">
<name>example</name>
<version>0.0.0</version>
<description>The is example code for the 129 projects</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="student@todo.todo">student</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/basic</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>shared_demo</build_depend>
<build_depend>bot_description</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>visualization_msgs</build_export_depend>
<build_export_depend>shared_demo</build_export_depend>
<build_export_depend>bot_description</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>visualization_msgs</exec_depend>
<exec_depend>shared_demo</exec_depend>
<exec_depend>bot_description</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
example/rviz/odometry.rviz 0 → 100644
View file @ 0fef89f8
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /RobotModel1/Links1
Splitter Ratio: 0.5
Tree Height: 366
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
leftwheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
rightwheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 100
Name: Odometry
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 255; 25; 0
Head Length: 0.029999999329447746
Head Radius: 0.019999999552965164
Shaft Length: 0.10000000149011612
Shaft Radius: 0.004999999888241291
Value: Arrow
Topic: /odom
Unreliable: false
Value: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
base:
Value: true
leftwheel:
Value: true
odom:
Value: true
rightwheel:
Value: true
Marker Scale: 0.5
Name: TF
Show Arrows: false
Show Axes: true
Show Names: true
Tree:
odom:
base:
leftwheel:
{}
rightwheel:
{}
Update Interval: 0
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: odom
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 2.2899391651153564
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.4353983998298645
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.8203982710838318
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 691
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000204fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006500fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004400000204000000de00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000223fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004400000223000000b900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ee00000044fc0100000002fb0000000800540069006d00650100000000000003ee0000028000fffffffb0000000800540069006d006501000000000000045000000000000000000000027e0000020400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1006
X: -3
Y: 40
example/rviz/pointcloud.rviz 0 → 100644
View file @ 0fef89f8
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /RobotModel1/Links1
Splitter Ratio: 0.5
Tree Height: 366
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
axle:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base:
Alpha: 1
Show Axes: false
Show Trail: false
camera_link:
Alpha: 1
Show Axes: false
Show Trail: false
cameraplate:
Alpha: 1
Show Axes: false
Show Trail: false
laser:
Alpha: 1
Show Axes: false
Show Trail: false
leftwheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
rightwheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 100
Name: Odometry
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 255; 25; 0
Head Length: 0.029999999329447746
Head Radius: 0.019999999552965164
Shaft Length: 0.10000000149011612
Shaft Radius: 0.004999999888241291
Value: Arrow
Topic: /odom
Unreliable: false
Value: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
axle:
Value: true
base:
Value: true
camera_color_frame:
Value: true
camera_color_optical_frame:
Value: true
camera_depth_frame:
Value: true
camera_depth_optical_frame:
Value: true
camera_link:
Value: true
cameraplate:
Value: true
laser:
Value: true
leftwheel:
Value: true
odom:
Value: true
rightwheel:
Value: true
Marker Scale: 0.5
Name: TF
Show Arrows: false
Show Axes: true
Show Names: true
Tree:
odom:
base:
axle:
cameraplate:
camera_link:
camera_color_frame:
camera_color_optical_frame:
{}
camera_depth_frame:
camera_depth_optical_frame:
{}
laser:
{}
leftwheel:
{}
rightwheel:
{}
Update Interval: 0
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /points
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: odom
Frame Rate: 6
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 2.4555232524871826
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.36064258217811584
Y: 0.1478605717420578
Z: 0.20718321204185486
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.4203982651233673
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 2.5653975009918213
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 691
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000204fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006500fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004400000204000000de00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000223fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004400000223000000b900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ee00000044fc0100000002fb0000000800540069006d00650100000000000003ee0000028000fffffffb0000000800540069006d006501000000000000045000000000000000000000027e0000020400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1006
X: 9
Y: 38
example/scripts/fixpointcloudtimestamp.py 0 → 100755
View file @ 0fef89f8
#!/usr/bin/env python
#
# fixpointcloudtimestamp.py
#
# The RealSense images and pointcloud seem to have an incorrect time
# stamp - for me, 10 hours in the past and also not monotonically
# advancing. This doesn't disturb the data. But it means the TF
# library can't convert the data into other frames and RVIZ can't
# display the data with respect to the odom frame (fixed in the
# world). So, effectively, RVIZ doesn't like to display the data.
#
# We fix this, simply by resetting the timestamp to the current
# time.
#
# Node: /fixpointcloud
# Publish: /points sensor_msgs/PointCloud2
# subscribe: /camera/depth/color/points sensor_msgs/PointCloud2
#
import rospy
from sensor_msgs.msg import PointCloud2
#
# Pointcloud Callback
#
def callback(msg):
# Update the header timestamp and republish.
msg.header.stamp = rospy.Time.now()
pub.publish(msg)
#
# Main Code
#
if __name__ == "__main__":
# Initialize the ROS node.
rospy.init_node('fixpointcloud')
# Create a publisher for the fixed pointcloud.
pub = rospy.Publisher('/points', PointCloud2, queue_size=10)
# Create a subscriber to listen to raw pointcloud.
rospy.Subscriber('/camera/depth/color/points', PointCloud2, callback)
# Report and spin (waiting while callbacks do their work).
rospy.loginfo("Fixing the pointcloud timestamps...")
rospy.spin()
rospy.loginfo("Done fixing the pointcloud timestamps.")
example/scripts/odometry.py 0 → 100755
View file @ 0fef89f8
#!/usr/bin/env python
#
# odometry.py
#
# Odometry node.
#
# Node: /odometry
# Publish: /odom geometry_msgs/TransJointState
# TF map -> odom geometry_msgs/TransformStamped
# /wheel_command sensor_msgs/JointState
# Subscribe: /vel_cmd geometry_msgs/Twist
# /wheel_state sensor_msgs/JointState
#
import math
import rospy
import tf2_ros
from geometry_msgs.msg import Twist
from geometry_msgs.msg import TransformStamped
from nav_msgs.msg import Odometry
from sensor_msgs.msg import JointState
#
# Constants
#
R = 0.03 # Wheel radius
d = 0.065 # Halfwidth between wheels
# Global variables
lpsi_last = 0.0 # Last wheel position (psi)
rpsi_last = 0.0
x = 0.0 # Current pose (x,y,theta)
y = 0.0
theta = 0.0
#
# Twist Command Callback
#
def callback_vel_cmd(msg):
# Grab the forward and spin velocities.
vx = msg.linear.x
wz = msg.angular.z
# Compute the wheel velocities.
lpsi_dot = ( vx - d*wz) / R
rpsi_dot = (-vx - d*wz) / R
# Create the wheel command msg and publish.
msg_wcmd = JointState()
msg_wcmd.header.stamp = rospy.Time.now()
msg_wcmd.name = ['leftwheel', 'rightwheel']
msg_wcmd.velocity = [lpsi_dot, rpsi_dot]
pub_wcmd.publish(msg_wcmd)
#
# Wheel State Callback
#
def callback_wheel_state(msg):
# Grab the wheel position/velocities.
try:
lpsi = msg.position[msg.name.index('leftwheel')]
lpsi_dot = msg.velocity[msg.name.index('leftwheel')]
rpsi = msg.position[msg.name.index('rightwheel')]
rpsi_dot = msg.velocity[msg.name.index('rightwheel')]
except:
rospy.logerr("Ill-formed /wheel_state message!")
return
# Compute the velocity.
vx = 0.5 * R * ( lpsi_dot - rpsi_dot)
wz = 0.5 * R/d * (- lpsi_dot - rpsi_dot)
# Compute the wheel position changes.
global lpsi_last, rpsi_last
lpsi_diff = lpsi - lpsi_last
rpsi_diff = rpsi - rpsi_last
lpsi_last = lpsi
rpsi_last = rpsi
# Compute the position/heading change
dp = 0.5 * R * ( lpsi_diff - rpsi_diff)
dtheta = 0.5 * R/d * (- lpsi_diff - rpsi_diff)
# Update the pose.
global x, y, theta
x += dp * math.cos(theta + 0.5*dtheta)
y += dp * math.sin(theta + 0.5*dtheta)
theta += dtheta
# Create the odometry msg and publish.
msg_odom = Odometry()
msg_odom.header.stamp = msg.header.stamp
msg_odom.header.frame_id = 'odom'
msg_odom.child_frame_id = 'base'
msg_odom.pose.pose.position.x = x
msg_odom.pose.pose.position.y = y
msg_odom.pose.pose.position.z = 0.0
msg_odom.pose.pose.orientation.x = 0.0
msg_odom.pose.pose.orientation.y = 0.0
msg_odom.pose.pose.orientation.z = math.sin(theta/2)
msg_odom.pose.pose.orientation.w = math.cos(theta/2)
msg_odom.twist.twist.linear.x = vx
msg_odom.twist.twist.linear.y = 0.0
msg_odom.twist.twist.linear.z = 0.0
msg_odom.twist.twist.angular.x = 0.0
msg_odom.twist.twist.angular.y = 0.0
msg_odom.twist.twist.angular.z = wz
pub_odom.publish(msg_odom)
# Create the transform msg and broadcast.
msg_tf = TransformStamped()
msg_tf.header.stamp = msg.header.stamp
msg_tf.header.frame_id = 'odom'
msg_tf.child_frame_id = 'base'
msg_tf.transform.translation.x = x
msg_tf.transform.translation.y = y
msg_tf.transform.translation.z = 0.0
msg_tf.transform.rotation.x = 0.0
msg_tf.transform.rotation.y = 0.0
msg_tf.transform.rotation.z = math.sin(theta/2)
msg_tf.transform.rotation.w = math.cos(theta/2)
brd_tf.sendTransform(msg_tf)
#
# Main Code
#
if __name__ == "__main__":
# Initialize the ROS node.
rospy.init_node('odometry')
# Create a publisher to send wheel commands.
pub_wcmd = rospy.Publisher('/wheel_command', JointState, queue_size=10)
# Create a publisher to send odometry information.
pub_odom = rospy.Publisher('/odom', Odometry, queue_size=10)
# Create a TF2 transform broadcaster.
brd_tf = tf2_ros.TransformBroadcaster()
# Create a subscriber to listen to twist commands.
rospy.Subscriber('/vel_cmd', Twist, callback_vel_cmd)
# Create a subscriber to listen to wheel state.
rospy.Subscriber('/wheel_state', JointState, callback_wheel_state)
# Report and spin (waiting while callbacks do their work).
rospy.loginfo("Odometry spinning...")
rospy.spin()
rospy.loginfo("Odometry stopped.")
example/scripts/wheelcontrol.py 0 → 100755
View file @ 0fef89f8
#!/usr/bin/env python
#
# wheelcontrol.py
#
# This is a full implementation of the Wheel-Control node. Beyond
# the basic, it
# - stops if no commands are received after 0.25sec
# - filters the actual velocity
# - predicts the wheel speed to more carefully set direction
# - filters the incoming command to avoid jerks/high torque loads
# - adds feedback to perfectly achieve the velocity commands
# - returns the PWM value the effort of the /wheel_state
#
# Node: /wheelcontrol
# Publish: /wheel_state sensor_msgs/JointState
# Subscribe: /wheel_command sensor_msgs/JointState
#
# Other Inputs: Two Encoder Channels (GPIO)
# Other Outputs: Motor Driver Commands (via I2C, two channels)
#
import math
import sys
import time
import rospy
import pigpio
import qwiic_scmd
from sensor_msgs.msg import JointState
#
# Constants
#
LEFT = 0
RIGHT = 1
AXES = 2
AXISNAME = ['leftwheel', 'rightwheel']
# Motor parameters
MOTOR_CHANNEL = [0, 1] # Channel number. Should be unique.
MOTOR_FWD = [1, 1] # Each direction can be either 1 or 0.
PWM_MIN = [35.0, 35.0]
PWM_PER_RADPERSEC = [ 9.0, 9.0]
# Encoder parameters
GPIO_PIN = [6, 16]
ENCTICKS_PER_REV = [360, 360]
# Tuning parameters and time constants.
RATE = 50.0
DT = 1/RATE
T_CMD_FILTER = 0.1 # Filter the incoming velocity command
T_VEL_FILTER = 0.1 # Filter the measured velocity
T_VEL_EXP = 0.1 # Expected/predicted velocity
T_TIMEOUT = 0.25 # Shutoff timeout
T_FEEDBACK = 0.5 # Convergence time of feedback
MINIMUM_SENSE_VEL = 1.0 # Direction should be clear above this
MINIMUM_MOVE_VEL = 0.02 # Minimum velocity we want to move
MAXIMUM_FEEDBACK_VEL = 4.0 # Max speed to add for feedback
#
# Global Variables (used between callbacks)
#
# Encoders:
encdirection = [0] * AXES
enccount = [0] * AXES
enclast = [0] * AXES
# Incoming command messages:
cmdvel = [0] * AXES
cmdtime = [rospy.Time(0)] * AXES
# Desired (possibly smoothed command) and additive feedback velocity
pos_des = [0.0] * AXES
vel_des = [0.0] * AXES
vel_fbk = [0.0] * AXES
# Expected - use only to determine the encoder counting direction.
vel_exp = [0.0] * AXES
# Actual. The effort is the signed PWM value.
pos = [0.0] * AXES
vel = [0.0] * AXES
eff = [0.0] * AXES
#
# Encoder Callback Functions
#
def callback_left(gpio, level, tick):
global enccount, encdirection
enccount[LEFT] += encdirection[LEFT]
def callback_right(gpio, level, tick):
global enccount, encdirection
enccount[RIGHT] += encdirection[RIGHT]
#
# Command Callback Function
#
# Note this allows for the incoming message to only specify one of
# the motors (in case we ever wanted to command them independently).
#
def callback_command(msg):
# Check the message structure.
if len(msg.name) != len(msg.velocity):
rospy.logerr("Wheel command msg name/velocity must have same length")
return
# Note the current time (to timeout the command).
now = rospy.Time.now()
# Extract the velocity commands.
global cmdvel, cmdtime
for i in range(len(msg.name)):
if msg.name[i] in AXISNAME:
axis = AXISNAME.index(msg.name[i])
cmdvel[axis] = msg.velocity[i]
cmdtime[axis] = now
else:
rospy.logerr("Wheel Command msg unknown name '%s'" % msg.name[i])
return
#
# Timer Callback Function
#
def callback_timer(event):
# write to the global variables
global encdirection, enccount, enclast
global cmdvel, cmdtime
global pos_des, vel_des, vel_fbk
global vel_exp
global pos, vel, eff
# Process the encoders into position, filtering the velocity.
for axis in range(AXES):
ratio = 2 * math.pi / ENCTICKS_PER_REV[axis]
enc = enccount[axis]
pos[axis] = ratio * enc
velraw = ratio * (enc - enclast[axis]) / DT
vel[axis] += min(1, DT/T_VEL_FILTER) * (velraw-vel[axis])
enclast[axis] = enc
# Predict the velocity to set the direction (from last cycle's cmd).
for axis in range(AXES):
vel_exp[axis] += min(1, DT/T_VEL_EXP) * (vel_des[axis]-vel_exp[axis])
# Process the commands.
for axis in range(AXES):
# Check the validity of the new command.
if ((event.current_real-cmdtime[axis] > rospy.Duration(T_TIMEOUT))
or (abs(cmdvel[axis]) < MINIMUM_MOVE_VEL)):
cmdvel[axis] = 0.0
# Update the desired position and velocity.
pos_des[axis] += DT * vel_des[axis]
vel_des[axis] += min(1, DT/T_CMD_FILTER) * (cmdvel[axis]-vel_des[axis])
if abs(vel_des[axis]) < min(1, DT/T_CMD_FILTER) * MINIMUM_MOVE_VEL:
vel_des[axis] = 0.0
# Compute an additional feedback velocity. Never exceed the
# limit or change the desired velocity direction. Adjust the
# desired position accordingly, so we don't build up errors.
vel_fbk[axis] = (pos_des[axis]-pos[axis]) / T_FEEDBACK
vel_fbk[axis] = min(vel_fbk[axis], MAXIMUM_FEEDBACK_VEL)
vel_fbk[axis] = max(vel_fbk[axis], -MAXIMUM_FEEDBACK_VEL)
if (vel_fbk[axis] * vel_des[axis] <= - 0.5 * vel_des[axis]**2):
vel_fbk[axis] = - 0.5 * vel_des[axis]
pos_des[axis] = pos[axis] + vel_fbk[axis] * T_FEEDBACK
# Send the PWM.
for axis in range(AXES):
# Set the motor speed from the desired and feedback velocity.
speed = vel_des[axis] + vel_fbk[axis]
# Set the PWM.
pwm = min(254, PWM_MIN[axis] + PWM_PER_RADPERSEC[axis] * abs(speed))
if speed > 0.0:
driver.set_drive(MOTOR_CHANNEL[axis], MOTOR_FWD[axis], pwm)
eff[axis] = pwm
elif speed < 0.0:
driver.set_drive(MOTOR_CHANNEL[axis], 1-MOTOR_FWD[axis], pwm)
eff[axis] = -pwm
else:
driver.set_drive(MOTOR_CHANNEL[axis], 0, 0)
eff[axis] = 0
# Check the encoder count direction.
for axis in range(AXES):
if vel_exp[axis] > MINIMUM_SENSE_VEL:
encdirection[axis] = 1
elif vel_exp[axis] < -MINIMUM_SENSE_VEL:
encdirection[axis] = -1
elif (vel_des[axis] > vel_exp[axis]) and (vel_exp[axis] >= 0):
encdirection[axis] = 1
elif (vel_des[axis] < vel_exp[axis]) and (vel_exp[axis] <= 0):
encdirection[axis] = -1
else:
encdirection[axis] = 0
# Collect the current info into a message and publish.
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = AXISNAME
msg.position = pos
msg.velocity = vel
msg.effort = eff
pub.publish(msg)
#
# Main Code
#
if __name__ == "__main__":
# Initialize the ROS node.
rospy.init_node('wheelcontrol')
# Initialize a connection to the pigpio daemon.
io = pigpio.pi()
if not io.connected:
rospy.logerr("Unable to connection to pigpio daemon!")
sys.exit(-1)
rospy.loginfo("Connected to pigpio daemon")
# Initialize a connection to the motor driver.
driver = qwiic_scmd.QwiicScmd()
if not driver.connected:
rospy.logerr("Motor Driver not connected!")
io.stop()
sys.exit(-1)
driver.begin()
rospy.loginfo("Motor driver initialized.")
# Setup the input pins with a pull-up resistor. The encoders
# (Hall Effect chip) are open drain output, i.e. only pull down.
for pin in GPIO_PIN:
io.set_mode(pin, pigpio.INPUT)
io.set_pull_up_down(pin, pigpio.PUD_UP)
# Set up the encoder callback handlers.
cb_left = io.callback(GPIO_PIN[LEFT], pigpio.EITHER_EDGE, callback_left)
cb_right = io.callback(GPIO_PIN[RIGHT], pigpio.EITHER_EDGE, callback_right)
# Create a subscriber to listen to wheel commands.
sub = rospy.Subscriber("/wheel_command", JointState, callback_command)
# Create a publisher to send the wheel state.
pub = rospy.Publisher('/wheel_state', JointState, queue_size=10)
# Create the timer.
timer = rospy.Timer(rospy.Duration(DT), callback_timer)
# Finally enable the motor driver.
for channel in MOTOR_CHANNEL:
driver.set_drive(channel, 0, 0)
driver.enable()
rospy.loginfo("Motor driver enabled.")
# Spin while the callbacks are doing all the work.
rospy.loginfo("Running with dt = %.3f sec..." % DT)
rospy.spin()
rospy.loginfo("Stopping...")
# Stop the timer (if not already done).
timer.shutdown()
# Cleanup the motor driver.
for channel in MOTOR_CHANNEL:
driver.set_drive(channel, 0, 0)
driver.disable()
rospy.loginfo("Motor driver disabled.")
# Cleanup the GPIO (first disable callbacks and wait for anything pending).
cb_left.cancel()
cb_right.cancel()
time.sleep(0.25)
io.stop()
rospy.loginfo("GPIO cleaned up.")
example/scripts/wheelcontrol_basic.py 0 → 100755
View file @ 0fef89f8
#!/usr/bin/env python
#
# wheelcontrol_basic.py
#
# This is a basic implementation of the Wheel-Control node.
#
# Node: /wheelcontrol
# Publish: /wheel_state sensor_msgs/JointState
# Subscribe: /wheel_command sensor_msgs/JointState
#
# Other Inputs: Two Encoder Channels (GPIO)
# Other Outputs: Motor Driver Commands (via I2C, two channels)
#
import math
import sys
import time
import rospy
import pigpio
import qwiic_scmd
from sensor_msgs.msg import JointState
#
# Constants
#
LEFT = 0
RIGHT = 1
AXES = 2
AXISNAME = ['leftwheel', 'rightwheel']
# Motor parameters
MOTOR_CHANNEL = [0, 1] # Channel number. Should be unique.
MOTOR_FWD = [1, 1] # Each direction can be either 1 or 0.
PWM_MIN = [35.0, 35.0]
PWM_PER_RADPERSEC = [ 9.0, 9.0]
# Encoder parameters
GPIO_PIN = [6, 16]
ENCTICKS_PER_REV = [360, 360]
# Tuning parameters and time constants.
RATE = 50.0
DT = 1/RATE
#
# Global Variables (used between callbacks)
#
# Encoders:
encdirection = [0] * AXES
enccount = [0] * AXES
enclast = [0] * AXES
# Actual. The effort is the signed PWM value.
pos = [0.0] * AXES
vel = [0.0] * AXES
eff = [0.0] * AXES
#
# Encoder Callback Functions
#
def callback_left(gpio, level, tick):
global enccount, encdirection
enccount[LEFT] += encdirection[LEFT]
def callback_right(gpio, level, tick):
global enccount, encdirection
enccount[RIGHT] += encdirection[RIGHT]
#
# Command Callback Function
#
def callback_command(msg):
# Process the commands.
global encdirection, eff
for axis in range(AXES):
# Grab the desired speed.
speed = msg.velocity[msg.name.index(AXISNAME[axis])]
# Set the PWM and encoder direction,
pwm = min(254, PWM_MIN[axis] + PWM_PER_RADPERSEC[axis] * abs(speed))
if speed > 0.0:
driver.set_drive(MOTOR_CHANNEL[axis], MOTOR_FWD[axis], pwm)
encdirection[axis] = 1
elif speed < 0.0:
driver.set_drive(MOTOR_CHANNEL[axis], 1-MOTOR_FWD[axis], pwm)
encdirection[axis] = -1
else:
driver.set_drive(MOTOR_CHANNEL[axis], 0, 0)
encdirection[axis] = 0
# Save the signed PWM value.
eff[axis] = encdirection[axis] * pwm
#
# Timer Callback Function
#
def callback_timer(event):
# Process the encoders into position, differentiate for velocity.
global enclast
global pos, vel, eff
for axis in range(AXES):
ratio = 2 * math.pi / ENCTICKS_PER_REV[axis]
enc = enccount[axis]
pos[axis] = ratio * enc
vel[axis] = ratio * (enc - enclast[axis]) / DT
enclast[axis] = enc
# Collect the current info into a message and publish.
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = AXISNAME
msg.position = pos
msg.velocity = vel
msg.effort = eff
pub.publish(msg)
#
# Main Code
#
if __name__ == "__main__":
# Initialize the ROS node.
rospy.init_node('wheelcontrol')
# Initialize a connection to the pigpio daemon.
io = pigpio.pi()
if not io.connected:
rospy.logerr("Unable to connection to pigpio daemon!")
sys.exit(-1)
rospy.loginfo("Connected to pigpio daemon")
# Initialize a connection to the motor driver.
driver = qwiic_scmd.QwiicScmd()
if not driver.connected:
rospy.logerr("Motor Driver not connected!")
io.stop()
sys.exit(-1)
driver.begin()
rospy.loginfo("Motor driver initialized.")
# Setup the input pins with a pull-up resistor. The encoders
# (Hall Effect chip) are open drain output, i.e. only pull down.
for pin in GPIO_PIN:
io.set_mode(pin, pigpio.INPUT)
io.set_pull_up_down(pin, pigpio.PUD_UP)
# Set up the encoder callback handlers.
cb_left = io.callback(GPIO_PIN[LEFT], pigpio.EITHER_EDGE, callback_left)
cb_right = io.callback(GPIO_PIN[RIGHT], pigpio.EITHER_EDGE, callback_right)
# Create a subscriber to listen to wheel commands.
sub = rospy.Subscriber("/wheel_command", JointState, callback_command)
# Create a publisher to send the wheel state.
pub = rospy.Publisher('/wheel_state', JointState, queue_size=10)
# Create the timer.
timer = rospy.Timer(rospy.Duration(DT), callback_timer)
# Finally enable the motor driver.
for channel in MOTOR_CHANNEL:
driver.set_drive(channel, 0, 0)
driver.enable()
rospy.loginfo("Motor driver enabled.")
# Spin while the callbacks are doing all the work.
rospy.loginfo("Running with dt = %.3f sec..." % DT)
rospy.spin()
rospy.loginfo("Stopping...")
# Stop the timer (if not already done).
timer.shutdown()
# Cleanup the motor driver.
for channel in MOTOR_CHANNEL:
driver.set_drive(channel, 0, 0)
driver.disable()
rospy.loginfo("Motor driver disabled.")
# Cleanup the GPIO (first disable callbacks and wait for anything pending).
cb_left.cancel()
cb_right.cancel()
time.sleep(0.25)
io.stop()
rospy.loginfo("GPIO cleaned up.")
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment