yemai/mmwave_ti_ros
1
0
Fork 0
mirror of https://git.ti.com/git/mmwave_radar/mmwave_ti_ros.git synced 2026-05-21 09:22:15 +00:00
Code Issues Packages Projects Releases Wiki Activity

melodic upgrade

Browse Source
This commit is contained in:
Sabeeh Khan
2021-03-22 19:10:54 -05:00
parent 2b64a5d70e
commit 9825b5e40c
577 changed files with 9973 additions and 12684 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
autonomous_robotics_ros/src/CMakeLists.txt
View File

@@ -1 +1 @@
/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake
/opt/ros/melodic/share/catkin/cmake/toplevel.cmake

36
autonomous_robotics_ros/src/turtlebot/turtlebot_simulator/turtlebot_gazebo/.cproject → autonomous_robotics_ros/src/kobuki_bumper2pc/.cproject Executable file → Normal file
View File

@@ -3,8 +3,8 @@
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="0.1155920974">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.1155920974" moduleId="org.eclipse.cdt.core.settings" name="Default">
<cconfiguration id="0.2081330952">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.2081330952" moduleId="org.eclipse.cdt.core.settings" name="Default">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
@@ -16,23 +16,20 @@
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration buildProperties="" description="" id="0.1155920974" name="Default" parent="org.eclipse.cdt.build.core.prefbase.cfg">
<folderInfo id="0.1155920974." name="/" resourcePath="">
<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.1370838257" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
<targetPlatform id="org.eclipse.cdt.build.core.prefbase.toolchain.1370838257.1947833174" name=""/>
<builder id="org.eclipse.cdt.build.core.settings.default.builder.986747662" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.libs.88371408" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.1052729546" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1456068118" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
<configuration buildProperties="" description="" id="0.2081330952" name="Default" parent="org.eclipse.cdt.build.core.prefbase.cfg">
<folderInfo id="0.2081330952." name="/" resourcePath="">
<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.1428329009" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
<targetPlatform id="org.eclipse.cdt.build.core.prefbase.toolchain.1428329009.1383860481" name=""/>
<builder id="org.eclipse.cdt.build.core.settings.default.builder.1947365243" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.libs.1759942927" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.1054142257" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1679260014" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
<tool id="org.eclipse.cdt.build.core.settings.holder.665107775" name="UPC" superClass="org.eclipse.cdt.build.core.settings.holder">
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.895901008" languageId="org.eclipse.cdt.core.parser.upc.upc" languageName="UPC" sourceContentType="org.eclipse.cdt.core.parser.upc.upcSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.331353793" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder">
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1277351141" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
<tool id="org.eclipse.cdt.build.core.settings.holder.1454360599" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder">
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.78997331" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
<tool id="org.eclipse.cdt.build.core.settings.holder.364380332" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder">
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.118197584" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.1212070909" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder">
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.736401172" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
</toolChain>
</folderInfo>
@@ -42,12 +39,13 @@
</cconfiguration>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="turtlebot_gazebo.null.2073026945" name="turtlebot_gazebo"/>
<project id="kobuki_bumper2pc.null.1800014363" name="kobuki_bumper2pc"/>
</storageModule>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
<scannerConfigBuildInfo instanceId="0.1155920974">
<scannerConfigBuildInfo instanceId="0.2081330952">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
</scannerConfigBuildInfo>
</storageModule>
<storageModule moduleId="refreshScope"/>
</cproject>

2
autonomous_robotics_ros/src/turtlebot/turtlebot_apps/turtlebot_apps/.project → autonomous_robotics_ros/src/kobuki_bumper2pc/.project Executable file → Normal file
View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>turtlebot_apps</name>
<name>kobuki_bumper2pc</name>
<comment></comment>
<projects>
</projects>

56
autonomous_robotics_ros/src/kobuki_bumper2pc/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,56 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package kobuki_bumper2pc
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.6.6 (2015-05-27)
------------------
0.6.5 (2014-11-21)
------------------
0.6.4 (2014-08-26)
------------------
0.6.3 (2014-08-25)
------------------
0.6.2 (2014-08-11)
------------------
0.6.1 (2014-08-08)
------------------
0.6.0 (2014-08-08)
------------------
* added comments to explain about the faraway points
* added side_point_angle param to change the angle of the bumper pointcloud's side points
* Add missing run dependency on yocs_cmd_vel_mux
* Contributors: Jorge Santos, Kaijen Hsiao
0.5.5 (2013-10-11)
------------------
0.5.4 (2013-09-06)
------------------
0.5.3 (2013-08-30)
------------------
0.5.0 (2013-08-29)
------------------
* kobuki : Added extra url info on all packages.
* Fix URL to the previous changelog wiki.
* Changelogs at package level.
0.4.0 (2013-08-09)
------------------
* Remove the dependency on pcl-ros. We compose sensor_msgs/PointCloud2 messages by hand, replacing pcl dependency by sensor_msgs one.
* Fixed Eigenlib alignment error on 32 bit architectures.
* Publish the pc continuously as long as bumper/cliff events are present.
* Publish stamped pointcloud.
Previous versions, bugfixing
============================
Available in ROS wiki: http://ros.org/wiki/kobuki/ChangeList

28
autonomous_robotics_ros/src/kobuki_bumper2pc/CMakeLists.txt Normal file
View File

@@ -0,0 +1,28 @@
cmake_minimum_required(VERSION 2.8.3)
project(kobuki_bumper2pc)
find_package(catkin REQUIRED COMPONENTS roscpp nodelet pluginlib sensor_msgs kobuki_msgs)
catkin_package(
INCLUDE_DIRS include
LIBRARIES kobuki_bumper2pc_nodelet
CATKIN_DEPENDS roscpp nodelet pluginlib sensor_msgs kobuki_msgs
)
include_directories(include ${catkin_INCLUDE_DIRS})
add_library(kobuki_bumper2pc_nodelet src/kobuki_bumper2pc.cpp)
add_dependencies(kobuki_bumper2pc_nodelet ${catkin_EXPORTED_TARGETS})
target_link_libraries(kobuki_bumper2pc_nodelet ${catkin_LIBRARIES})
install(TARGETS kobuki_bumper2pc_nodelet
DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(DIRECTORY plugins
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

107
autonomous_robotics_ros/src/kobuki_bumper2pc/include/kobuki_bumper2pc/kobuki_bumper2pc.hpp Normal file
View File

@@ -0,0 +1,107 @@
/*
* Copyright (c) 2013, Yujin Robot.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Yujin Robot nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file /include/kobuki_bumper2pc/kobuki_bumper2pc.hpp
*
* @brief Bumper/cliff to pointcloud nodelet class declaration.
*
* Publish bumpers and cliff sensors events as points in a pointcloud, so navistack can use them
* for poor-man navigation. Implemented as a nodelet intended to run together with kobuki_node.
*
* @author Jorge Santos, Yujin Robot
*
**/
#ifndef _KOBUKI_BUMPER2PC_HPP_
#define _KOBUKI_BUMPER2PC_HPP_
/*****************************************************************************
** Includes
*****************************************************************************/
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <sensor_msgs/PointCloud2.h>
#include <kobuki_msgs/SensorState.h>
/*****************************************************************************
** Namespace
*****************************************************************************/
namespace kobuki_bumper2pc
{
/**
* @brief Bumper2PcNodelet class declaration
*/
class Bumper2PcNodelet : public nodelet::Nodelet
{
public:
Bumper2PcNodelet()
: P_INF_X(+100*sin(0.34906585)),
P_INF_Y(+100*cos(0.34906585)),
N_INF_Y(-100*cos(0.34906585)),
ZERO(0), prev_bumper(0), prev_cliff(0) { }
~Bumper2PcNodelet() { }
void onInit();
private:
const float P_INF_X; // somewhere out of reach from the robot (positive x)
const float P_INF_Y; // somewhere out of reach from the robot (positive y)
const float N_INF_Y; // somewhere out of reach from the robot (negative y)
const float ZERO;
uint8_t prev_bumper;
uint8_t prev_cliff;
float pc_radius_;
float pc_height_;
float p_side_x_;
float p_side_y_;
float n_side_y_;
ros::Publisher pointcloud_pub_;
ros::Subscriber core_sensor_sub_;
sensor_msgs::PointCloud2 pointcloud_;
/**
* @brief Core sensors state structure callback
* @param msg incoming topic message
*/
void coreSensorCB(const kobuki_msgs::SensorState::ConstPtr& msg);
};
} // namespace kobuki_bumper2pc
#endif // _KOBUKI_BUMPER2PC_HPP_

18
autonomous_robotics_ros/src/kobuki_bumper2pc/launch/standalone.launch Normal file
View File

@@ -0,0 +1,18 @@
<!--
Example standalone launcher for the bumper/cliff to pointcloud nodelet.
Parameter pointcloud_radius gives the bumper/cliff pointcloud distance to base frame;
should be something like the robot radius plus plus costmap resolution plus an extra to
cope with robot inertia. This is a bit tricky parameter: if it's too low, costmap will
ignore this pointcloud (the robot footprint runs over the hit obstacle), but if it's too
big, hit obstacles will be mapped too far from the robot and the navigation around them
will probably fail.
-->
<launch>
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
<node pkg="nodelet" type="nodelet" name="bumper2pointcloud" args="load kobuki_bumper2pc/Bumper2PcNodelet nodelet_manager">
<param name="pointcloud_radius" value="0.25"/>
<remap from="bumper2pointcloud/pointcloud" to="mobile_base/sensors/bumper_pointcloud"/>
<remap from="bumper2pointcloud/core_sensors" to="mobile_base/sensors/core"/>
</node>
</launch>

33
autonomous_robotics_ros/src/kobuki_bumper2pc/package.xml Normal file
View File

@@ -0,0 +1,33 @@
<package>
<name>kobuki_bumper2pc</name>
<version>0.7.6</version>
<description>
Bumper/cliff to pointcloud nodelet:
Publish bumpers and cliff sensors events as points in a pointcloud, so navistack can use them
for poor-man navigation. Implemented as a nodelet intended to run together with kobuki_node.
</description>
<author email="jorge@yujinrobot.com">Jorge Santos Simon</author>
<maintainer email="jorge@yujinrobot.com">Jorge Santos Simon</maintainer>
<license>BSD</license>
<url type="bugtracker">https://github.com/yujinrobot/kobuki/issues</url>
<url type="repository">https://github.com/yujinrobot/kobuki</url>
<url type="website">http://ros.org/wiki/kobuki_bumper2pc</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>pluginlib</build_depend>
<build_depend>kobuki_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>nodelet</run_depend>
<run_depend>pluginlib</run_depend>
<run_depend>kobuki_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<export>
<nodelet plugin="${prefix}/plugins/nodelet_plugins.xml"/>
</export>
</package>

8
autonomous_robotics_ros/src/kobuki_bumper2pc/plugins/nodelet_plugins.xml Normal file
View File

@@ -0,0 +1,8 @@
<library path="lib/libkobuki_bumper2pc_nodelet">
<class name="kobuki_bumper2pc/Bumper2PcNodelet" type="kobuki_bumper2pc::Bumper2PcNodelet" base_class_type="nodelet::Nodelet">
<description>
Implementation for the bumper/cliff to pointcloud relay as a nodelet.
</description>
</class>
</library>

140
autonomous_robotics_ros/src/kobuki_bumper2pc/src/kobuki_bumper2pc.cpp Normal file
View File

@@ -0,0 +1,140 @@
/**
* @file /src/kobuki_bumper2pc.cpp
*
* @brief Bumper to pointcloud nodelet class implementation.
*
* @author Jorge Santos, Yujin Robot
*
**/
/*****************************************************************************
** Includes
*****************************************************************************/
#include <pluginlib/class_list_macros.h>
#include "kobuki_bumper2pc/kobuki_bumper2pc.hpp"
namespace kobuki_bumper2pc
{
void Bumper2PcNodelet::coreSensorCB(const kobuki_msgs::SensorState::ConstPtr& msg)
{
if (pointcloud_pub_.getNumSubscribers() == 0)
return;
// We publish just one "no events" pc (with all three points far away) and stop spamming when bumper/cliff conditions disappear
if (! msg->bumper && ! msg->cliff && ! prev_bumper && ! prev_cliff)
return;
prev_bumper = msg->bumper;
prev_cliff = msg->cliff;
// We replicate the sensors order of bumper/cliff event messages: LEFT = 0, CENTER = 1 and RIGHT = 2
// For any of {left/center/right} with no bumper/cliff event, we publish a faraway point that won't get used
if ((msg->bumper & kobuki_msgs::SensorState::BUMPER_LEFT) ||
(msg->cliff & kobuki_msgs::SensorState::CLIFF_LEFT))
{
memcpy(&pointcloud_.data[0 * pointcloud_.point_step + pointcloud_.fields[0].offset], &p_side_x_, sizeof(float));
memcpy(&pointcloud_.data[0 * pointcloud_.point_step + pointcloud_.fields[1].offset], &p_side_y_, sizeof(float));
}
else
{
memcpy(&pointcloud_.data[0 * pointcloud_.point_step + pointcloud_.fields[0].offset], &P_INF_X, sizeof(float));
memcpy(&pointcloud_.data[0 * pointcloud_.point_step + pointcloud_.fields[1].offset], &P_INF_Y, sizeof(float));
}
if ((msg->bumper & kobuki_msgs::SensorState::BUMPER_CENTRE) ||
(msg->cliff & kobuki_msgs::SensorState::CLIFF_CENTRE))
{
memcpy(&pointcloud_.data[1 * pointcloud_.point_step + pointcloud_.fields[0].offset], &pc_radius_, sizeof(float));
}
else
{
memcpy(&pointcloud_.data[1 * pointcloud_.point_step + pointcloud_.fields[0].offset], &P_INF_X, sizeof(float));
}
if ((msg->bumper & kobuki_msgs::SensorState::BUMPER_RIGHT) ||
(msg->cliff & kobuki_msgs::SensorState::CLIFF_RIGHT))
{
memcpy(&pointcloud_.data[2 * pointcloud_.point_step + pointcloud_.fields[0].offset], &p_side_x_, sizeof(float));
memcpy(&pointcloud_.data[2 * pointcloud_.point_step + pointcloud_.fields[1].offset], &n_side_y_, sizeof(float));
}
else
{
memcpy(&pointcloud_.data[2 * pointcloud_.point_step + pointcloud_.fields[0].offset], &P_INF_X, sizeof(float));
memcpy(&pointcloud_.data[2 * pointcloud_.point_step + pointcloud_.fields[1].offset], &N_INF_Y, sizeof(float));
}
pointcloud_.header.stamp = msg->header.stamp;
pointcloud_pub_.publish(pointcloud_);
}
void Bumper2PcNodelet::onInit()
{
ros::NodeHandle nh = this->getPrivateNodeHandle();
// Bumper/cliff pointcloud distance to base frame; should be something like the robot radius plus
// costmap resolution plus an extra to cope with robot inertia. This is a bit tricky parameter: if
// it's too low, costmap will ignore this pointcloud (the robot footprint runs over the hit obstacle),
// but if it's too big, hit obstacles will be mapped too far from the robot and the navigation around
// them will probably fail.
std::string base_link_frame;
double r, h, angle;
nh.param("pointcloud_radius", r, 0.25); pc_radius_ = r;
nh.param("pointcloud_height", h, 0.04); pc_height_ = h;
nh.param("side_point_angle", angle, 0.34906585);
nh.param<std::string>("base_link_frame", base_link_frame, "base_link");
// Lateral points x/y coordinates; we need to store float values to memcopy later
p_side_x_ = + pc_radius_*sin(angle); // angle degrees from vertical
p_side_y_ = + pc_radius_*cos(angle); // angle degrees from vertical
n_side_y_ = - pc_radius_*cos(angle); // angle degrees from vertical
// Prepare constant parts of the pointcloud message to be published
pointcloud_.header.frame_id = base_link_frame;
pointcloud_.width = 3;
pointcloud_.height = 1;
pointcloud_.fields.resize(3);
// Set x/y/z as the only fields
pointcloud_.fields[0].name = "x";
pointcloud_.fields[1].name = "y";
pointcloud_.fields[2].name = "z";
int offset = 0;
// All offsets are *4, as all field data types are float32
for (size_t d = 0; d < pointcloud_.fields.size(); ++d, offset += 4)
{
pointcloud_.fields[d].count = 1;
pointcloud_.fields[d].offset = offset;
pointcloud_.fields[d].datatype = sensor_msgs::PointField::FLOAT32;
}
pointcloud_.point_step = offset;
pointcloud_.row_step = pointcloud_.point_step * pointcloud_.width;
pointcloud_.data.resize(3 * pointcloud_.point_step);
pointcloud_.is_bigendian = false;
pointcloud_.is_dense = true;
// Bumper/cliff "points" fix coordinates (the others depend on sensor activation/deactivation)
// y: always 0 for central bumper
memcpy(&pointcloud_.data[1 * pointcloud_.point_step + pointcloud_.fields[1].offset], &ZERO, sizeof(float));
// z: constant elevation from base frame
memcpy(&pointcloud_.data[0 * pointcloud_.point_step + pointcloud_.fields[2].offset], &pc_height_, sizeof(float));
memcpy(&pointcloud_.data[1 * pointcloud_.point_step + pointcloud_.fields[2].offset], &pc_height_, sizeof(float));
memcpy(&pointcloud_.data[2 * pointcloud_.point_step + pointcloud_.fields[2].offset], &pc_height_, sizeof(float));
pointcloud_pub_ = nh.advertise <sensor_msgs::PointCloud2> ("pointcloud", 10);
core_sensor_sub_ = nh.subscribe("core_sensors", 10, &Bumper2PcNodelet::coreSensorCB, this);
ROS_INFO("Bumper/cliff pointcloud configured at distance %f and height %f from base frame", pc_radius_, pc_height_);
}
} // namespace kobuki_bumper2pc
PLUGINLIB_EXPORT_CLASS(kobuki_bumper2pc::Bumper2PcNodelet, nodelet::Nodelet);

28
autonomous_robotics_ros/src/turtlebot/turtlebot_msgs/.cproject → autonomous_robotics_ros/src/kobuki_description/.cproject Executable file → Normal file
View File

@@ -3,8 +3,8 @@
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.base.1913250642">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.1913250642" moduleId="org.eclipse.cdt.core.settings" name="Default">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.base.1267367692">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.1267367692" moduleId="org.eclipse.cdt.core.settings" name="Default">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
@@ -16,17 +16,17 @@
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration buildProperties="" id="cdt.managedbuild.toolchain.gnu.base.1913250642" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
<folderInfo id="cdt.managedbuild.toolchain.gnu.base.1913250642.277313680" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.base.1573811255" name="cdt.managedbuild.toolchain.gnu.base" superClass="cdt.managedbuild.toolchain.gnu.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.1325303568" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
<builder id="cdt.managedbuild.target.gnu.builder.base.230040716" managedBuildOn="false" name="Gnu Make Builder.Default" superClass="cdt.managedbuild.target.gnu.builder.base"/>
<tool id="cdt.managedbuild.tool.gnu.archiver.base.679557899" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.730706182" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base"/>
<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.1271818185" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base"/>
<tool id="cdt.managedbuild.tool.gnu.c.linker.base.888348919" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.1424808496" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base"/>
<tool id="cdt.managedbuild.tool.gnu.assembler.base.2047397035" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base"/>
<configuration buildProperties="" id="cdt.managedbuild.toolchain.gnu.base.1267367692" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
<folderInfo id="cdt.managedbuild.toolchain.gnu.base.1267367692.979416689" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.base.1674511489" name="cdt.managedbuild.toolchain.gnu.base" superClass="cdt.managedbuild.toolchain.gnu.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.633990643" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
<builder id="cdt.managedbuild.target.gnu.builder.base.215832160" managedBuildOn="false" name="Gnu Make Builder.Default" superClass="cdt.managedbuild.target.gnu.builder.base"/>
<tool id="cdt.managedbuild.tool.gnu.archiver.base.1103898243" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.997765247" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base"/>
<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.1840442111" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base"/>
<tool id="cdt.managedbuild.tool.gnu.c.linker.base.1871214662" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.1231177396" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base"/>
<tool id="cdt.managedbuild.tool.gnu.assembler.base.1372070793" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base"/>
</toolChain>
</folderInfo>
</configuration>
@@ -35,7 +35,7 @@
</cconfiguration>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="turtlebot_msgs.null.422679330" name="turtlebot_msgs"/>
<project id="kobuki_description.null.340833361" name="kobuki_description"/>
</storageModule>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>

2
autonomous_robotics_ros/src/turtlebot/turtlebot_apps/turtlebot_follower/.project → autonomous_robotics_ros/src/kobuki_description/.project Executable file → Normal file
View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>turtlebot_follower</name>
<name>kobuki_description</name>
<comment></comment>
<projects>
</projects>

49
autonomous_robotics_ros/src/kobuki_description/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,49 @@
=========
Changelog
=========
0.7.4 (2017-04-01)
------------------
* redirect gazebo bumper contact sensor for the new gazebo
0.6.0 (2014-08-08)
------------------
* update body friction and revert torque limit
* update kobuki_gazebo.urdf.xacro to make gazebo simulation more stable.
* Add missing run dependency on yocs_cmd_vel_mux
* Contributors: John Hsu, Jorge Santos
0.5.5 (2013-10-11)
------------------
0.5.4 (2013-09-06)
------------------
0.5.3 (2013-08-30)
------------------
* disables vertical rays for the cliff sensors.
* slightly increases collision model for the base.
0.5.0 (2013-08-29)
------------------
* fixes collision object name for gazebo contact sensor.
* changes center cliff sensor name.
* changes simulated IMU.
* Added extra url info on all packages.
* Updated old rnd email address.
* Fix URL to the previous changelog wiki.
* Changelogs at package level.
* Do not use robot_pose_ekf. Instead, use imu for heading and encoders por x and y. However, parameter use_imu_heading makes trivial to switch back to the previous system.
* corrects inertia and center of mass using an approximation.
0.4.0 (2013-08-09)
------------------
* Many Gazebo 1.9+ related fixes.
* Update urdf and mesh files.
* We can recuperate the catkin version of this package because xacro has been (finally) catkinized.
Previous versions, bugfixing
============================
Available in ROS wiki: http://ros.org/wiki/kobuki/ChangeList

20
autonomous_robotics_ros/src/kobuki_description/CMakeLists.txt Normal file
View File

@@ -0,0 +1,20 @@
cmake_minimum_required(VERSION 2.8.3)
project(kobuki_description)
find_package(catkin REQUIRED COMPONENTS urdf xacro)
catkin_package(
CATKIN_DEPENDS urdf xacro
)
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY meshes
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY rviz
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

16
autonomous_robotics_ros/src/kobuki_description/launch/view_model.launch Normal file
View File

@@ -0,0 +1,16 @@
<!--
Useful for debugging or just viewing the kobuki model/urdf/transform.
-->
<launch>
<arg name="urdf_file" default="$(find xacro)/xacro '$(find kobuki_description)/urdf/kobuki_standalone.urdf.xacro'"/>
<param name="robot_description" command="$(arg urdf_file)"/>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="5.0"/>
</node>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="use_gui" value="true"/>
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find kobuki_description)/rviz/model.rviz"/>
</launch>

BIN
autonomous_robotics_ros/src/kobuki_description/meshes/images/main_body.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 559 KiB

BIN
autonomous_robotics_ros/src/kobuki_description/meshes/images/wheel.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 92 KiB

164
autonomous_robotics_ros/src/kobuki_description/meshes/main_body.dae Normal file
View File

File diff suppressed because one or more lines are too long

164
autonomous_robotics_ros/src/kobuki_description/meshes/wheel.dae Normal file
View File

File diff suppressed because one or more lines are too long

29
autonomous_robotics_ros/src/kobuki_description/package.xml Normal file
View File

@@ -0,0 +1,29 @@
<package>
<name>kobuki_description</name>
<version>0.7.6</version>
<description>
Description of the Kobuki model.
Provides the model description of Kobuki for simulation and visualisation. The files in this
package are parsed and used by a variety of other components. Most users will not interact directly
with this package.
WARNING: This package is disabled because it cannot be catkinized by now, as xacro dependency is not
catkin still. In the interim we use a unary pre-catkin stack named kobuki_description.
</description>
<author email="stonier@yujinrobot.com">Daniel Stonier</author>
<author email="marcus.liebhardt@yujinrobot.com">Marcus Liebhardt</author>
<author email="yhju@yujinrobot.com">Younghun Ju</author>
<maintainer email="yhju@yujinrobot.com">Younghun Ju</maintainer>
<license>BSD</license>
<url type="bugtracker">https://github.com/yujinrobot/kobuki/issues</url>
<url type="repository">https://github.com/yujinrobot/kobuki</url>
<url type="website">http://ros.org/wiki/kobuki_description</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>urdf</build_depend>
<build_depend>xacro</build_depend>
<run_depend>urdf</run_depend>
<run_depend>xacro</run_depend>
</package>

187
autonomous_robotics_ros/src/turtlebot/turtlebot_interactions/turtlebot_rviz_launchers/rviz/model.rviz → autonomous_robotics_ros/src/kobuki_description/rviz/model.rviz Executable file → Normal file
View File

@@ -5,11 +5,11 @@ Panels:
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Grid1
- /RobotModel1
- /TF1
- /TF1/Frames1
Splitter Ratio: 0.5
Tree Height: 673
Tree Height: 627
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -39,7 +39,6 @@ Visualization Manager:
Name: Grid
Normal Cell Count: 0
Offset:
Value: 0; 0; 0
X: 0
Y: 0
Z: 0
@@ -62,116 +61,6 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
camera_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
caster_back_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
caster_front_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
plate_bottom_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
plate_middle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
plate_top_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pole_bottom_0_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pole_bottom_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pole_bottom_2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pole_bottom_3_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pole_bottom_4_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pole_bottom_5_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pole_kinect_0_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pole_kinect_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pole_middle_0_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pole_middle_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pole_middle_2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pole_middle_3_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pole_top_0_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pole_top_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pole_top_2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pole_top_3_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheel_left_link:
Alpha: 1
Show Axes: false
@@ -188,6 +77,49 @@ Visualization Manager:
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
/base_footprint:
Value: true
/base_link:
Value: true
/cliff_sensor_front_link:
Value: true
/cliff_sensor_left_link:
Value: true
/cliff_sensor_right_link:
Value: true
/gyro_link:
Value: true
/wheel_left_link:
Value: true
/wheel_right_link:
Value: true
All Enabled: true
Marker Scale: 1
Name: TF
Show Arrows: false
Show Axes: false
Show Names: false
Tree:
/base_footprint:
/base_link:
/cliff_sensor_front_link:
{}
/cliff_sensor_left_link:
{}
/cliff_sensor_right_link:
{}
/gyro_link:
{}
/wheel_left_link:
{}
/wheel_right_link:
{}
Update Interval: 0
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -205,24 +137,33 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 1.18557
Distance: 1.44857
Focal Point:
Value: 0; 0; 0
X: 0
Y: 0
Z: 0
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.490399
Pitch: 0.435398
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 5.71357
Yaw: 1.5654
Saved: ~
Window Geometry:
Height: 916
Displays:
collapsed: false
Height: 880
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000018c0000032bfc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006901000005fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000320000032b000000d701000005000000010000010b0000032bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000320000032b000000a901000005fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000001eb01000005fb0000000800540069006d00650100000000000004500000000000000000000002f30000032b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Width: 1428
X: 56
Y: -31
QMainWindow State: 000000ff00000000fd00000004000000000000013c000002fefc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006901000005fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000033000002fe000000d801000005000000010000010b000002fefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000033000002fe000000ab01000005fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000023a01000005fb0000000800540069006d0065010000000000000450000000000000000000000263000002fe00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1210
X: 51
Y: -60

0
autonomous_robotics_ros/src/turtlebot/turtlebot/turtlebot_description/urdf/common_properties.urdf.xacro → autonomous_robotics_ros/src/kobuki_description/urdf/common_properties.urdf.xacro Executable file → Normal file
View File

227
autonomous_robotics_ros/src/kobuki_description/urdf/kobuki.urdf.xacro Normal file
View File

@@ -0,0 +1,227 @@
<?xml version="1.0" ?>
<!--
This is not a standalone urdf for kobuki. It simply defines the 'kobuki' tag that can
be incorporated by other urdf files (e.g. turtlebot).
See kobuki_standalone.urdf.xacro for a standalone urdf to be tested with
kobuki_description/launch/view_model.launch
-->
<robot name="kobuki" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:include filename="$(find kobuki_description)/urdf/common_properties.urdf.xacro"/>
<xacro:include filename="$(find kobuki_description)/urdf/kobuki_gazebo.urdf.xacro"/>
<!-- Kobuki -->
<xacro:macro name="kobuki">
<link name="base_footprint"/>
<!--
Base link is set at the bottom of the base mould.
This is done to be compatible with the way base link
was configured for turtlebot 1. Refer to
https://github.com/turtlebot/turtlebot/issues/40
To put the base link at the more oft used wheel
axis, set the z-distance from the base_footprint
to 0.352.
-->
<joint name="base_joint" type="fixed">
<origin xyz="0 0 0.0102" rpy="0 0 0" />
<parent link="base_footprint"/>
<child link="base_link" />
</joint>
<link name="base_link">
<visual>
<geometry>
<!-- new mesh -->
<mesh filename="package://kobuki_description/meshes/main_body.dae" />
</geometry>
<origin xyz="0.001 0 0.05199" rpy="0 0 0"/>
</visual>
<collision name="base">
<geometry>
<cylinder length="0.10938" radius="0.178"/>
</geometry>
<origin xyz="0.0 0 0.05949" rpy="0 0 0"/>
</collision>
<inertial>
<!-- COM experimentally determined -->
<origin xyz="0.01 0 0"/>
<mass value="2.4"/> <!-- 2.4/2.6 kg for small/big battery pack -->
<!-- Kobuki's inertia tensor is approximated by a cylinder with homogeneous mass distribution
More details: http://en.wikipedia.org/wiki/List_of_moment_of_inertia_tensors
m = 2.4 kg; h = 0.09 m; r = 0.175 m
ixx = 1/12 * m * (3 * r^2 + h^2)
iyy = 1/12 * m * (3 * r^2 + h^2)
izz = 1/2 * m * r^2
-->
<inertia ixx="0.019995" ixy="0.0" ixz="0.0"
iyy="0.019995" iyz="0.0"
izz="0.03675" />
</inertial>
</link>
<joint name="wheel_left_joint" type="continuous">
<parent link="base_link"/>
<child link="wheel_left_link"/>
<origin xyz="0.00 ${0.23/2} 0.0250" rpy="${-M_PI/2} 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<link name="wheel_left_link">
<visual>
<geometry>
<mesh filename="package://kobuki_description/meshes/wheel.dae"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 0"/>
</visual>
<collision>
<geometry>
<cylinder length="0.0206" radius="0.035"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
</link>
<joint name="wheel_right_joint" type="continuous">
<parent link="base_link"/>
<child link="wheel_right_link"/>
<origin xyz="0.00 -${0.23/2} 0.0250" rpy="${-M_PI/2} 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<link name="wheel_right_link">
<visual>
<geometry>
<mesh filename="package://kobuki_description/meshes/wheel.dae"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 0"/>
</visual>
<collision>
<geometry>
<cylinder length="0.0206" radius="0.035"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
</link>
<joint name="caster_front_joint" type="fixed">
<parent link="base_link"/>
<child link="caster_front_link"/>
<origin xyz="0.115 0.0 0.007" rpy="${-M_PI/2} 0 0"/>
</joint>
<link name="caster_front_link">
<collision>
<geometry>
<cylinder length="0.0176" radius="0.017"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
</link>
<joint name="caster_back_joint" type="fixed">
<parent link="base_link"/>
<child link="caster_back_link"/>
<origin xyz="-0.135 0.0 0.009" rpy="${-M_PI/2} 0 0"/>
</joint>
<link name="caster_back_link">
<collision>
<geometry>
<cylinder length="0.0176" radius="0.017"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
</link>
<!-- Kobuki's sensors -->
<joint name="gyro_joint" type="fixed">
<axis xyz="0 1 0"/>
<origin xyz="0.056 0.062 0.0202" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="gyro_link"/>
</joint>
<link name="gyro_link">
<inertial>
<mass value="0.001"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0"
iyy="0.000001" iyz="0"
izz="0.0001"/>
</inertial>
</link>
<joint name="cliff_sensor_left_joint" type="fixed">
<origin xyz="0.08734 0.13601 0.0214" rpy="0 ${M_PI/2} 0" />
<parent link="base_link"/>
<child link="cliff_sensor_left_link" />
</joint>
<link name="cliff_sensor_left_link">
<inertial>
<mass value="0.0001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>
</link>
<joint name="cliff_sensor_right_joint" type="fixed">
<origin xyz="0.085 -0.13601 0.0214" rpy="0 ${M_PI/2} 0" />
<parent link="base_link"/>
<child link="cliff_sensor_right_link" />
</joint>
<link name="cliff_sensor_right_link">
<inertial>
<mass value="0.0001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>
</link>
<joint name="cliff_sensor_front_joint" type="fixed">
<origin xyz="0.156 0.00 0.0214" rpy="0 ${M_PI/2} 0" />
<parent link="base_link"/>
<child link="cliff_sensor_front_link" />
</joint>
<link name="cliff_sensor_front_link">
<inertial>
<mass value="0.0001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>
</link>
<!-- Kobuki Gazebo simulation details -->
<xacro:kobuki_sim/>
</xacro:macro>
</robot>

190
autonomous_robotics_ros/src/kobuki_description/urdf/kobuki_gazebo.urdf.xacro Normal file
View File

@@ -0,0 +1,190 @@
<?xml version="1.0"?>
<robot name="kobuki_sim" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="kobuki_sim">
<gazebo reference="wheel_left_link">
<mu1>1.0</mu1>
<mu2>1.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
</gazebo>
<gazebo reference="wheel_right_link">
<mu1>1.0</mu1>
<mu2>1.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
</gazebo>
<gazebo reference="caster_front_link">
<mu1>0.0</mu1>
<mu2>0.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
</gazebo>
<gazebo reference="caster_back_link">
<mu1>0.0</mu1>
<mu2>0.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
</gazebo>
<gazebo reference="base_link">
<mu1>0.3</mu1>
<mu2>0.3</mu2>
<sensor type="contact" name="bumpers">
<always_on>1</always_on>
<update_rate>50.0</update_rate>
<visualize>true</visualize>
<contact>
<collision>base_footprint_fixed_joint_lump__base_collision</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="cliff_sensor_left_link">
<sensor type="ray" name="cliff_sensor_left">
<always_on>true</always_on>
<update_rate>50</update_rate>
<visualize>true</visualize>
<ray>
<scan>
<horizontal>
<samples>50</samples>
<resolution>1.0</resolution>
<min_angle>-0.0436</min_angle> <!-- -2.5 degree -->
<max_angle>0.0436</max_angle> <!-- 2.5 degree -->
</horizontal>
<!-- Can't use vertical rays until this bug is resolved: -->
<!-- https://bitbucket.org/osrf/gazebo/issue/509/vertical-sensor-doesnt-works -->
<!-- <vertical> -->
<!-- <samples>50</samples> -->
<!-- <resolution>1.0</resolution> -->
<!-- <min_angle>-0.0436</min_angle> -2.5 degree -->
<!-- <max_angle>0.0436</max_angle> 2.5 degree -->
<!-- </vertical> -->
</scan>
<range>
<min>0.01</min>
<max>0.15</max>
<resolution>1.0</resolution>
</range>
</ray>
</sensor>
</gazebo>
<gazebo reference="cliff_sensor_right_link">
<sensor type="ray" name="cliff_sensor_right">
<always_on>true</always_on>
<update_rate>50</update_rate>
<visualize>true</visualize>
<ray>
<scan>
<horizontal>
<samples>50</samples>
<resolution>1.0</resolution>
<min_angle>-0.0436</min_angle> <!-- -2.5 degree -->
<max_angle>0.0436</max_angle> <!-- 2.5 degree -->
</horizontal>
<!-- Can't use vertical rays until this bug is resolved: -->
<!-- https://bitbucket.org/osrf/gazebo/issue/509/vertical-sensor-doesnt-works -->
<!-- <vertical> -->
<!-- <samples>50</samples> -->
<!-- <resolution>1.0</resolution> -->
<!-- <min_angle>-0.0436</min_angle> -2.5 degree -->
<!-- <max_angle>0.0436</max_angle> 2.5 degree -->
<!-- </vertical> -->
</scan>
<range>
<min>0.01</min>
<max>0.15</max>
<resolution>1.0</resolution>
</range>
</ray>
</sensor>
</gazebo>
<gazebo reference="cliff_sensor_front_link">
<sensor type="ray" name="cliff_sensor_front">
<always_on>true</always_on>
<update_rate>50</update_rate>
<visualize>true</visualize>
<ray>
<scan>
<horizontal>
<samples>50</samples>
<resolution>1.0</resolution>
<min_angle>-0.0436</min_angle> <!-- -2.5 degree -->
<max_angle>0.0436</max_angle> <!-- 2.5 degree -->
</horizontal>
<!-- Can't use vertical rays until this bug is resolved: -->
<!-- https://bitbucket.org/osrf/gazebo/issue/509/vertical-sensor-doesnt-works -->
<!-- <vertical> -->
<!-- <samples>50</samples> -->
<!-- <resolution>1.0</resolution> -->
<!-- <min_angle>-0.0436</min_angle> -2.5 degree -->
<!-- <max_angle>0.0436</max_angle> 2.5 degree -->
<!-- </vertical> -->
</scan>
<range>
<min>0.01</min>
<max>0.15</max>
<resolution>1.0</resolution>
</range>
</ray>
</sensor>
</gazebo>
<gazebo reference="gyro_link">
<sensor type="imu" name="imu">
<always_on>true</always_on>
<update_rate>50</update_rate>
<visualize>false</visualize>
<imu>
<noise>
<type>gaussian</type>
<rate>
<mean>0.0</mean>
<stddev>${0.0014*0.0014}</stddev> <!-- 0.25 x 0.25 (deg/s) -->
<bias_mean>0.0</bias_mean>
<bias_stddev>0.0</bias_stddev>
</rate>
<accel> <!-- not used in the plugin and real robot, hence using tutorial values -->
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</accel>
</noise>
</imu>
</sensor>
</gazebo>
<gazebo>
<plugin name="kobuki_controller" filename="libgazebo_ros_kobuki.so">
<publish_tf>1</publish_tf>
<left_wheel_joint_name>wheel_left_joint</left_wheel_joint_name>
<right_wheel_joint_name>wheel_right_joint</right_wheel_joint_name>
<wheel_separation>.230</wheel_separation>
<wheel_diameter>0.070</wheel_diameter>
<torque>1.0</torque>
<velocity_command_timeout>0.6</velocity_command_timeout>
<cliff_sensor_left_name>cliff_sensor_left</cliff_sensor_left_name>
<cliff_sensor_center_name>cliff_sensor_front</cliff_sensor_center_name>
<cliff_sensor_right_name>cliff_sensor_right</cliff_sensor_right_name>
<cliff_detection_threshold>0.04</cliff_detection_threshold>
<bumper_name>bumpers</bumper_name>
<imu_name>imu</imu_name>
</plugin>
</gazebo>
</xacro:macro>
</robot>

11
autonomous_robotics_ros/src/kobuki_description/urdf/kobuki_standalone.urdf.xacro Normal file
View File

@@ -0,0 +1,11 @@
<?xml version="1.0"?>
<robot name="kobuki_standalone"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Defines the kobuki component tag. -->
<xacro:include filename="$(find kobuki_description)/urdf/kobuki.urdf.xacro" />
<xacro:kobuki/>
</robot>

68
autonomous_robotics_ros/src/kobuki_gazebo_plugins/.cproject Normal file
View File

@@ -0,0 +1,68 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.base.1622095619">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.1622095619" moduleId="org.eclipse.cdt.core.settings" name="Default">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.base.1622095619" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
<folderInfo id="cdt.managedbuild.toolchain.gnu.base.1622095619.1628255752" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.base.8860267" name="cdt.managedbuild.toolchain.gnu.base" superClass="cdt.managedbuild.toolchain.gnu.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.2104812079" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
<builder arguments="VERBOSE=1 -j8 -C ${CKX_WORKSPACE}/build/${ProjName}" buildPath="${workspace_loc:/kobuki_gazebo_plugins}" command="make" id="cdt.managedbuild.target.gnu.builder.base.1135077290" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="cdt.managedbuild.target.gnu.builder.base"/>
<tool id="cdt.managedbuild.tool.gnu.archiver.base.1170120805" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.28810386" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base">
<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.2116270962" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
</tool>
<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.1736919230" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base">
<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.101083703" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
</tool>
<tool id="cdt.managedbuild.tool.gnu.c.linker.base.761930581" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.756436350" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base">
<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.796696924" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
<additionalInput kind="additionalinput" paths="$(LIBS)"/>
</inputType>
</tool>
<tool id="cdt.managedbuild.tool.gnu.assembler.base.241725084" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base">
<inputType id="cdt.managedbuild.tool.gnu.assembler.input.1997225673" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
</tool>
</toolChain>
</folderInfo>
<sourceEntries>
<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="include/kobuki_gazebo_plugins"/>
<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="src"/>
</sourceEntries>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
</cconfiguration>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="kobuki_gazebo_plugins.null.1149984813" name="kobuki_gazebo_plugins"/>
</storageModule>
<storageModule moduleId="refreshScope" versionNumber="2">
<configuration configurationName="Default">
<resource resourceType="PROJECT" workspacePath="/kobuki_gazebo_plugins"/>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.1622095619;cdt.managedbuild.toolchain.gnu.base.1622095619.1628255752;cdt.managedbuild.tool.gnu.c.compiler.base.1736919230;cdt.managedbuild.tool.gnu.c.compiler.input.101083703">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
</scannerConfigBuildInfo>
<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.1622095619;cdt.managedbuild.toolchain.gnu.base.1622095619.1628255752;cdt.managedbuild.tool.gnu.cpp.compiler.base.28810386;cdt.managedbuild.tool.gnu.cpp.compiler.input.2116270962">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
</scannerConfigBuildInfo>
</storageModule>
</cproject>

2
autonomous_robotics_ros/src/turtlebot/turtlebot_interactions/turtlebot_dashboard/.project → autonomous_robotics_ros/src/kobuki_gazebo_plugins/.project Executable file → Normal file
View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>turtlebot_dashboard</name>
<name>kobuki_gazebo_plugins</name>
<comment></comment>
<projects>
</projects>

56
autonomous_robotics_ros/src/kobuki_gazebo_plugins/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,56 @@
=========
Changelog
=========
0.5.6 (2017-04-01)
------------------
* update at the end of the event loop so the bumper sensor can be caught
0.5.3 (2017-02-23)
------------------
* add_dependencies with catkin_EXPORTED_TARGETS, not xyz_gencpp targets which might not be there
0.5.1 (2016-09-20)
------------------
* bugfix for ros logging.
0.4.2 (2015-03-02)
------------------
* kobuki_gazebo_plugins: Resolve IMU sensor using fully scoped name
This ensures uniqueness among multiple models in which the sensor has the
same name. In this case, our specific motivation is the Kobuki Gazebo
plugin being spawned multiple times using kobuki_gazebo.urdf.xacro in the
package kobuki_description.
* Contributors: Scott Livingston
0.4.1 (2014-09-19)
------------------
* kobuki_gazebo_plugins: makes bump detection more reliable
* remove duplicated spinonce
* renamed updater to updates and loader to loads. reorder headers
* tf_prefix added. base_prefix added
* Contributors: Jihoon Lee, Marcus Liebhardt
0.4.0 (2014-08-11)
------------------
* cherry-picking `#30 <https://github.com/yujinrobot/kobuki_desktop/issues/30>`_
* trivial update.
* removes email addresses from authors
* replace deprecated shared_dynamic_cast (fixes `#25 <https://github.com/yujinrobot/kobuki_desktop/issues/25>`_)
* Contributors: Daniel Stonier, Marcus Liebhardt, Nikolaus Demmel, Samir Benmendil
0.3.1 (2013-10-14)
------------------
* fixes gazebo header paths (refs `#22 <https://github.com/yujinrobot/kobuki_desktop/issues/22>`_)
0.3.0 (2013-08-30)
------------------
* fixes bumper & cliff event publishing
* adds reset odometry and fixes imu and odom data processing
* adds IMU data processing
* adds bugtracker and repo info to package.xml
0.2.0 (2013-07-11)
------------------
* ROS Hydro beta release.
* Adds catkinized kobuki_qtestsuite

45
autonomous_robotics_ros/src/kobuki_gazebo_plugins/CMakeLists.txt Normal file
View File

@@ -0,0 +1,45 @@
cmake_minimum_required(VERSION 2.8.3)
project(kobuki_gazebo_plugins)
find_package(gazebo REQUIRED)
find_package(catkin REQUIRED COMPONENTS gazebo_ros
gazebo_plugins
geometry_msgs
kobuki_msgs
nav_msgs
roscpp
sensor_msgs
std_msgs
tf)
catkin_package(INCLUDE_DIRS include
LIBRARIES gazebo_ros_kobuki
CATKIN_DEPENDS gazebo_ros
gazebo_plugins
geometry_msgs
kobuki_msgs
nav_msgs
roscpp
sensor_msgs
std_msgs
tf)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
link_directories(${GAZEBO_LIBRARY_DIRS})
include_directories(include
${catkin_INCLUDE_DIRS}
${GAZEBO_INCLUDE_DIRS})
add_library(gazebo_ros_kobuki src/gazebo_ros_kobuki.cpp
src/gazebo_ros_kobuki_updates.cpp
src/gazebo_ros_kobuki_loads.cpp)
add_dependencies(gazebo_ros_kobuki ${catkin_EXPORTED_TARGETS})
target_link_libraries(gazebo_ros_kobuki
${catkin_LIBRARIES}
${GAZEBO_LIBRARIES})
install(TARGETS gazebo_ros_kobuki
DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})

259
autonomous_robotics_ros/src/kobuki_gazebo_plugins/include/kobuki_gazebo_plugins/gazebo_ros_kobuki.h Normal file
View File

@@ -0,0 +1,259 @@
/*
* Copyright (c) 2013, Yujin Robot.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Yujin Robot nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @author Marcus Liebhardt
*
* This work has been inspired by Nate Koenig's Gazebo plugin for the iRobot Create.
*/
#ifndef GAZEBO_ROS_KOBUKI_H
#define GAZEBO_ROS_KOBUKI_H
#include <cmath>
#include <cstring>
#include <string>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <boost/shared_ptr.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/common/common.hh>
#include <gazebo/common/Time.hh>
#if GAZEBO_MAJOR_VERSION >= 9
// #include <ignition/math.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Quaternion.hh>
#else
#include <gazebo/math/gzmath.hh>
#endif
#include <gazebo/physics/physics.hh>
#include <gazebo/sensors/sensors.hh>
#include <gazebo_plugins/gazebo_ros_utils.h>
#include <ros/ros.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Float64.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/LinearMath/Quaternion.h>
#include <kobuki_msgs/MotorPower.h>
#include <kobuki_msgs/CliffEvent.h>
#include <kobuki_msgs/BumperEvent.h>
namespace gazebo
{
enum {LEFT= 0, RIGHT=1};
class GazeboRosKobuki : public ModelPlugin
{
public:
/// Constructor
GazeboRosKobuki();
/// Destructor
~GazeboRosKobuki();
/// Called when plugin is loaded
void Load(physics::ModelPtr parent, sdf::ElementPtr sdf);
/// Called by the world update start event
void OnUpdate();
private:
/*
* Methods
*/
/// Callback for incoming velocity commands
void cmdVelCB(const geometry_msgs::TwistConstPtr &msg);
/// Callback for incoming velocity commands
void motorPowerCB(const kobuki_msgs::MotorPowerPtr &msg);
/// Callback for resetting the odometry data
void resetOdomCB(const std_msgs::EmptyConstPtr &msg);
/// Spin method for the spinner thread
void spin();
// void OnContact(const std::string &name, const physics::Contact &contact); necessary?
// internal functions for load
void prepareMotorPower();
bool prepareJointState();
void preparePublishTf();
bool prepareWheelAndTorque();
void prepareOdom();
bool prepareVelocityCommand();
bool prepareCliffSensor();
bool prepareBumper();
bool prepareIMU();
void setupRosApi(std::string& model_name);
// internal functions for update
void updateJointState();
void updateOdometry(common::Time& step_time);
void updateIMU();
void propagateVelocityCommands();
void updateCliffSensor();
void updateBumper();
/*
* Parameters
*/
/// ROS node handles (relative & private)
ros::NodeHandle nh_, nh_priv_;
/// node name
std::string node_name_;
/// TF Prefix
std::string tf_prefix_;
/// extra thread for triggering ROS callbacks
// boost::shared_ptr<boost::thread> ros_spinner_thread_; necessary?
/// flag for shutting down the spinner thread
bool shutdown_requested_;
/// pointer to the model
physics::ModelPtr model_;
/// pointer to the gazebo ros node
GazeboRosPtr gazebo_ros_;
sdf::ElementPtr sdf_;
/// pointer to simulated world
physics::WorldPtr world_;
/// pointer to the update event connection (triggers the OnUpdate callback when event update event is received)
event::ConnectionPtr update_connection_;
/// rate control
double update_rate_;
/// Simulation time on previous update
common::Time prev_update_time_;
/// ROS subscriber for motor power commands
ros::Subscriber motor_power_sub_;
/// Flag indicating if the motors are turned on or not
bool motors_enabled_;
/// Pointers to Gazebo's joints
physics::JointPtr joints_[2];
/// Left wheel's joint name
std::string left_wheel_joint_name_;
/// Right wheel's joint name
std::string right_wheel_joint_name_;
/// ROS publisher for joint state messages
ros::Publisher joint_state_pub_;
/// ROS message for joint sates
sensor_msgs::JointState joint_state_;
/// ROS subscriber for velocity commands
ros::Subscriber cmd_vel_sub_;
/// Simulation time of the last velocity command (used for time out)
common::Time last_cmd_vel_time_;
/// Time out for velocity commands in seconds
double cmd_vel_timeout_;
/// Speeds of the wheels
double wheel_speed_cmd_[2];
/// Max. torque applied to the wheels
double torque_;
/// Separation between the wheels
double wheel_sep_;
/// Diameter of the wheels
double wheel_diam_;
/// Vector for pose
double odom_pose_[3];
/// Vector for velocity
double odom_vel_[3];
/// Pointer to pose covariance matrix
double *pose_cov_[36];
/// Pointer to twist covariance matrix
double *twist_cov_[36];
/// ROS publisher for odometry messages
ros::Publisher odom_pub_;
/// ROS message for odometry data
nav_msgs::Odometry odom_;
/// Flag for (not) publish tf transform for odom -> robot
bool publish_tf_;
/// TF transform publisher for the odom frame
tf::TransformBroadcaster tf_broadcaster_;
/// TF transform for the odom frame
geometry_msgs::TransformStamped odom_tf_;
/// Pointer to left cliff sensor
sensors::RaySensorPtr cliff_sensor_left_;
/// Pointer to frontal cliff sensor
sensors::RaySensorPtr cliff_sensor_center_;
/// Pointer to left right sensor
sensors::RaySensorPtr cliff_sensor_right_;
/// ROS publisher for cliff detection events
ros::Publisher cliff_event_pub_;
/// Kobuki ROS message for cliff event
kobuki_msgs::CliffEvent cliff_event_;
/// Cliff flag for the left sensor
bool cliff_detected_left_;
/// Cliff flag for the center sensor
bool cliff_detected_center_;
/// Cliff flag for the right sensor
bool cliff_detected_right_;
/// measured distance in meter for detecting a cliff
float cliff_detection_threshold_;
/// Maximum distance to floor
int floot_dist_;
/// Pointer to bumper sensor simulating Kobuki's left, centre and right bumper sensors
sensors::ContactSensorPtr bumper_;
/// ROS publisher for bumper events
ros::Publisher bumper_event_pub_;
/// Kobuki ROS message for bumper event
kobuki_msgs::BumperEvent bumper_event_;
/// Flag for left bumper's last state
bool bumper_left_was_pressed_;
/// Flag for center bumper's last state
bool bumper_center_was_pressed_;
/// Flag for right bumper's last state
bool bumper_right_was_pressed_;
/// Flag for left bumper's current state
bool bumper_left_is_pressed_;
/// Flag for left bumper's current state
bool bumper_center_is_pressed_;
/// Flag for left bumper's current state
bool bumper_right_is_pressed_;
/// Pointer to IMU sensor model
sensors::ImuSensorPtr imu_;
/// Storage for the angular velocity reported by the IMU
#if GAZEBO_MAJOR_VERSION >= 9
ignition::math::Vector3d vel_angular_;
#else
math::Vector3 vel_angular_;
#endif
/// ROS publisher for IMU data
ros::Publisher imu_pub_;
/// ROS message for publishing IMU data
sensor_msgs::Imu imu_msg_;
/// ROS subscriber for reseting the odometry data
ros::Subscriber odom_reset_sub_;
};
} // namespace gazebo
#endif /* GAZEBO_ROS_KOBUKI_H */

35
autonomous_robotics_ros/src/kobuki_gazebo_plugins/package.xml Normal file
View File

@@ -0,0 +1,35 @@
<package>
<name>kobuki_gazebo_plugins</name>
<version>0.5.7</version>
<description>Kobuki-specific ROS plugins for Gazebo</description>
<maintainer email="marcus.liebhardt@yujinrobot.com">Marcus Liebhardt</maintainer>
<license>BSD</license>
<url type="website">http://ros.org/wiki/kobuki_gazebo_plugins</url>
<url type="repository">https://github.com/yujinrobot/kobuki_desktop</url>
<url type="bugtracker">https://github.com/yujinrobot/kobuki_desktop/issues</url>
<author>Marcus Liebhardt</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>boost</build_depend>
<build_depend>gazebo_ros</build_depend>
<build_depend>gazebo_plugins</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>kobuki_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf</build_depend>
<run_depend>boost</run_depend>
<run_depend>gazebo_ros</run_depend>
<run_depend>gazebo_plugins</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>kobuki_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>tf</run_depend>
</package>

187
autonomous_robotics_ros/src/kobuki_gazebo_plugins/src/gazebo_ros_kobuki.cpp Normal file
View File

@@ -0,0 +1,187 @@
#include <cmath>
#include <cstring>
#include <boost/bind.hpp>
#include <sensor_msgs/JointState.h>
#include <tf/LinearMath/Quaternion.h>
#include "kobuki_gazebo_plugins/gazebo_ros_kobuki.h"
#if GAZEBO_MAJOR_VERSION >= 9
// #include <ignition/math.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Quaternion.hh>
#else
#include <gazebo/math/gzmath.hh>
#endif
namespace gazebo
{
GazeboRosKobuki::GazeboRosKobuki() : shutdown_requested_(false)
{
// Initialise variables
wheel_speed_cmd_[LEFT] = 0.0;
wheel_speed_cmd_[RIGHT] = 0.0;
cliff_detected_left_ = false;
cliff_detected_center_ = false;
cliff_detected_right_ = false;
}
GazeboRosKobuki::~GazeboRosKobuki()
{
// rosnode_->shutdown();
shutdown_requested_ = true;
// Wait for spinner thread to end
// ros_spinner_thread_->join();
// delete spinner_thread_;
// delete rosnode_;
}
void GazeboRosKobuki::Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
{
model_ = parent;
if (!model_)
{
ROS_ERROR_STREAM("Invalid model pointer! [" << node_name_ << "]");
return;
}
gazebo_ros_ = GazeboRosPtr(new GazeboRos(model_, sdf, "Kobuki"));
sdf_ = sdf;
gazebo_ros_->getParameter(this->update_rate_, "update_rate", 0.0);
// Make sure the ROS node for Gazebo has already been initialized
if (!ros::isInitialized())
{
ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
return;
}
// Get then name of the parent model and use it as node name
std::string model_name = sdf->GetParent()->Get<std::string>("name");
gzdbg << "Plugin model name: " << model_name << "\n";
node_name_ = model_name;
world_ = parent->GetWorld();
prepareMotorPower();
preparePublishTf();
if(prepareJointState() == false)
return;
if(prepareWheelAndTorque() == false)
return;
prepareOdom();
if(prepareVelocityCommand() == false)
return;
if(prepareCliffSensor() == false)
return;
if(prepareBumper() == false)
return;
if(prepareIMU() == false)
return;
setupRosApi(model_name);
#if GAZEBO_MAJOR_VERSION >= 9
prev_update_time_ = world_->SimTime();
#else
prev_update_time_ = world_->GetSimTime();
#endif
ROS_INFO_STREAM("GazeboRosKobuki plugin ready to go! [" << node_name_ << "]");
update_connection_ = event::Events::ConnectWorldUpdateEnd(boost::bind(&GazeboRosKobuki::OnUpdate, this));
}
void GazeboRosKobuki::OnUpdate()
{
/*
* First process ROS callbacks
*/
ros::spinOnce();
/*
* Update current time and time step
*/
common::Time time_now;
#if GAZEBO_MAJOR_VERSION >= 9
time_now = world_->SimTime();
#else
time_now = world_->GetSimTime();
#endif
if (time_now < prev_update_time_) {
ROS_WARN_NAMED("gazebo_ros_kobuki", "Negative update time difference detected.");
prev_update_time_ = time_now;
}
common::Time step_time = time_now - prev_update_time_;
// propagate wheel velocity commands at the full simulation rate
propagateVelocityCommands();
// publish rate control
if (this->update_rate_ > 0 && step_time.Double() < (1.0 / this->update_rate_)) {
return;
}
prev_update_time_ = time_now;
updateJointState();
updateOdometry(step_time);
updateIMU();
updateCliffSensor();
updateBumper();
}
void GazeboRosKobuki::spin()
{
ros::Rate r(10);
while(ros::ok() && !shutdown_requested_)
{
r.sleep();
}
}
void GazeboRosKobuki::motorPowerCB(const kobuki_msgs::MotorPowerPtr &msg)
{
if ((msg->state == kobuki_msgs::MotorPower::ON) && (!motors_enabled_))
{
motors_enabled_ = true;
ROS_INFO_STREAM("Motors fired up. [" << node_name_ << "]");
}
else if ((msg->state == kobuki_msgs::MotorPower::OFF) && (motors_enabled_))
{
motors_enabled_ = false;
ROS_INFO_STREAM("Motors taking a rest. [" << node_name_ << "]");
}
}
void GazeboRosKobuki::cmdVelCB(const geometry_msgs::TwistConstPtr &msg)
{
#if GAZEBO_MAJOR_VERSION >= 9
last_cmd_vel_time_ = world_->SimTime();
#else
last_cmd_vel_time_ = world_->GetSimTime();
#endif
wheel_speed_cmd_[LEFT] = msg->linear.x - msg->angular.z * (wheel_sep_) / 2;
wheel_speed_cmd_[RIGHT] = msg->linear.x + msg->angular.z * (wheel_sep_) / 2;
}
void GazeboRosKobuki::resetOdomCB(const std_msgs::EmptyConstPtr &msg)
{
odom_pose_[0] = 0.0;
odom_pose_[1] = 0.0;
odom_pose_[2] = 0.0;
}
// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(GazeboRosKobuki);
} // namespace gazebo

367
autonomous_robotics_ros/src/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp Normal file
View File

@@ -0,0 +1,367 @@
/*
* Copyright (c) 2013, Yujin Robot.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Yujin Robot nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @author Marcus Liebhardt
*
* This work has been inspired by Nate Koenig's Gazebo plugin for the iRobot Create.
*/
#include "kobuki_gazebo_plugins/gazebo_ros_kobuki.h"
namespace gazebo
{
/*
* Prepare receiving motor power commands
*/
void GazeboRosKobuki::prepareMotorPower() {
motors_enabled_ = true;
}
/*
* Prepare joint state publishing
*/
bool GazeboRosKobuki::prepareJointState()
{
if (sdf_->HasElement("left_wheel_joint_name"))
{
left_wheel_joint_name_ = sdf_->GetElement("left_wheel_joint_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find left wheel joint in the model description!"
<< " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
return false;
}
if (sdf_->HasElement("right_wheel_joint_name"))
{
right_wheel_joint_name_ = sdf_->GetElement("right_wheel_joint_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find right wheel joint in the model description!"
<< " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
return false;
}
joints_[LEFT] = model_->GetJoint(left_wheel_joint_name_);
joints_[RIGHT] = model_->GetJoint(right_wheel_joint_name_);
if (!joints_[LEFT] || !joints_[RIGHT])
{
ROS_ERROR_STREAM("Couldn't find specified wheel joints in the model! [" << node_name_ <<"]");
return false;
}
joint_state_.header.frame_id = "Joint States";
joint_state_.name.push_back(left_wheel_joint_name_);
joint_state_.position.push_back(0);
joint_state_.velocity.push_back(0);
joint_state_.effort.push_back(0);
joint_state_.name.push_back(right_wheel_joint_name_);
joint_state_.position.push_back(0);
joint_state_.velocity.push_back(0);
joint_state_.effort.push_back(0);
return true;
}
/*
* Prepare publishing odometry data
*/
void GazeboRosKobuki::preparePublishTf()
{
if (sdf_->HasElement("publish_tf"))
{
publish_tf_ = sdf_->GetElement("publish_tf")->Get<bool>();
if (publish_tf_)
{
ROS_INFO_STREAM("Will publish tf." << " [" << node_name_ <<"]");
}
else
{
ROS_INFO_STREAM("Won't publish tf." << " [" << node_name_ <<"]");
}
}
else
{
publish_tf_ = false;
ROS_INFO_STREAM("Couldn't find the 'publish tf' parameter in the model description."
<< " Won't publish tf." << " [" << node_name_ <<"]");
}
}
bool GazeboRosKobuki::prepareWheelAndTorque()
{
if (sdf_->HasElement("wheel_separation"))
{
wheel_sep_ = sdf_->GetElement("wheel_separation")->Get<double>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return false;
}
if (sdf_->HasElement("wheel_diameter"))
{
wheel_diam_ = sdf_->GetElement("wheel_diameter")->Get<double>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the wheel diameter parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return false;
}
if (sdf_->HasElement("torque"))
{
torque_ = sdf_->GetElement("torque")->Get<double>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the torque parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return false;
}
return true;
}
void GazeboRosKobuki::prepareOdom()
{
odom_pose_[0] = 0.0;
odom_pose_[1] = 0.0;
odom_pose_[2] = 0.0;
}
/*
* Prepare receiving velocity commands
*/
bool GazeboRosKobuki::prepareVelocityCommand()
{
if (sdf_->HasElement("velocity_command_timeout"))
{
cmd_vel_timeout_ = sdf_->GetElement("velocity_command_timeout")->Get<double>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return false;
}
#if GAZEBO_MAJOR_VERSION >= 9
last_cmd_vel_time_ = world_->SimTime();
#else
last_cmd_vel_time_ = world_->GetSimTime();
#endif
return true;
}
bool GazeboRosKobuki::prepareCliffSensor()
{
/*
* Prepare cliff sensors
*/
std::string cliff_sensor_left_name, cliff_sensor_center_name, cliff_sensor_right_name;
if (sdf_->HasElement("cliff_sensor_left_name"))
{
cliff_sensor_left_name = sdf_->GetElement("cliff_sensor_left_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of left cliff sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return false;
}
if (sdf_->HasElement("cliff_sensor_center_name"))
{
cliff_sensor_center_name = sdf_->GetElement("cliff_sensor_center_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of frontal cliff sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return false;
}
if (sdf_->HasElement("cliff_sensor_right_name"))
{
cliff_sensor_right_name = sdf_->GetElement("cliff_sensor_right_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of right cliff sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return false;
}
cliff_sensor_left_ = std::dynamic_pointer_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor(cliff_sensor_left_name));
cliff_sensor_center_ = std::dynamic_pointer_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor(cliff_sensor_center_name));
cliff_sensor_right_ = std::dynamic_pointer_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor(cliff_sensor_right_name));
if (!cliff_sensor_left_)
{
ROS_ERROR_STREAM("Couldn't find the left cliff sensor in the model! [" << node_name_ <<"]");
return false;
}
if (!cliff_sensor_center_)
{
ROS_ERROR_STREAM("Couldn't find the center cliff sensor in the model! [" << node_name_ <<"]");
return false;
}
if (!cliff_sensor_right_)
{
ROS_ERROR_STREAM("Couldn't find the right cliff sensor in the model! [" << node_name_ <<"]");
return false;
}
if (sdf_->HasElement("cliff_detection_threshold"))
{
cliff_detection_threshold_ = sdf_->GetElement("cliff_detection_threshold")->Get<double>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the cliff detection threshold parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return false;
}
cliff_sensor_left_->SetActive(true);
cliff_sensor_center_->SetActive(true);
cliff_sensor_right_->SetActive(true);
return true;
}
/*
* Prepare bumper
*/
bool GazeboRosKobuki::prepareBumper()
{
std::string bumper_name;
if (sdf_->HasElement("bumper_name"))
{
bumper_name = sdf_->GetElement("bumper_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of bumper sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return false;
}
bumper_ = std::dynamic_pointer_cast<sensors::ContactSensor>(
sensors::SensorManager::Instance()->GetSensor(bumper_name));
if (!bumper_)
{
ROS_ERROR_STREAM("Couldn't find the bumpers in the model! [" << node_name_ <<"]");
return false;
}
bumper_->SetActive(true);
return true;
}
/*
* Prepare IMU
*/
bool GazeboRosKobuki::prepareIMU()
{
std::string imu_name;
if (sdf_->HasElement("imu_name"))
{
imu_name = sdf_->GetElement("imu_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of IMU sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return false;
}
#if GAZEBO_MAJOR_VERSION >= 9
imu_ = std::dynamic_pointer_cast<sensors::ImuSensor>(
sensors::get_sensor(world_->Name()+"::"+node_name_+"::base_footprint::"+imu_name));
#else
imu_ = std::dynamic_pointer_cast<sensors::ImuSensor>(
sensors::get_sensor(world_->GetName()+"::"+node_name_+"::base_footprint::"+imu_name));
#endif
if (!imu_)
{
ROS_ERROR_STREAM("Couldn't find the IMU in the model! [" << node_name_ <<"]");
return false;
}
imu_->SetActive(true);
return true;
}
void GazeboRosKobuki::setupRosApi(std::string& model_name)
{
std::string base_prefix;
gazebo_ros_->node()->param("base_prefix", base_prefix, std::string("mobile_base"));
// Public topics
// joint_states
std::string joint_states_topic = "joint_states";
joint_state_pub_ = gazebo_ros_->node()->advertise<sensor_msgs::JointState>(joint_states_topic, 1);
ROS_INFO("%s: Advertise joint_states[%s]!", gazebo_ros_->info(), joint_states_topic.c_str());
// odom
std::string odom_topic = "odom";
odom_pub_ = gazebo_ros_->node()->advertise<nav_msgs::Odometry>(odom_topic, 1);
ROS_INFO("%s: Advertise Odometry[%s]!", gazebo_ros_->info(), odom_topic.c_str());
// Private Topics
// motor power
std::string motor_power_topic = base_prefix + "/commands/motor_power";
motor_power_sub_ = gazebo_ros_->node()->subscribe(motor_power_topic, 10, &GazeboRosKobuki::motorPowerCB, this);
ROS_INFO("%s: Try to subscribe to %s!", gazebo_ros_->info(), motor_power_topic.c_str());
std::string odom_reset_topic = base_prefix + "/commands/reset_odometry";
odom_reset_sub_ = gazebo_ros_->node()->subscribe(odom_reset_topic, 10, &GazeboRosKobuki::resetOdomCB, this);
ROS_INFO("%s: Try to subscribe to %s!", gazebo_ros_->info(), odom_reset_topic.c_str());
// cmd_vel
std::string cmd_vel_topic = base_prefix + "/commands/velocity";
cmd_vel_sub_ = gazebo_ros_->node()->subscribe(cmd_vel_topic, 100, &GazeboRosKobuki::cmdVelCB, this);
ROS_INFO("%s: Try to subscribe to %s!", gazebo_ros_->info(), cmd_vel_topic.c_str());
// cliff
std::string cliff_topic = base_prefix + "/events/cliff";
cliff_event_pub_ = gazebo_ros_->node()->advertise<kobuki_msgs::CliffEvent>(cliff_topic, 1);
ROS_INFO("%s: Advertise Cliff[%s]!", gazebo_ros_->info(), cliff_topic.c_str());
// bumper
std::string bumper_topic = base_prefix + "/events/bumper";
bumper_event_pub_ = gazebo_ros_->node()->advertise<kobuki_msgs::BumperEvent>(bumper_topic, 1);
ROS_INFO("%s: Advertise Bumper[%s]!", gazebo_ros_->info(), bumper_topic.c_str());
// IMU
std::string imu_topic = base_prefix + "/sensors/imu_data";
imu_pub_ = gazebo_ros_->node()->advertise<sensor_msgs::Imu>(imu_topic, 1);
ROS_INFO("%s: Advertise IMU[%s]!", gazebo_ros_->info(), imu_topic.c_str());
}
}

416
autonomous_robotics_ros/src/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp Normal file
View File

@@ -0,0 +1,416 @@
/*
* Copyright (c) 2013, Yujin Robot.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Yujin Robot nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @author Marcus Liebhardt
*
* This work has been inspired by Nate Koenig's Gazebo plugin for the iRobot Create.
*/
#include "kobuki_gazebo_plugins/gazebo_ros_kobuki.h"
namespace gazebo {
void GazeboRosKobuki::updateJointState()
{
/*
* Joint states
*/
std::string baselink_frame = gazebo_ros_->resolveTF("base_link");
joint_state_.header.stamp = ros::Time::now();
joint_state_.header.frame_id = baselink_frame;
#if GAZEBO_MAJOR_VERSION >= 9
joint_state_.position[LEFT] = joints_[LEFT]->Position(0);
joint_state_.position[RIGHT] = joints_[RIGHT]->Position(0);
#else
joint_state_.position[LEFT] = joints_[LEFT]->GetAngle(0).Radian();
joint_state_.position[RIGHT] = joints_[RIGHT]->GetAngle(0).Radian();
#endif
joint_state_.velocity[LEFT] = joints_[LEFT]->GetVelocity(0);
joint_state_.velocity[RIGHT] = joints_[RIGHT]->GetVelocity(0);
joint_state_pub_.publish(joint_state_);
}
/*
* Odometry (encoders & IMU)
*/
void GazeboRosKobuki::updateOdometry(common::Time& step_time)
{
std::string odom_frame = gazebo_ros_->resolveTF("odom");
std::string base_frame = gazebo_ros_->resolveTF("base_footprint");
odom_.header.stamp = joint_state_.header.stamp;
odom_.header.frame_id = odom_frame;
odom_.child_frame_id = base_frame;
// Distance travelled by main wheels
double d1, d2;
double dr, da;
d1 = d2 = 0;
dr = da = 0;
d1 = step_time.Double() * (wheel_diam_ / 2) * joints_[LEFT]->GetVelocity(0);
d2 = step_time.Double() * (wheel_diam_ / 2) * joints_[RIGHT]->GetVelocity(0);
// Can see NaN values here, just zero them out if needed
if (std::isnan(d1))
{
ROS_WARN_STREAM_THROTTLE(0.1, "Gazebo ROS Kobuki plugin: NaN in d1. Step time: " << step_time.Double()
<< ", WD: " << wheel_diam_ << ", velocity: " << joints_[LEFT]->GetVelocity(0));
d1 = 0;
}
if (std::isnan(d2))
{
ROS_WARN_STREAM_THROTTLE(0.1, "Gazebo ROS Kobuki plugin: NaN in d2. Step time: " << step_time.Double()
<< ", WD: " << wheel_diam_ << ", velocity: " << joints_[RIGHT]->GetVelocity(0));
d2 = 0;
}
dr = (d1 + d2) / 2;
da = (d2 - d1) / wheel_sep_; // ignored
// Just as in the Kobuki driver, the angular velocity is taken directly from the IMU
vel_angular_ = imu_->AngularVelocity();
// Compute odometric pose
odom_pose_[0] += dr * cos( odom_pose_[2] );
odom_pose_[1] += dr * sin( odom_pose_[2] );
#if GAZEBO_MAJOR_VERSION >= 9
odom_pose_[2] += vel_angular_.Z() * step_time.Double();
#else
odom_pose_[2] += vel_angular_.z * step_time.Double();
#endif
// Compute odometric instantaneous velocity
odom_vel_[0] = dr / step_time.Double();
odom_vel_[1] = 0.0;
#if GAZEBO_MAJOR_VERSION >= 9
odom_vel_[2] = vel_angular_.Z();
#else
odom_vel_[2] = vel_angular_.z;
#endif
odom_.pose.pose.position.x = odom_pose_[0];
odom_.pose.pose.position.y = odom_pose_[1];
odom_.pose.pose.position.z = 0;
tf::Quaternion qt;
qt.setEuler(0,0,odom_pose_[2]);
odom_.pose.pose.orientation.x = qt.getX();
odom_.pose.pose.orientation.y = qt.getY();
odom_.pose.pose.orientation.z = qt.getZ();
odom_.pose.pose.orientation.w = qt.getW();
odom_.pose.covariance[0] = 0.1;
odom_.pose.covariance[7] = 0.1;
odom_.pose.covariance[35] = 0.05;
odom_.pose.covariance[14] = 1e6;
odom_.pose.covariance[21] = 1e6;
odom_.pose.covariance[28] = 1e6;
odom_.twist.twist.linear.x = odom_vel_[0];
odom_.twist.twist.linear.y = 0;
odom_.twist.twist.linear.z = 0;
odom_.twist.twist.angular.x = 0;
odom_.twist.twist.angular.y = 0;
odom_.twist.twist.angular.z = odom_vel_[2];
odom_pub_.publish(odom_); // publish odom message
if (publish_tf_)
{
odom_tf_.header = odom_.header;
odom_tf_.child_frame_id = odom_.child_frame_id;
odom_tf_.transform.translation.x = odom_.pose.pose.position.x;
odom_tf_.transform.translation.y = odom_.pose.pose.position.y;
odom_tf_.transform.translation.z = odom_.pose.pose.position.z;
odom_tf_.transform.rotation = odom_.pose.pose.orientation;
tf_broadcaster_.sendTransform(odom_tf_);
}
}
/*
* Publish IMU data
*/
void GazeboRosKobuki::updateIMU()
{
imu_msg_.header = joint_state_.header;
#if GAZEBO_MAJOR_VERSION >= 9
ignition::math::Quaterniond quat = imu_->Orientation();
imu_msg_.orientation.x = quat.X();
imu_msg_.orientation.y = quat.Y();
imu_msg_.orientation.z = quat.Z();
imu_msg_.orientation.w = quat.W();
#else
math::Quaternion quat = imu_->Orientation();
imu_msg_.orientation.x = quat.x;
imu_msg_.orientation.y = quat.y;
imu_msg_.orientation.z = quat.z;
imu_msg_.orientation.w = quat.w;
#endif
imu_msg_.orientation_covariance[0] = 1e6;
imu_msg_.orientation_covariance[4] = 1e6;
imu_msg_.orientation_covariance[8] = 0.05;
#if GAZEBO_MAJOR_VERSION >= 9
imu_msg_.angular_velocity.x = vel_angular_.X();
imu_msg_.angular_velocity.y = vel_angular_.Y();
imu_msg_.angular_velocity.z = vel_angular_.Z();
#else
imu_msg_.angular_velocity.x = vel_angular_.x;
imu_msg_.angular_velocity.y = vel_angular_.y;
imu_msg_.angular_velocity.z = vel_angular_.z;
#endif
imu_msg_.angular_velocity_covariance[0] = 1e6;
imu_msg_.angular_velocity_covariance[4] = 1e6;
imu_msg_.angular_velocity_covariance[8] = 0.05;
#if GAZEBO_MAJOR_VERSION >= 9
ignition::math::Vector3d lin_acc = imu_->LinearAcceleration();
imu_msg_.linear_acceleration.x = lin_acc.X();
imu_msg_.linear_acceleration.y = lin_acc.Y();
imu_msg_.linear_acceleration.z = lin_acc.Z();
#else
math::Vector3 lin_acc = imu_->LinearAcceleration();
imu_msg_.linear_acceleration.x = lin_acc.x;
imu_msg_.linear_acceleration.y = lin_acc.y;
imu_msg_.linear_acceleration.z = lin_acc.z;
#endif
imu_pub_.publish(imu_msg_); // publish odom message
}
/*
* Propagate velocity commands
* TODO: Check how to simulate disabled motors, e.g. set MaxForce to zero, but then damping is important!
*/
void GazeboRosKobuki::propagateVelocityCommands()
{
common::Time time_now;
#if GAZEBO_MAJOR_VERSION >= 9
time_now = world_->SimTime();
#else
time_now = world_->GetSimTime();
#endif
if (((time_now - last_cmd_vel_time_).Double() > cmd_vel_timeout_) || !motors_enabled_)
{
wheel_speed_cmd_[LEFT] = 0.0;
wheel_speed_cmd_[RIGHT] = 0.0;
}
joints_[LEFT]->SetVelocity(0, wheel_speed_cmd_[LEFT] / (wheel_diam_ / 2.0));
joints_[RIGHT]->SetVelocity(0, wheel_speed_cmd_[RIGHT] / (wheel_diam_ / 2.0));
}
/*
* Cliff sensors
* Check each sensor separately
*/
void GazeboRosKobuki::updateCliffSensor()
{
// Left cliff sensor
if ((cliff_detected_left_ == false) &&
(cliff_sensor_left_->Range(0) >= cliff_detection_threshold_))
{
cliff_detected_left_ = true;
cliff_event_.sensor = kobuki_msgs::CliffEvent::LEFT;
cliff_event_.state = kobuki_msgs::CliffEvent::CLIFF;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_left_->Range(0)));
cliff_event_pub_.publish(cliff_event_);
}
else if ((cliff_detected_left_ == true) &&
(cliff_sensor_left_->Range(0) < cliff_detection_threshold_))
{
cliff_detected_left_ = false;
cliff_event_.sensor = kobuki_msgs::CliffEvent::LEFT;
cliff_event_.state = kobuki_msgs::CliffEvent::FLOOR;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_left_->Range(0)));
cliff_event_pub_.publish(cliff_event_);
}
// Centre cliff sensor
if ((cliff_detected_center_ == false) &&
(cliff_sensor_center_->Range(0) >= cliff_detection_threshold_))
{
cliff_detected_center_ = true;
cliff_event_.sensor = kobuki_msgs::CliffEvent::CENTER;
cliff_event_.state = kobuki_msgs::CliffEvent::CLIFF;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_center_->Range(0)));
cliff_event_pub_.publish(cliff_event_);
}
else if ((cliff_detected_center_ == true) &&
(cliff_sensor_center_->Range(0) < cliff_detection_threshold_))
{
cliff_detected_center_ = false;
cliff_event_.sensor = kobuki_msgs::CliffEvent::CENTER;
cliff_event_.state = kobuki_msgs::CliffEvent::FLOOR;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_center_->Range(0)));
cliff_event_pub_.publish(cliff_event_);
}
// Right cliff sensor
if ((cliff_detected_right_ == false) &&
(cliff_sensor_right_->Range(0) >= cliff_detection_threshold_))
{
cliff_detected_right_ = true;
cliff_event_.sensor = kobuki_msgs::CliffEvent::RIGHT;
cliff_event_.state = kobuki_msgs::CliffEvent::CLIFF;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_right_->Range(0)));
cliff_event_pub_.publish(cliff_event_);
}
else if ((cliff_detected_right_ == true) &&
(cliff_sensor_right_->Range(0) < cliff_detection_threshold_))
{
cliff_detected_right_ = false;
cliff_event_.sensor = kobuki_msgs::CliffEvent::RIGHT;
cliff_event_.state = kobuki_msgs::CliffEvent::FLOOR;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_right_->Range(0)));
cliff_event_pub_.publish(cliff_event_);
}
}
/*
* Bumpers
*/
// In order to simulate the three bumper sensors, a contact is assigned to one of the bumpers
// depending on its position. Each sensor covers a range of 60 degrees.
// +90 ... +30: left bumper
// +30 ... -30: centre bumper
// -30 ... -90: right bumper
void GazeboRosKobuki::updateBumper()
{
// reset flags
bumper_left_is_pressed_ = false;
bumper_center_is_pressed_ = false;
bumper_right_is_pressed_ = false;
// parse contacts
msgs::Contacts contacts;
contacts = bumper_->Contacts();
double robot_heading;
#if GAZEBO_MAJOR_VERSION >= 9
ignition::math::Pose3d current_pose = model_->WorldPose();
robot_heading = current_pose.Rot().Yaw();
#else
math::Pose current_pose = model_->GetWorldPose();
robot_heading = current_pose.rot.GetYaw();
#endif
for (int i = 0; i < contacts.contact_size(); ++i)
{
double rel_contact_pos;
#if GAZEBO_MAJOR_VERSION >= 9
rel_contact_pos = contacts.contact(i).position(0).z() - current_pose.Pos().Z();
#else
rel_contact_pos = contacts.contact(i).position(0).z() - current_pose.pos.z;
#endif
// Actually, only contacts at the height of the bumper should be considered, but since we use a simplified collision model
// contacts further below and above need to be consider as well to identify "bumps" reliably.
if ((rel_contact_pos >= 0.01)
&& (rel_contact_pos <= 0.13))
{
// using the force normals below, since the contact position is given in world coordinates
// also negating the normal, because it points from contact to robot centre
double global_contact_angle = std::atan2(-contacts.contact(i).normal(0).y(), -contacts.contact(i).normal(0).x());
double relative_contact_angle = global_contact_angle - robot_heading;
if ((relative_contact_angle <= (M_PI/2)) && (relative_contact_angle > (M_PI/6)))
{
bumper_left_is_pressed_ = true;
}
else if ((relative_contact_angle <= (M_PI/6)) && (relative_contact_angle >= (-M_PI/6)))
{
bumper_center_is_pressed_ = true;
}
else if ((relative_contact_angle < (-M_PI/6)) && (relative_contact_angle >= (-M_PI/2)))
{
bumper_right_is_pressed_ = true;
}
}
}
// check for bumper state change
if (bumper_left_is_pressed_ && !bumper_left_was_pressed_)
{
bumper_left_was_pressed_ = true;
bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::LEFT;
bumper_event_pub_.publish(bumper_event_);
}
else if (!bumper_left_is_pressed_ && bumper_left_was_pressed_)
{
bumper_left_was_pressed_ = false;
bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::LEFT;
bumper_event_pub_.publish(bumper_event_);
}
if (bumper_center_is_pressed_ && !bumper_center_was_pressed_)
{
bumper_center_was_pressed_ = true;
bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::CENTER;
bumper_event_pub_.publish(bumper_event_);
}
else if (!bumper_center_is_pressed_ && bumper_center_was_pressed_)
{
bumper_center_was_pressed_ = false;
bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::CENTER;
bumper_event_pub_.publish(bumper_event_);
}
if (bumper_right_is_pressed_ && !bumper_right_was_pressed_)
{
bumper_right_was_pressed_ = true;
bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::RIGHT;
bumper_event_pub_.publish(bumper_event_);
}
else if (!bumper_right_is_pressed_ && bumper_right_was_pressed_)
{
bumper_right_was_pressed_ = false;
bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::RIGHT;
bumper_event_pub_.publish(bumper_event_);
}
}
}

32
autonomous_robotics_ros/src/turtlebot/turtlebot_interactions/turtlebot_interactions/.cproject → autonomous_robotics_ros/src/kobuki_keyop/.cproject Executable file → Normal file
View File

@@ -3,8 +3,8 @@
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.base.292549303">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.292549303" moduleId="org.eclipse.cdt.core.settings" name="Default">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.base.764999879">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.764999879" moduleId="org.eclipse.cdt.core.settings" name="Default">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
@@ -16,17 +16,17 @@
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration buildProperties="" id="cdt.managedbuild.toolchain.gnu.base.292549303" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
<folderInfo id="cdt.managedbuild.toolchain.gnu.base.292549303.851934431" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.base.26396418" name="cdt.managedbuild.toolchain.gnu.base" superClass="cdt.managedbuild.toolchain.gnu.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.54978654" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
<builder id="cdt.managedbuild.target.gnu.builder.base.254702896" managedBuildOn="false" name="Gnu Make Builder.Default" superClass="cdt.managedbuild.target.gnu.builder.base"/>
<tool id="cdt.managedbuild.tool.gnu.archiver.base.814527293" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.47368163" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base"/>
<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.1432401321" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base"/>
<tool id="cdt.managedbuild.tool.gnu.c.linker.base.56660708" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.763263509" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base"/>
<tool id="cdt.managedbuild.tool.gnu.assembler.base.891453422" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base"/>
<configuration buildProperties="" id="cdt.managedbuild.toolchain.gnu.base.764999879" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
<folderInfo id="cdt.managedbuild.toolchain.gnu.base.764999879.1338312139" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.base.1216247846" name="cdt.managedbuild.toolchain.gnu.base" superClass="cdt.managedbuild.toolchain.gnu.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.350170162" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
<builder id="cdt.managedbuild.target.gnu.builder.base.718647454" managedBuildOn="false" name="Gnu Make Builder.Default" superClass="cdt.managedbuild.target.gnu.builder.base"/>
<tool id="cdt.managedbuild.tool.gnu.archiver.base.329257889" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.1762563256" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base"/>
<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.1230521187" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base"/>
<tool id="cdt.managedbuild.tool.gnu.c.linker.base.860032783" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.174486257" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base"/>
<tool id="cdt.managedbuild.tool.gnu.assembler.base.1912851036" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base"/>
</toolChain>
</folderInfo>
</configuration>
@@ -34,10 +34,10 @@
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
</cconfiguration>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="turtlebot_interactions.null.17435872" name="turtlebot_interactions"/>
</storageModule>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="kobuki_keyop.null.2117610536" name="kobuki_keyop"/>
</storageModule>
</cproject>

2
autonomous_robotics_ros/src/turtlebot/turtlebot_msgs/.project → autonomous_robotics_ros/src/kobuki_keyop/.project Executable file → Normal file
View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>turtlebot_msgs</name>
<name>kobuki_keyop</name>
<comment></comment>
<projects>
</projects>

55
autonomous_robotics_ros/src/kobuki_keyop/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,55 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package kobuki_keyop
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.6.6 (2015-05-27)
------------------
* Installed both C/C++ headers.
* Installed missing include directory.
* Contributors: James Goppert
0.6.5 (2014-11-21)
------------------
0.6.4 (2014-08-26)
------------------
0.6.3 (2014-08-25)
------------------
0.6.2 (2014-08-11)
------------------
0.6.1 (2014-08-08)
------------------
0.6.0 (2014-08-08)
------------------
* Add missing run dependency on yocs_cmd_vel_mux
* Contributors: Jorge Santos
0.5.5 (2013-10-11)
------------------
* Rename cmd_vel_mux as yocs_cmd_vel_mux.
0.5.4 (2013-09-06)
------------------
0.5.3 (2013-08-30)
------------------
0.5.0 (2013-08-29)
------------------
* kobuki : Added extra url info on all packages.
* Fix URL to the previous changelog wiki.
* Changelogs at package level.
0.4.0 (2013-08-09)
------------------
* Removed remaping for fake_local.
Previous versions, bugfixing
============================
Available in ROS wiki: http://ros.org/wiki/kobuki/ChangeList

24
autonomous_robotics_ros/src/kobuki_keyop/CMakeLists.txt Normal file
View File

@@ -0,0 +1,24 @@
cmake_minimum_required(VERSION 2.8.3)
project(kobuki_keyop)
find_package(catkin REQUIRED COMPONENTS geometry_msgs std_srvs std_msgs roscpp ecl_exceptions ecl_threads ecl_time kobuki_msgs)
catkin_package(
INCLUDE_DIRS include
LIBRARIES kobuki
CATKIN_DEPENDS geometry_msgs std_srvs std_msgs roscpp ecl_exceptions ecl_threads ecl_time kobuki_msgs
)
include_directories(include ${catkin_INCLUDE_DIRS})
add_subdirectory(src)
install(DIRECTORY include
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h*")
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY param
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

48
autonomous_robotics_ros/src/kobuki_keyop/doxygen/mainpage.dox Normal file
View File

@@ -0,0 +1,48 @@
/**
\mainpage
\htmlinclude manifest.html
\section Overview
Provides a keyboard teleoperation control to kobukibot. Just enables a simple
increase velocity mechanism both linearly and angularly as well as power off
and power on functions.
\section published Published Topics
- <tt>kobuki_keyop/cmd_vel (geometry_msgs/Twist)</tt>
Sends command velocities to the mobile base interface.
- <tt>kobuki_keyop/enable (std_msgs/String)</tt>
- <tt>kobuki_keyop/disable (std_msgs/String)</tt>
Send power-off and power-on commands to the robot core.
\section Usage
Use a launcher to fire up the keyop command as this will allow you to map the above
topics to the appropriate namespace. e.g. there is a launcher in
kobuki_keyop/resources/launch/robot_core.launch for ycs' robot core:
@code
<launch>
<node pkg="kobuki_keyop" type="keyop" name="kobuki_keyop">
<remap from="kobuki_keyop/enable" to="/robot_core/enable"/>
<remap from="kobuki_keyop/disable" to="/robot_core/disable"/>
<remap from="kobuki_keyop/vel_cmd" to="/robot_core/cmd_vel"/>
</node>
</launch>
@endcode
You may wish to change the remappings to suit the currently running robot. You can initiate with
@code
roslaunch kobuki_keyop robot_core.launch
@endcode
and use the arrow keys to navigation, with 'e', 'd' to enable
or disable the motors and 'q' to quit.
*/

125
autonomous_robotics_ros/src/kobuki_keyop/include/keyop_core/keyop_core.hpp Normal file
View File

@@ -0,0 +1,125 @@
/*
* Copyright (c) 2012, Yujin Robot.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Yujin Robot nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file /include/teleop_core/keyop_core.hpp
*
* @brief The controlling node for remote operations on robot_core.
*
**/
/*****************************************************************************
** Ifdefs
*****************************************************************************/
#ifndef KEYOP_CORE_NODE_HPP_
#define KEYOP_CORE_NODE_HPP_
/*****************************************************************************
** Includes
*****************************************************************************/
#include <ros/ros.h>
#include <termios.h> // for keyboard input
#include <ecl/threads.hpp>
#include <geometry_msgs/Twist.h> // for velocity commands
#include <geometry_msgs/TwistStamped.h> // for velocity commands
#include <kobuki_msgs/KeyboardInput.h> // keycodes from remote teleops.
/*****************************************************************************
** Namespaces
*****************************************************************************/
namespace keyop_core
{
/*****************************************************************************
** Interface
*****************************************************************************/
/**
* @brief Keyboard remote control for our robot core (mobile base).
*
*/
class KeyOpCore
{
public:
/*********************
** C&D
**********************/
KeyOpCore();
~KeyOpCore();
bool init();
/*********************
** Runtime
**********************/
void spin();
private:
ros::Subscriber keyinput_subscriber;
ros::Publisher velocity_publisher_;
ros::Publisher motor_power_publisher_;
bool last_zero_vel_sent;
bool accept_incoming;
bool power_status;
bool wait_for_connection_;
geometry_msgs::TwistPtr cmd;
geometry_msgs::TwistStampedPtr cmd_stamped;
double linear_vel_step, linear_vel_max;
double angular_vel_step, angular_vel_max;
std::string name;
/*********************
** Commands
**********************/
void enable();
void disable();
void incrementLinearVelocity();
void decrementLinearVelocity();
void incrementAngularVelocity();
void decrementAngularVelocity();
void resetVelocity();
/*********************
** Keylogging
**********************/
void keyboardInputLoop();
void processKeyboardInput(char c);
void remoteKeyInputReceived(const kobuki_msgs::KeyboardInput& key);
void restoreTerminal();
bool quit_requested;
int key_file_descriptor;
struct termios original_terminal_state;
ecl::Thread thread;
};
} // namespace keyop_core
#endif /* KEYOP_CORE_NODE_HPP_ */

14
autonomous_robotics_ros/src/kobuki_keyop/launch/keyop.launch Normal file
View File

@@ -0,0 +1,14 @@
<!--
Raw keyop configuration for working with the default kobuki launcher (minimal.launch).
-->
<launch>
<node pkg="kobuki_keyop" type="keyop" name="keyop" output="screen">
<remap from="keyop/motor_power" to="mobile_base/commands/motor_power"/>
<remap from="keyop/cmd_vel" to="mobile_base/commands/velocity"/>
<param name="linear_vel_step" value="0.05" type="double"/>
<param name="linear_vel_max" value="1.5" type="double"/>
<param name="angular_vel_step" value="0.33" type="double"/>
<param name="angular_vel_max" value="6.6" type="double"/>
<param name="wait_for_connection_" value="true" type="bool"/>
</node>
</launch>

15
autonomous_robotics_ros/src/kobuki_keyop/launch/readme.txt Normal file
View File

@@ -0,0 +1,15 @@
== Overview ==
All your launchers should go in here - ros will automatically find them when you
use the roslaunch command.
== Debugging ==
To run gdb on any node in the launch file, simply insert
launch-prefix="gdb --args"
before any of the node calls or nodelet manager/standalone calls. For example:
<node launch-prefix="gdb --args" pkg="nodelet" type="nodelet" name="robot_core" ns="robot_core" args="manager"/>

35
autonomous_robotics_ros/src/kobuki_keyop/launch/safe_keyop.launch Normal file
View File

@@ -0,0 +1,35 @@
<!--
Keyop configuration for working with the default kobuki launcher (minimal.launch).
-->
<launch>
<node pkg="nodelet" type="nodelet" name="cmd_vel_mux" args="load yocs_cmd_vel_mux/CmdVelMuxNodelet mobile_base_nodelet_manager">
<param name="yaml_cfg_file" value="$(find kobuki_keyop)/param/keyop_mux.yaml"/>
<remap from="cmd_vel_mux/output" to="mobile_base/commands/velocity"/>
</node>
<node pkg="nodelet" type="nodelet" name="kobuki_safety_controller" args="load kobuki_safety_controller/SafetyControllerNodelet mobile_base_nodelet_manager">
<remap from="kobuki_safety_controller/cmd_vel" to="cmd_vel_mux/safety_controller"/>
<remap from="kobuki_safety_controller/events/bumper" to="mobile_base/events/bumper"/>
<remap from="kobuki_safety_controller/events/cliff" to="mobile_base/events/cliff"/>
<remap from="kobuki_safety_controller/events/wheel_drop" to="mobile_base/events/wheel_drop"/>
</node>
<node pkg="nodelet" type="nodelet" name="keyop_vel_smoother" args="load yocs_velocity_smoother/VelocitySmootherNodelet mobile_base_nodelet_manager">
<rosparam file="$(find kobuki_keyop)/param/keyop_smoother.yaml" command="load"/>
<remap from="keyop_vel_smoother/smooth_cmd_vel" to="cmd_vel_mux/keyboard_teleop"/>
<!-- Robot velocity feedbacks; use the one configured as base default -->
<remap from="keyop_vel_smoother/odometry" to="odom"/>
<remap from="keyop_vel_smoother/robot_cmd_vel" to="mobile_base/commands/velocity"/>
</node>
<node pkg="kobuki_keyop" type="keyop" name="keyop" output="screen">
<remap from="keyop/motor_power" to="mobile_base/commands/motor_power"/>
<remap from="keyop/cmd_vel" to="keyop_vel_smoother/raw_cmd_vel"/>
<param name="linear_vel_step" value="0.05" type="double"/>
<param name="linear_vel_max" value="1.5" type="double"/>
<param name="angular_vel_step" value="0.33" type="double"/>
<param name="angular_vel_max" value="6.6" type="double"/>
<param name="wait_for_connection_" value="true" type="bool"/>
</node>
</launch>

48
autonomous_robotics_ros/src/kobuki_keyop/package.xml Normal file
View File

@@ -0,0 +1,48 @@
<package>
<name>kobuki_keyop</name>
<version>0.7.6</version>
<description>
Keyboard teleoperation for Kobuki: relays commands from a keyboard to Kobuki.
</description>
<author email="stonier@yujinrobot.com">Daniel Stonier</author>
<maintainer email="stonier@yujinrobot.com">Daniel Stonier</maintainer>
<license>BSD</license>
<url type="bugtracker">https://github.com/yujinrobot/kobuki/issues</url>
<url type="repository">https://github.com/yujinrobot/kobuki</url>
<url type="website">http://ros.org/wiki/kobuki_keyop</url>
<buildtool_depend>catkin</buildtool_depend>
<!-- Ros -->
<build_depend>geometry_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>roscpp</build_depend>
<!-- Ecl -->
<build_depend>ecl_exceptions</build_depend>
<build_depend>ecl_threads</build_depend>
<build_depend>ecl_time</build_depend>
<!-- Kobuki -->
<build_depend>kobuki_msgs</build_depend>
<!-- Ros -->
<run_depend>geometry_msgs</run_depend>
<run_depend>std_srvs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>roscpp</run_depend>
<!-- Ecl -->
<run_depend>ecl_exceptions</run_depend>
<run_depend>ecl_threads</run_depend>
<run_depend>ecl_time</run_depend>
<!-- Kobuki -->
<run_depend>kobuki_msgs</run_depend>
<!-- Yujin ocs -->
<run_depend>yocs_cmd_vel_mux</run_depend>
<run_depend>yocs_velocity_smoother</run_depend>
</package>

13
autonomous_robotics_ros/src/turtlebot/turtlebot_apps/turtlebot_follower/param/mux.yaml → autonomous_robotics_ros/src/kobuki_keyop/param/keyop_mux.yaml Executable file → Normal file
View File

@@ -1,6 +1,7 @@
# Created on: Dec 5, 2012
# Created on: Oct 29, 2012
# Author: jorge
# Configuration for subscribers to cmd_vel sources.
# Configuration for subscribers to cmd_vel sources for kobuki keyop operation
# with safety reactive controller.
#
# Individual subscriber configuration:
# name: Source name
@@ -11,10 +12,10 @@
subscribers:
- name: "Safe reactive controller"
topic: "input/safety_controller"
topic: "safety_controller"
timeout: 0.2
priority: 10
- name: "Follower"
topic: "input/follower"
timeout: 0.5
- name: "Keyboard operation"
topic: "keyboard_teleop"
timeout: 0.2
priority: 7

11
autonomous_robotics_ros/src/kobuki_keyop/param/keyop_smoother.yaml Normal file
View File

@@ -0,0 +1,11 @@
# Mandatory parameters
speed_lim_v: 0.8
speed_lim_w: 5.4
accel_lim_v: 1.0
accel_lim_w: 7.0
# Optional parameters
frequency: 20.0
decel_factor: 1.0

17
autonomous_robotics_ros/src/kobuki_keyop/src/CMakeLists.txt Normal file
View File

@@ -0,0 +1,17 @@
##############################################################################
# Sources
##############################################################################
file(GLOB SOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.cpp)
##############################################################################
# Targets
##############################################################################
add_executable(keyop ${SOURCES})
add_dependencies(keyop ${catkin_EXPORTED_TARGETS})
target_link_libraries(keyop ${catkin_LIBRARIES})
install(TARGETS keyop
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

490
autonomous_robotics_ros/src/kobuki_keyop/src/keyop_core.cpp Normal file
View File

@@ -0,0 +1,490 @@
/*
* Copyright (c) 2012, Yujin Robot.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Yujin Robot nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file /kobuki_keyop/src/keyop_core.cpp
*
* @brief Creates a node for remote controlling parts of robot_core.
*
**/
/*****************************************************************************
** Includes
*****************************************************************************/
#include <ros/ros.h>
#include <ecl/time.hpp>
#include <ecl/exceptions.hpp>
#include <std_srvs/Empty.h>
#include <kobuki_msgs/MotorPower.h>
#include "../include/keyop_core/keyop_core.hpp"
/*****************************************************************************
** Namespaces
*****************************************************************************/
namespace keyop_core
{
/*****************************************************************************
** Implementation
*****************************************************************************/
/**
* @brief Default constructor, needs initialisation.
*/
KeyOpCore::KeyOpCore() : last_zero_vel_sent(true), // avoid zero-vel messages from the beginning
accept_incoming(true),
power_status(false),
wait_for_connection_(true),
cmd(new geometry_msgs::Twist()),
cmd_stamped(new geometry_msgs::TwistStamped()),
linear_vel_step(0.1),
linear_vel_max(3.4),
angular_vel_step(0.02),
angular_vel_max(1.2),
quit_requested(false),
key_file_descriptor(0)
{
tcgetattr(key_file_descriptor, &original_terminal_state); // get terminal properties
}
KeyOpCore::~KeyOpCore()
{
tcsetattr(key_file_descriptor, TCSANOW, &original_terminal_state);
}
/**
* @brief Initialises the node.
*/
bool KeyOpCore::init()
{
ros::NodeHandle nh("~");
name = nh.getUnresolvedNamespace();
/*********************
** Parameters
**********************/
nh.getParam("linear_vel_step", linear_vel_step);
nh.getParam("linear_vel_max", linear_vel_max);
nh.getParam("angular_vel_step", angular_vel_step);
nh.getParam("angular_vel_max", angular_vel_max);
nh.getParam("wait_for_connection", wait_for_connection_);
ROS_INFO_STREAM("KeyOpCore : using linear vel step [" << linear_vel_step << "].");
ROS_INFO_STREAM("KeyOpCore : using linear vel max [" << linear_vel_max << "].");
ROS_INFO_STREAM("KeyOpCore : using angular vel step [" << angular_vel_step << "].");
ROS_INFO_STREAM("KeyOpCore : using angular vel max [" << angular_vel_max << "].");
/*********************
** Subscribers
**********************/
keyinput_subscriber = nh.subscribe("teleop", 1, &KeyOpCore::remoteKeyInputReceived, this);
/*********************
** Publishers
**********************/
velocity_publisher_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
motor_power_publisher_ = nh.advertise<kobuki_msgs::MotorPower>("motor_power", 1);
/*********************
** Velocities
**********************/
cmd->linear.x = 0.0;
cmd->linear.y = 0.0;
cmd->linear.z = 0.0;
cmd->angular.x = 0.0;
cmd->angular.y = 0.0;
cmd->angular.z = 0.0;
/*********************
** Wait for connection
**********************/
if (!wait_for_connection_)
{
return true;
}
ecl::MilliSleep millisleep;
int count = 0;
bool connected = false;
while (!connected)
{
if (motor_power_publisher_.getNumSubscribers() > 0)
{
connected = true;
break;
}
if (count == 6)
{
connected = false;
break;
}
else
{
ROS_WARN_STREAM("KeyOp: could not connect, trying again after 500ms...");
try
{
millisleep(500);
}
catch (ecl::StandardException e)
{
ROS_ERROR_STREAM("Waiting has been interrupted.");
ROS_DEBUG_STREAM(e.what());
return false;
}
++count;
}
}
if (!connected)
{
ROS_ERROR("KeyOp: could not connect.");
ROS_ERROR("KeyOp: check remappings for enable/disable topics).");
}
else
{
kobuki_msgs::MotorPower power_cmd;
power_cmd.state = kobuki_msgs::MotorPower::ON;
motor_power_publisher_.publish(power_cmd);
ROS_INFO("KeyOp: connected.");
power_status = true;
}
// start keyboard input thread
thread.start(&KeyOpCore::keyboardInputLoop, *this);
return true;
}
/*****************************************************************************
** Implementation [Spin]
*****************************************************************************/
/**
* @brief Worker thread loop; sends current velocity command at a fixed rate.
*
* It also process ros functions as well as aborting when requested.
*/
void KeyOpCore::spin()
{
ros::Rate loop_rate(10);
while (!quit_requested && ros::ok())
{
// Avoid spamming robot with continuous zero-velocity messages
if ((cmd->linear.x != 0.0) || (cmd->linear.y != 0.0) || (cmd->linear.z != 0.0) ||
(cmd->angular.x != 0.0) || (cmd->angular.y != 0.0) || (cmd->angular.z != 0.0))
{
velocity_publisher_.publish(cmd);
last_zero_vel_sent = false;
}
else if (last_zero_vel_sent == false)
{
velocity_publisher_.publish(cmd);
last_zero_vel_sent = true;
}
accept_incoming = true;
ros::spinOnce();
loop_rate.sleep();
}
if (quit_requested)
{ // ros node is still ok, send a disable command
disable();
}
else
{
// just in case we got here not via a keyboard quit request
quit_requested = true;
thread.cancel();
}
thread.join();
}
/*****************************************************************************
** Implementation [Keyboard]
*****************************************************************************/
/**
* @brief The worker thread function that accepts input keyboard commands.
*
* This is ok here - but later it might be a good idea to make a node which
* posts keyboard events to a topic. Recycle common code if used by many!
*/
void KeyOpCore::keyboardInputLoop()
{
struct termios raw;
memcpy(&raw, &original_terminal_state, sizeof(struct termios));
raw.c_lflag &= ~(ICANON | ECHO);
// Setting a new line, then end of file
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(key_file_descriptor, TCSANOW, &raw);
puts("Reading from keyboard");
puts("---------------------------");
puts("Forward/back arrows : linear velocity incr/decr.");
puts("Right/left arrows : angular velocity incr/decr.");
puts("Spacebar : reset linear/angular velocities.");
puts("d : disable motors.");
puts("e : enable motors.");
puts("q : quit.");
char c;
while (!quit_requested)
{
if (read(key_file_descriptor, &c, 1) < 0)
{
perror("read char failed():");
exit(-1);
}
processKeyboardInput(c);
}
}
/**
* @brief Callback function for remote keyboard inputs subscriber.
*/
void KeyOpCore::remoteKeyInputReceived(const kobuki_msgs::KeyboardInput& key)
{
processKeyboardInput(key.pressedKey);
}
/**
* @brief Process individual keyboard inputs.
*
* @param c keyboard input.
*/
void KeyOpCore::processKeyboardInput(char c)
{
/*
* Arrow keys are a bit special, they are escape characters - meaning they
* trigger a sequence of keycodes. In this case, 'esc-[-Keycode_xxx'. We
* ignore the esc-[ and just parse the last one. So long as we avoid using
* the last one for its actual purpose (e.g. left arrow corresponds to
* esc-[-D) we can keep the parsing simple.
*/
switch (c)
{
case kobuki_msgs::KeyboardInput::KeyCode_Left:
{
incrementAngularVelocity();
break;
}
case kobuki_msgs::KeyboardInput::KeyCode_Right:
{
decrementAngularVelocity();
break;
}
case kobuki_msgs::KeyboardInput::KeyCode_Up:
{
incrementLinearVelocity();
break;
}
case kobuki_msgs::KeyboardInput::KeyCode_Down:
{
decrementLinearVelocity();
break;
}
case kobuki_msgs::KeyboardInput::KeyCode_Space:
{
resetVelocity();
break;
}
case 'q':
{
quit_requested = true;
break;
}
case 'd':
{
disable();
break;
}
case 'e':
{
enable();
break;
}
default:
{
break;
}
}
}
/*****************************************************************************
** Implementation [Commands]
*****************************************************************************/
/**
* @brief Disables commands to the navigation system.
*
* This does the following things:
*
* - Disables power to the navigation motors (via device_manager).
* @param msg
*/
void KeyOpCore::disable()
{
cmd->linear.x = 0.0;
cmd->angular.z = 0.0;
velocity_publisher_.publish(cmd);
accept_incoming = false;
if (power_status)
{
ROS_INFO("KeyOp: die, die, die (disabling power to the device's motor system).");
kobuki_msgs::MotorPower power_cmd;
power_cmd.state = kobuki_msgs::MotorPower::OFF;
motor_power_publisher_.publish(power_cmd);
power_status = false;
}
else
{
ROS_WARN("KeyOp: Motor system has already been powered down.");
}
}
/**
* @brief Reset/re-enable the navigation system.
*
* - resets the command velocities.
* - reenable power if not enabled.
*/
void KeyOpCore::enable()
{
accept_incoming = false;
cmd->linear.x = 0.0;
cmd->angular.z = 0.0;
velocity_publisher_.publish(cmd);
if (!power_status)
{
ROS_INFO("KeyOp: Enabling power to the device subsystem.");
kobuki_msgs::MotorPower power_cmd;
power_cmd.state = kobuki_msgs::MotorPower::ON;
motor_power_publisher_.publish(power_cmd);
power_status = true;
}
else
{
ROS_WARN("KeyOp: Device has already been powered up.");
}
}
/**
* @brief If not already maxxed, increment the command velocities..
*/
void KeyOpCore::incrementLinearVelocity()
{
if (power_status)
{
if (cmd->linear.x <= linear_vel_max)
{
cmd->linear.x += linear_vel_step;
}
ROS_INFO_STREAM("KeyOp: linear velocity incremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
}
else
{
ROS_WARN_STREAM("KeyOp: motors are not yet powered up.");
}
}
/**
* @brief If not already minned, decrement the linear velocities..
*/
void KeyOpCore::decrementLinearVelocity()
{
if (power_status)
{
if (cmd->linear.x >= -linear_vel_max)
{
cmd->linear.x -= linear_vel_step;
}
ROS_INFO_STREAM("KeyOp: linear velocity decremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
}
else
{
ROS_WARN_STREAM("KeyOp: motors are not yet powered up.");
}
}
/**
* @brief If not already maxxed, increment the angular velocities..
*/
void KeyOpCore::incrementAngularVelocity()
{
if (power_status)
{
if (cmd->angular.z <= angular_vel_max)
{
cmd->angular.z += angular_vel_step;
}
ROS_INFO_STREAM("KeyOp: angular velocity incremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
}
else
{
ROS_WARN_STREAM("KeyOp: motors are not yet powered up.");
}
}
/**
* @brief If not already mined, decrement the angular velocities..
*/
void KeyOpCore::decrementAngularVelocity()
{
if (power_status)
{
if (cmd->angular.z >= -angular_vel_max)
{
cmd->angular.z -= angular_vel_step;
}
ROS_INFO_STREAM("KeyOp: angular velocity decremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
}
else
{
ROS_WARN_STREAM("KeyOp: motors are not yet powered up.");
}
}
void KeyOpCore::resetVelocity()
{
if (power_status)
{
cmd->angular.z = 0.0;
cmd->linear.x = 0.0;
ROS_INFO_STREAM("KeyOp: reset linear/angular velocities.");
}
else
{
ROS_WARN_STREAM("KeyOp: motors are not yet powered up.");
}
}
} // namespace keyop_core

68
autonomous_robotics_ros/src/kobuki_keyop/src/main.cpp Normal file
View File

@@ -0,0 +1,68 @@
/*
* Copyright (c) 2012, Yujin Robot.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Yujin Robot nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file /kobuki_keyop/src/main.cpp
*
* @brief Executable code for the keyop node.
*
**/
/*****************************************************************************
** Includes
*****************************************************************************/
#include "../include/keyop_core/keyop_core.hpp"
/*****************************************************************************
** Using
*****************************************************************************/
using keyop_core::KeyOpCore;
/*****************************************************************************
** Main
*****************************************************************************/
int main(int argc, char **argv)
{
ros::init(argc, argv, "kobuki_keyop");
KeyOpCore keyop;
if (keyop.init())
{
keyop.spin();
}
else
{
ROS_ERROR_STREAM("Couldn't initialise KeyOpCore!");
}
ROS_INFO_STREAM("Program exiting");
return 0;
}

1
autonomous_robotics_ros/src/kobuki_msgs Submodule

Submodule autonomous_robotics_ros/src/kobuki_msgs added at ec509794be

49
autonomous_robotics_ros/src/kobuki_node/.cproject Normal file
View File

@@ -0,0 +1,49 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?>
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.base.1315454201">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.1315454201" moduleId="org.eclipse.cdt.core.settings" name="Default">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration buildProperties="" id="cdt.managedbuild.toolchain.gnu.base.1315454201" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
<folderInfo id="cdt.managedbuild.toolchain.gnu.base.1315454201.920758464" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.base.1848869794" name="cdt.managedbuild.toolchain.gnu.base" superClass="cdt.managedbuild.toolchain.gnu.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.1999764023" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
<builder id="cdt.managedbuild.target.gnu.builder.base.1327781509" managedBuildOn="false" name="Gnu Make Builder.Default" superClass="cdt.managedbuild.target.gnu.builder.base"/>
<tool id="cdt.managedbuild.tool.gnu.archiver.base.686385995" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.1164343677" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base"/>
<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.1198345006" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base"/>
<tool id="cdt.managedbuild.tool.gnu.c.linker.base.1820051360" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.285654227" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base"/>
<tool id="cdt.managedbuild.tool.gnu.assembler.base.1270327028" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base"/>
</toolChain>
</folderInfo>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
</cconfiguration>
</storageModule>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.50829974;cdt.managedbuild.toolchain.gnu.base.50829974.343060456;cdt.managedbuild.tool.gnu.cpp.compiler.base.30430456;cdt.managedbuild.tool.gnu.cpp.compiler.input.1575080765">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"/>
</scannerConfigBuildInfo>
<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.50829974;cdt.managedbuild.toolchain.gnu.base.50829974.343060456;cdt.managedbuild.tool.gnu.c.compiler.base.1852192628;cdt.managedbuild.tool.gnu.c.compiler.input.1651955734">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"/>
</scannerConfigBuildInfo>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="kobuki_node.null.1245504987" name="kobuki_node"/>
</storageModule>
</cproject>

3
autonomous_robotics_ros/src/turtlebot/turtlebot_apps/turtlebot_calibration/.project → autonomous_robotics_ros/src/kobuki_node/.project Executable file → Normal file
View File

@@ -1,8 +1,9 @@
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>turtlebot_calibration</name>
<name>kobuki_node</name>
<comment></comment>
<projects>
<project>kobuki_driver</project>
</projects>
<buildSpec>
<buildCommand>

0
autonomous_robotics_ros/src/turtlebot/turtlebot/turtlebot_bringup/.pydevproject → autonomous_robotics_ros/src/kobuki_node/.pydevproject Executable file → Normal file
View File

88
autonomous_robotics_ros/src/kobuki_node/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,88 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package kobuki_node
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.7.6 (2018-03-14)
------------------
* Use more finite values for initial odometry covariance (i.e. not DBL_MAX) so as not to cause problems upstream
0.6.6 (2015-05-27)
------------------
* install image directory closes `#357 <https://github.com/yujinrobot/kobuki/issues/357>`_
* Contributors: Jihoon Lee
0.6.5 (2014-11-21)
------------------
* update diagnostics.yaml to show kobuki battery properly in kobuki dashboard. Fix `#350 <https://github.com/yujinrobot/kobuki/issues/350>`_
* Contributors: Jihoon Lee
0.6.4 (2014-08-26)
------------------
* Merge branch 'indigo' of https://github.com/yujinrobot/kobuki into indigo
* rename run_depend of kobuki_node from kobuki_apps to kobuki_rapps
* Contributors: Jihoon Lee
0.6.3 (2014-08-25)
------------------
0.6.2 (2014-08-11)
------------------
* add queue_size on publiehrs `#338 <https://github.com/yujinrobot/kobuki/issues/338>`_
* fixing the file link resolves `#339 <https://github.com/yujinrobot/kobuki/issues/339>`_
* Contributors: Jihoon Lee
0.6.1 (2014-08-08)
------------------
0.6.0 (2014-08-08)
------------------
* remove kobuki_capabilities dependency from package.xml remove author emails
* moves app manager launcher to kobuki_capabilities (solves `#331 <https://github.com/yujinrobot/kobuki/issues/331>`_)
* kobuki_node: adds shutdown flag to nodelet (fixes `#324 <https://github.com/yujinrobot/kobuki/issues/324>`_)
* fixes typo
* updates icons for apps and app manager launcher
* adds minor changes due to capability server and app manager updates
* updates for new rapp lists
* publish_tf arg for the launcher.
* removes rviz launcher and dependency (fixes `#315 <https://github.com/yujinrobot/kobuki/issues/315>`_)
* adds app manager and capability server launcher for kobuki
* Add missing run dependency on yocs_cmd_vel_mux
* Contributors: Daniel Stonier, Jihoon Lee, Jorge Santos, Marcus Liebhardt
0.5.5 (2013-10-11)
------------------
* Add ftdi dependency.
0.5.4 (2013-09-06)
------------------
0.5.3 (2013-08-30)
------------------
* adds view robot launcher.
0.5.0 (2013-08-29)
------------------
* Added new interface about custom PID gain setting.
* Added extra url info on all packages.
* Updated old rnd email address.
* lock api for protecting data access with asynchronous getXXX calls.
* Fix URL to the previous changelog wiki.
* Changelogs at package level.
* Reset odometry also for heading.
* Set use_imu_heading as true (default).
* Do not use robot_pose_ekf. Instead, use imu for heading and encoders por x and y. However, parameter use_imu_heading makes trivial to switch back to the previous system.
* Added a debug topic that publish actual base command sent to robot.
* Added flexible logging features to using named logging system of ros/log4cxx.
* Reset odometry also works for heading (gyro).
* Do not use robot_pose_ekf; use imu for heading and encoders for position.
0.4.0 (2013-08-09)
------------------
* Change deprecated state_publisher to robot_state_publisher in launch files.
* Add launch file for full tf.
Previous versions, bugfixing
============================
Available in ROS wiki: http://ros.org/wiki/kobuki/ChangeList

39
autonomous_robotics_ros/src/kobuki_node/CMakeLists.txt Normal file
View File

@@ -0,0 +1,39 @@
cmake_minimum_required(VERSION 2.8.3)
project(kobuki_node)
find_package(catkin REQUIRED COMPONENTS rospy roscpp nodelet pluginlib tf angles
geometry_msgs sensor_msgs nav_msgs std_msgs diagnostic_updater diagnostic_msgs
kobuki_msgs kobuki_driver kobuki_keyop kobuki_safety_controller
ecl_exceptions ecl_sigslots ecl_streams ecl_threads)
catkin_package(
INCLUDE_DIRS include
LIBRARIES kobuki_ros kobuki_nodelet
CATKIN_DEPENDS rospy roscpp nodelet pluginlib tf angles
geometry_msgs sensor_msgs nav_msgs std_msgs diagnostic_updater diagnostic_msgs
kobuki_msgs kobuki_driver kobuki_keyop kobuki_safety_controller
ecl_exceptions ecl_sigslots ecl_streams ecl_threads
)
include_directories(include ${catkin_INCLUDE_DIRS})
add_subdirectory(src)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(DIRECTORY plugins
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY param
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY image
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(PROGRAMS scripts/getOdom2D.py
scripts/getYaw.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

32
autonomous_robotics_ros/src/kobuki_node/LICENSE Normal file
View File

@@ -0,0 +1,32 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2012 Yujin Robot, Daniel Stonier, Jorge Santos,
# Marcus Liebhardt, Younghun Ju, Jihoon Lee
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Yujin Robot nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

BIN
autonomous_robotics_ros/src/kobuki_node/image/kobuki_icon.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 3.4 KiB

173
autonomous_robotics_ros/src/kobuki_node/include/kobuki_node/diagnostics.hpp Normal file
View File

@@ -0,0 +1,173 @@
/**
* @file /kobuki_node/include/kobuki_node/diagnostics.hpp
*
* @brief Diagnostics for the kobuki node.
*
* License: BSD
* https://raw.github.com/yujinrobot/kobuki/hydro-devel/kobuki_node/LICENSE
**/
/*****************************************************************************
** Ifdefs
*****************************************************************************/
#ifndef KOBUKI_NODE_DIAGNOSTICS_HPP_
#define KOBUKI_NODE_DIAGNOSTICS_HPP_
/*****************************************************************************
** Includes
*****************************************************************************/
#include <kobuki_driver/packets/cliff.hpp>
#include <kobuki_driver/modules/battery.hpp>
#include <kobuki_driver/packets/core_sensors.hpp>
#include <diagnostic_updater/diagnostic_updater.h>
/*****************************************************************************
** Namespaces
*****************************************************************************/
namespace kobuki {
/*****************************************************************************
** Interfaces
*****************************************************************************/
/**
* Diagnostic checking the robot battery and charging status.
*/
class BatteryTask : public diagnostic_updater::DiagnosticTask {
public:
BatteryTask() : DiagnosticTask("Battery") {}
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
void update(const Battery &battery) { status = battery; }
private:
Battery status;
};
/**
* Simple diagnostic checking to see if kobuki is streaming data or not.
*/
class WatchdogTask : public diagnostic_updater::DiagnosticTask {
public:
WatchdogTask() : DiagnosticTask("Watchdog"), alive(false) {}
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
void update(const bool &is_alive) { alive = is_alive; }
bool isAlive() const { return alive; }
private:
bool alive;
};
/**
* Diagnostic checking the cliff sensors status.
*/
class CliffSensorTask : public diagnostic_updater::DiagnosticTask {
public:
CliffSensorTask() : DiagnosticTask("Cliff Sensor") {}
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
void update(const uint8_t &new_status, const Cliff::Data &new_values) {
status = new_status; values = new_values;
}
private:
uint8_t status;
Cliff::Data values;
};
/**
* Diagnostic checking the wall sensors (aka bumpers) status.
*/
class WallSensorTask : public diagnostic_updater::DiagnosticTask {
public:
WallSensorTask() : DiagnosticTask("Wall Sensor") {}
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
void update(const uint8_t &new_status) { status = new_status; }
private:
uint8_t status;
};
/**
* Diagnostic checking whether the wheels stay in contact with the ground.
*/
class WheelDropTask : public diagnostic_updater::DiagnosticTask {
public:
WheelDropTask() : DiagnosticTask("Wheel Drop") {}
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
void update(const uint8_t &new_status) { status = new_status; }
private:
uint8_t status;
};
/**
* Diagnostic checking the current supplied to the motors, what
* can be useful for detecting whether the robot is blocked.
*/
class MotorCurrentTask : public diagnostic_updater::DiagnosticTask {
public:
MotorCurrentTask() : DiagnosticTask("Motor Current") {}
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
void update(const std::vector<uint8_t> &new_values) { values = new_values; }
private:
std::vector<uint8_t> values;
};
/**
* Diagnostic checking the on/off state of the motors
*/
class MotorStateTask : public diagnostic_updater::DiagnosticTask {
public:
MotorStateTask() : DiagnosticTask("Motor State") {}
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
void update(bool new_state) { state = new_state; };
private:
bool state;
};
/**
* Diagnostic checking the gyro sensor status.
*/
class GyroSensorTask : public diagnostic_updater::DiagnosticTask {
public:
GyroSensorTask() : DiagnosticTask("Gyro Sensor") {}
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
void update(int16_t new_heading) { heading = new_heading; }
private:
int16_t heading;
};
/**
* Diagnostic checking the state of the digital input port (four bits).
*/
class DigitalInputTask : public diagnostic_updater::DiagnosticTask {
public:
DigitalInputTask() : DiagnosticTask("Digital Input") {}
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
void update(uint16_t new_status) { status = new_status; }
private:
uint16_t status;
};
/**
* Diagnostic checking the state of the analog input port (four short integers).
*/
class AnalogInputTask : public diagnostic_updater::DiagnosticTask {
public:
AnalogInputTask() : DiagnosticTask("Analog Input") {}
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
void update(const std::vector<uint16_t> &new_values) { values = new_values; }
private:
std::vector<uint16_t> values;
};
} // namespace kobuki
#endif /* KOBUKI_NODE_DIAGNOSTICS_HPP_ */

219
autonomous_robotics_ros/src/kobuki_node/include/kobuki_node/kobuki_ros.hpp Normal file
View File

@@ -0,0 +1,219 @@
/*
* Copyright (c) 2012, Yujin Robot.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Yujin Robot nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file /kobuki_node/include/kobuki_node/kobuki_ros.hpp
*
* @brief Wraps the kobuki driver in a ROS-specific library
*
**/
/*****************************************************************************
** Ifdefs
*****************************************************************************/
#ifndef KOBUKI_ROS_HPP_
#define KOBUKI_ROS_HPP_
/*****************************************************************************
** Includes
*****************************************************************************/
#include <string>
#include <boost/shared_ptr.hpp>
#include <ros/ros.h>
#include <angles/angles.h>
#include <std_msgs/Empty.h>
#include <std_msgs/String.h>
#include <std_msgs/Int16MultiArray.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/Imu.h>
#include <ecl/sigslots.hpp>
#include <kobuki_msgs/ButtonEvent.h>
#include <kobuki_msgs/BumperEvent.h>
#include <kobuki_msgs/CliffEvent.h>
#include <kobuki_msgs/ControllerInfo.h>
#include <kobuki_msgs/DigitalOutput.h>
#include <kobuki_msgs/DigitalInputEvent.h>
#include <kobuki_msgs/ExternalPower.h>
#include <kobuki_msgs/DockInfraRed.h>
#include <kobuki_msgs/Led.h>
#include <kobuki_msgs/MotorPower.h>
#include <kobuki_msgs/PowerSystemEvent.h>
#include <kobuki_msgs/RobotStateEvent.h>
#include <kobuki_msgs/SensorState.h>
#include <kobuki_msgs/Sound.h>
#include <kobuki_msgs/VersionInfo.h>
#include <kobuki_msgs/WheelDropEvent.h>
#include <kobuki_driver/kobuki.hpp>
#include "diagnostics.hpp"
#include "odometry.hpp"
/*****************************************************************************
** Namespaces
*****************************************************************************/
namespace kobuki
{
class KobukiRos
{
public:
KobukiRos(std::string& node_name);
~KobukiRos();
bool init(ros::NodeHandle& nh, ros::NodeHandle& nh_pub);
bool update();
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
/*********************
** Variables
**********************/
std::string name; // name of the ROS node
Kobuki kobuki;
sensor_msgs::JointState joint_states;
Odometry odometry;
bool cmd_vel_timed_out_; // stops warning spam when cmd_vel flags as timed out more than once in a row
bool serial_timed_out_; // stops warning spam when serial connection timed out more than once in a row
/*********************
** Ros Comms
**********************/
ros::Publisher version_info_publisher, controller_info_publisher;
ros::Publisher imu_data_publisher, sensor_state_publisher, joint_state_publisher, dock_ir_publisher, raw_imu_data_publisher;
ros::Publisher button_event_publisher, input_event_publisher, robot_event_publisher;
ros::Publisher bumper_event_publisher, cliff_event_publisher, wheel_event_publisher, power_event_publisher;
ros::Publisher raw_data_command_publisher, raw_data_stream_publisher, raw_control_command_publisher;
ros::Subscriber velocity_command_subscriber, digital_output_command_subscriber, external_power_command_subscriber;
ros::Subscriber controller_info_command_subscriber;
ros::Subscriber led1_command_subscriber, led2_command_subscriber, sound_command_subscriber;
ros::Subscriber motor_power_subscriber, reset_odometry_subscriber;
void advertiseTopics(ros::NodeHandle& nh);
void subscribeTopics(ros::NodeHandle& nh);
/*********************
** Ros Callbacks
**********************/
void subscribeVelocityCommand(const geometry_msgs::TwistConstPtr);
void subscribeLed1Command(const kobuki_msgs::LedConstPtr);
void subscribeLed2Command(const kobuki_msgs::LedConstPtr);
void subscribeDigitalOutputCommand(const kobuki_msgs::DigitalOutputConstPtr);
void subscribeExternalPowerCommand(const kobuki_msgs::ExternalPowerConstPtr);
void subscribeResetOdometry(const std_msgs::EmptyConstPtr);
void subscribeSoundCommand(const kobuki_msgs::SoundConstPtr);
void subscribeMotorPower(const kobuki_msgs::MotorPowerConstPtr msg);
void subscribeControllerInfoCommand(const kobuki_msgs::ControllerInfoConstPtr msg);
/*********************
** SigSlots
**********************/
ecl::Slot<const VersionInfo&> slot_version_info;
ecl::Slot<> slot_stream_data;
ecl::Slot<> slot_controller_info;
ecl::Slot<const ButtonEvent&> slot_button_event;
ecl::Slot<const BumperEvent&> slot_bumper_event;
ecl::Slot<const CliffEvent&> slot_cliff_event;
ecl::Slot<const WheelEvent&> slot_wheel_event;
ecl::Slot<const PowerEvent&> slot_power_event;
ecl::Slot<const InputEvent&> slot_input_event;
ecl::Slot<const RobotEvent&> slot_robot_event;
ecl::Slot<const std::string&> slot_debug, slot_info, slot_warn, slot_error;
ecl::Slot<const std::vector<std::string>&> slot_named;
ecl::Slot<Command::Buffer&> slot_raw_data_command;
ecl::Slot<PacketFinder::BufferType&> slot_raw_data_stream;
ecl::Slot<const std::vector<short>&> slot_raw_control_command;
/*********************
** Slot Callbacks
**********************/
void processStreamData();
void publishWheelState();
void publishInertia();
void publishRawInertia();
void publishSensorState();
void publishDockIRData();
void publishVersionInfo(const VersionInfo &version_info);
void publishControllerInfo();
void publishButtonEvent(const ButtonEvent &event);
void publishBumperEvent(const BumperEvent &event);
void publishCliffEvent(const CliffEvent &event);
void publishWheelEvent(const WheelEvent &event);
void publishPowerEvent(const PowerEvent &event);
void publishInputEvent(const InputEvent &event);
void publishRobotEvent(const RobotEvent &event);
// debugging
void rosDebug(const std::string &msg) { ROS_DEBUG_STREAM("Kobuki : " << msg); }
void rosInfo(const std::string &msg) { ROS_INFO_STREAM("Kobuki : " << msg); }
void rosWarn(const std::string &msg) { ROS_WARN_STREAM("Kobuki : " << msg); }
void rosError(const std::string &msg) { ROS_ERROR_STREAM("Kobuki : " << msg); }
void rosNamed(const std::vector<std::string> &msgs) {
if (msgs.size()==0) return;
if (msgs.size()==1) { ROS_INFO_STREAM("Kobuki : " << msgs[0]); }
if (msgs.size()==2) {
if (msgs[0] == "debug") { ROS_DEBUG_STREAM("Kobuki : " << msgs[1]); }
else if (msgs[0] == "info" ) { ROS_INFO_STREAM ("Kobuki : " << msgs[1]); }
else if (msgs[0] == "warn" ) { ROS_WARN_STREAM ("Kobuki : " << msgs[1]); }
else if (msgs[0] == "error") { ROS_ERROR_STREAM("Kobuki : " << msgs[1]); }
else if (msgs[0] == "fatal") { ROS_FATAL_STREAM("Kobuki : " << msgs[1]); }
}
if (msgs.size()==3) {
if (msgs[0] == "debug") { ROS_DEBUG_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); }
else if (msgs[0] == "info" ) { ROS_INFO_STREAM_NAMED (msgs[1], "Kobuki : " << msgs[2]); }
else if (msgs[0] == "warn" ) { ROS_WARN_STREAM_NAMED (msgs[1], "Kobuki : " << msgs[2]); }
else if (msgs[0] == "error") { ROS_ERROR_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); }
else if (msgs[0] == "fatal") { ROS_FATAL_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); }
}
}
void publishRawDataCommand(Command::Buffer &buffer);
void publishRawDataStream(PacketFinder::BufferType &buffer);
void publishRawControlCommand(const std::vector<short> &velocity_commands);
/*********************
** Diagnostics
**********************/
diagnostic_updater::Updater updater;
BatteryTask battery_diagnostics;
WatchdogTask watchdog_diagnostics;
CliffSensorTask cliff_diagnostics;
WallSensorTask bumper_diagnostics;
WheelDropTask wheel_diagnostics;
MotorCurrentTask motor_diagnostics;
MotorStateTask state_diagnostics;
GyroSensorTask gyro_diagnostics;
DigitalInputTask dinput_diagnostics;
AnalogInputTask ainput_diagnostics;
};
} // namespace kobuki
#endif /* KOBUKI_ROS_HPP_ */

68
autonomous_robotics_ros/src/kobuki_node/include/kobuki_node/odometry.hpp Normal file
View File

@@ -0,0 +1,68 @@
/**
* @file /kobuki_node/include/kobuki_node/odometry.hpp
*
* @brief File comment
*
* File comment
*
**/
/*****************************************************************************
** Ifdefs
*****************************************************************************/
#ifndef KOBUKI_NODE_ODOMETRY_HPP_
#define KOBUKI_NODE_ODOMETRY_HPP_
/*****************************************************************************
** Includes
*****************************************************************************/
#include <string>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <ecl/geometry/legacy_pose2d.hpp>
/*****************************************************************************
** Namespaces
*****************************************************************************/
namespace kobuki {
/*****************************************************************************
** Interfaces
*****************************************************************************/
/**
* @brief Odometry for the kobuki node.
**/
class Odometry {
public:
Odometry();
void init(ros::NodeHandle& nh, const std::string& name);
bool commandTimeout() const;
void update(const ecl::LegacyPose2D<double> &pose_update, ecl::linear_algebra::Vector3d &pose_update_rates,
double imu_heading, double imu_angular_velocity);
void resetOdometry() { pose.setIdentity(); }
const ros::Duration& timeout() const { return cmd_vel_timeout; }
void resetTimeout() { last_cmd_time = ros::Time::now(); }
private:
geometry_msgs::TransformStamped odom_trans;
ecl::LegacyPose2D<double> pose;
std::string odom_frame;
std::string base_frame;
ros::Duration cmd_vel_timeout;
ros::Time last_cmd_time;
bool publish_tf;
bool use_imu_heading;
tf::TransformBroadcaster odom_broadcaster;
ros::Publisher odom_publisher;
void publishTransform(const geometry_msgs::Quaternion &odom_quat);
void publishOdometry(const geometry_msgs::Quaternion &odom_quat, const ecl::linear_algebra::Vector3d &pose_update_rates);
};
} // namespace kobuki
#endif /* KOBUKI_NODE_ODOMETRY_HPP_ */

9
autonomous_robotics_ros/src/kobuki_node/launch/debugging.txt Normal file
View File

@@ -0,0 +1,9 @@
== Debugging ==
To run gdb on any node in the launch file, simply insert
launch-prefix="gdb --args"
before any of the node calls or nodelet manager/standalone calls. For example:
<node launch-prefix="gdb --args" pkg="nodelet" type="nodelet" name="robot_core" ns="robot_core" args="manager" output="screen"/>

18
autonomous_robotics_ros/src/kobuki_node/launch/minimal.launch Normal file
View File

@@ -0,0 +1,18 @@
<!--
Standalone launcher for running a default Kobuki
-->
<launch>
<arg name="kobuki_publish_tf" default="true"/> <!-- Publish base_footprint - odom transforms (usually good thing to have for localisation) -->
<node pkg="nodelet" type="nodelet" name="mobile_base_nodelet_manager" args="manager"/>
<node pkg="nodelet" type="nodelet" name="mobile_base" args="load kobuki_node/KobukiNodelet mobile_base_nodelet_manager">
<rosparam file="$(find kobuki_node)/param/base.yaml" command="load"/>
<param name="publish_tf" value="$(arg kobuki_publish_tf)"/>
<remap from="mobile_base/odom" to="odom"/>
<remap from="mobile_base/joint_states" to="joint_states"/>
</node>
<node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator" >
<rosparam command="load" file="$(find kobuki_node)/param/diagnostics.yaml" />
</node>
</launch>

24
autonomous_robotics_ros/src/kobuki_node/launch/robot_with_tf.launch Normal file
View File

@@ -0,0 +1,24 @@
<launch>
<arg name="urdf_file" default="$(find xacro)/xacro '$(find kobuki_description)/urdf/kobuki_standalone.urdf.xacro'"/>
<param name="robot_description" command="$(arg urdf_file)"/>
<node pkg="nodelet" type="nodelet" name="mobile_base_nodelet_manager" args="manager"/>
<node pkg="nodelet" type="nodelet" name="mobile_base" args="load kobuki_node/KobukiNodelet mobile_base_nodelet_manager">
<rosparam file="$(find kobuki_node)/param/base.yaml" command="load"/>
<remap from="mobile_base/odom" to="odom"/>
<!-- Don't do this - force applications to use a velocity mux for redirection
<remap from="mobile_base/commands/velocity" to="cmd_vel"/>
-->
<remap from="mobile_base/enable" to="enable"/>
<remap from="mobile_base/disable" to="disable"/>
<remap from="mobile_base/joint_states" to="joint_states"/>
</node>
<node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator">
<rosparam command="load" file="$(find kobuki_node)/param/diagnostics.yaml"/>
</node>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="30.0"/>
</node>
</launch>

6
autonomous_robotics_ros/src/kobuki_node/launch/test_get_odom.launch Normal file
View File

@@ -0,0 +1,6 @@
<launch>
<node pkg="kobuki_node" type="getOdom2D.py" name="getOdom2D">
<remap from="odom" to="/odom"/>
<remap from="pose2d" to="/pose2d"/>
</node>
</launch>

7
autonomous_robotics_ros/src/kobuki_node/launch/test_get_yaw.launch Normal file
View File

@@ -0,0 +1,7 @@
<launch>
<node pkg="kobuki_node" type="getYaw.py" name="getYaw">
<remap from="imu" to="/mobile_base/sensors/imu_data"/>
<remap from="angle" to="/mobile_base/sensors/gyro/angle"/>
<remap from="angle_rate" to="/mobile_base/sensors/gyro/angle_rate"/>
</node>
</launch>

83
autonomous_robotics_ros/src/kobuki_node/package.xml Normal file
View File

@@ -0,0 +1,83 @@
<package>
<name>kobuki_node</name>
<version>0.7.6</version>
<description>
ROS nodelet for Kobuki: ROS wrapper for the Kobuki driver.
</description>
<author>Daniel Stonier</author>
<author>Younghun Ju</author>
<author>Jorge Santos Simon</author>
<maintainer email="d.stonier@gmail.com">Daniel Stonier</maintainer>
<license>BSD</license>
<url type="bugtracker">https://github.com/yujinrobot/kobuki/issues</url>
<url type="repository">https://github.com/yujinrobot/kobuki</url>
<url type="website">http://ros.org/wiki/kobuki_node</url>
<buildtool_depend>catkin</buildtool_depend>
<!-- ROS -->
<build_depend>roscpp</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>angles</build_depend>
<build_depend>diagnostic_updater</build_depend>
<build_depend>diagnostic_msgs</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>pluginlib</build_depend>
<!-- Kobuki -->
<build_depend>kobuki_msgs</build_depend>
<build_depend>kobuki_driver</build_depend>
<build_depend>kobuki_ftdi</build_depend>
<build_depend>kobuki_keyop</build_depend>
<build_depend>kobuki_safety_controller</build_depend>
<!-- ECL -->
<build_depend>ecl_exceptions</build_depend>
<build_depend>ecl_sigslots</build_depend>
<build_depend>ecl_streams</build_depend>
<build_depend>ecl_threads</build_depend>
<!-- For Tests -->
<build_depend>rospy</build_depend>
<!-- ROS -->
<run_depend>capabilities</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>tf</run_depend>
<run_depend>angles</run_depend>
<run_depend>diagnostic_aggregator</run_depend>
<run_depend>diagnostic_updater</run_depend>
<run_depend>diagnostic_msgs</run_depend>
<run_depend>nodelet</run_depend>
<run_depend>pluginlib</run_depend>
<!-- Kobuki -->
<run_depend>kobuki_rapps</run_depend>
<run_depend>kobuki_msgs</run_depend>
<run_depend>kobuki_driver</run_depend>
<run_depend>kobuki_ftdi</run_depend>
<run_depend>kobuki_keyop</run_depend>
<run_depend>kobuki_safety_controller</run_depend>
<!-- ECL -->
<run_depend>ecl_exceptions</run_depend>
<run_depend>ecl_sigslots</run_depend>
<run_depend>ecl_streams</run_depend>
<run_depend>ecl_threads</run_depend>
<!-- For Tests -->
<run_depend>rospy</run_depend>
<export>
<nodelet plugin="${prefix}/plugins/nodelet_plugins.xml" />
</export>
</package>

36
autonomous_robotics_ros/src/kobuki_node/param/base.yaml Normal file
View File

@@ -0,0 +1,36 @@
##############################################################################
# Firmware Source
##############################################################################
device_port: /dev/kobuki
# published joint states
wheel_left_joint_name: wheel_left_joint
wheel_right_joint_name: wheel_right_joint
# battery voltage at full charge (100%) (float, default: 16.5)
battery_capacity: 16.5
# battery voltage at first warning (15%) (float, default: 13.5)
battery_low: 14.0
# battery voltage at critical level (5%) (float, default: 13.2)
battery_dangerous: 13.2
# If a new command isn't received within this many seconds, the base is stopped (double, default: 0.6)
cmd_vel_timeout: 0.6
# Causes node to publish TF for odom_frame to base_frame. Disable only if you plan to use robot_pose_ekf
# (see use_imu_heading description) (bool, default: true)
publish_tf: true
# Use imu readings for heading instead of encoders. That's the normal operation mode for Kobuki, as its
# gyro is very reliable. Disable only if you want to fuse encoders and imu readings in a more sophisticated
# way, for example filtering and fussing with robot_pose_ekf (bool, default: true)
use_imu_heading: true
# Name of the odometry TF frame (string, default: odom)
odom_frame: odom
# Name of the base TF frame (string, default: base_footprint)
base_frame: base_footprint

38
autonomous_robotics_ros/src/kobuki_node/param/cmd_vel_mux_minimal.yaml Normal file
View File

@@ -0,0 +1,38 @@
# Created on: Oct 29, 2012
# Author: jorge
# Configuration for subscribers to cmd_vel sources.
#
# Individual subscriber configuration:
# name: Source name
# topic: The topic that provides cmd_vel messages
# timeout: Time in seconds without incoming messages to consider this topic inactive
# priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT
# short_desc: Short description (optional)
subscribers:
- name: "Default input"
topic: "cmd_vel"
timeout: 0.1
priority: 0
short_desc: "Default velocity topic; controllers unaware that we are multiplexing cmd_vel will come here"
- name: "Navigation stack"
topic: "nav_cmd_vel"
timeout: 0.5
priority: 1
short_desc: "ROS navigation stack controller"
- name: "Onboard joystick"
topic: "joy_cmd_vel"
timeout: 0.1
priority: 10
- name: "Remote control"
topic: "rem_cmd_vel"
timeout: 0.1
priority: 9
- name: "Web application"
topic: "web_cmd_vel"
timeout: 0.3
priority: 8
- name: "Keyboard operation"
topic: "key_cmd_vel"
timeout: 0.1
priority: 7

23
autonomous_robotics_ros/src/kobuki_node/param/cmd_vel_mux_safe.yaml Normal file
View File

@@ -0,0 +1,23 @@
# Created on: Oct 29, 2012
# Author: jorge
# Configuration for subscribers to cmd_vel sources. This file is provided just as an example.
# Typically automatic controllers, as ROS navigation stack should have the minimum priority
#
# Individual subscriber configuration:
# name: Source name
# topic: The topic that provides cmd_vel messages
# timeout: Time in seconds without incoming messages to consider this topic inactive
# priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT
# short_desc: Short description (optional)
subscribers:
- name: "Default input"
topic: "cmd_vel"
timeout: 0.2
priority: 0
short_desc: "Default velocity topic; controllers unaware that we are multiplexing cmd_vel will come here"
- name: "Safety Controller"
topic: "cmd_vel_safety"
timeout: 0.2
priority: 10
short_desc: "Kobuki's safety controller"

37
autonomous_robotics_ros/src/kobuki_node/param/diagnostics.yaml Normal file
View File

@@ -0,0 +1,37 @@
# The remove_prefix is important - it corresponds to the name of
# the node that is automatically prefixed to cpp diagnostic task
# names. In our case it's the mobile base nodelet manager name.
# Android is not expecting this, so we need to remove it.
pub_rate: 1.0 # Optional
base_path: '' # Optional, prepended to all diagnostic output
analyzers:
power:
type: diagnostic_aggregator/GenericAnalyzer
path: 'Power System'
timeout: 5.0
contains: ['Battery']
remove_prefix: mobile_base_nodelet_manager
kobuki:
type: diagnostic_aggregator/GenericAnalyzer
path: 'Kobuki'
timeout: 5.0
contains: ['Watchdog', 'Motor State']
remove_prefix: mobile_base_nodelet_manager
sensors:
type: diagnostic_aggregator/GenericAnalyzer
path: 'Sensors'
timeout: 5.0
contains: ['Cliff Sensor', 'Wall Sensor', 'Wheel Drop', 'Motor Current', 'Gyro Sensor']
remove_prefix: mobile_base_nodelet_manager
input_ports:
type: diagnostic_aggregator/GenericAnalyzer
path: 'Input Ports'
timeout: 5.0
contains: ['Digital Input', 'Analog Input']
remove_prefix: mobile_base_nodelet_manager
# nodes:
# type: diagnostic_aggregator/GenericAnalyzer
# path: 'Nodes'
# timeout: 5.0
# contains: ['Node']

25
autonomous_robotics_ros/src/kobuki_node/param/webop_mux.yaml Normal file
View File

@@ -0,0 +1,25 @@
# Created on: Oct 29, 2012
# Author: jorge
# Configuration for subscribers to cmd_vel sources for kobuki web app operation
# with safety reactive controller.
#
# Individual subscriber configuration:
# name: Source name
# topic: The topic that provides cmd_vel messages
# timeout: Time in seconds without incoming messages to consider this topic inactive
# priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT
# short_desc: Short description (optional)
subscribers:
- name: "Safe reactive controller"
topic: "safety_controller"
timeout: 0.2
priority: 10
- name: "Keyboard operation"
topic: "keyboard_teleop"
timeout: 0.2
priority: 2
- name: "Web application"
topic: "web_app_teleop"
timeout: 0.3
priority: 8

8
autonomous_robotics_ros/src/kobuki_node/plugins/nodelet_plugins.xml Normal file
View File

@@ -0,0 +1,8 @@
<library path="lib/libkobuki_nodelet">
<class name="kobuki_node/KobukiNodelet" type="kobuki::KobukiNodelet" base_class_type="nodelet::Nodelet">
<description>
Nodelet version of the Kobuki node
</description>
</class>
</library>

50
autonomous_robotics_ros/src/kobuki_node/scripts/getOdom2D.py Executable file
View File

@@ -0,0 +1,50 @@
#!/usr/bin/env python
#AUTHOR: Younghun Ju <yhju@yujinrobot.comm>, <yhju83@gmail.com>
import roslib; roslib.load_manifest('kobuki_node')
import rospy
from tf.transformations import euler_from_quaternion
from math import degrees
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Pose2D
class Converter(object):
def __init__(self):
rospy.init_node("getOdom2D", anonymous=True)
self.sub = rospy.Subscriber("odom", Odometry, self.OdomCallback)
self.pub = rospy.Publisher("pose2d", Pose2D, queue_size=10)
def OdomCallback(self,data):
px = data.pose.pose.position.x
py = data.pose.pose.position.y
quat = data.pose.pose.orientation
q = [quat.x, quat.y, quat.z, quat.w]
roll, pitch, yaw = euler_from_quaternion(q)
vx = data.twist.twist.linear.x
vy = data.twist.twist.linear.y
yaw_rate = data.twist.twist.angular.z
print "pose: x: {0:+2.5f}".format(px) + ", y: {0:+2.5f}".format(py)\
+ ", th: {0:+.4f}".format(yaw) + " rad; "\
+ "{0:+.2f}".format(degrees(yaw)) + " deg"
print "rate: x: {0:+2.5f}".format(vx) + ", y: {0:+2.5f}".format(vy)\
+ ", th: {0:+.2f}".format(yaw_rate) + " rad/s; "\
+ "{0:+.2f}".format(degrees(yaw_rate)) + " deg/s"
print '---'
pose2d = Pose2D()
pose2d.x = px
pose2d.y = py
pose2d.theta = yaw
self.pub.publish(pose2d)
if __name__ == '__main__':
try:
instance = Converter()
print
print "It prints x, y, theta values from Odom message of mobile base."
print
rospy.spin()
except rospy.ROSInterruptException: pass

45
autonomous_robotics_ros/src/kobuki_node/scripts/getYaw.py Executable file
View File

@@ -0,0 +1,45 @@
#!/usr/bin/env python
#AUTHOR: Younghun Ju <yhju@yujinrobot.comm>, <yhju83@gmail.com>
import roslib; roslib.load_manifest('kobuki_node')
import rospy
from tf.transformations import euler_from_quaternion
from math import degrees
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Quaternion
from std_msgs.msg import Float64
class Converter(object):
def __init__(self):
rospy.init_node("getYaw", anonymous=True)
self.sub_imu = rospy.Subscriber("imu", Imu, self.ImuCallback)
self.pub_angle = rospy.Publisher("angle", Float64, queue_size=10)
self.pub_angle_rate = rospy.Publisher("angle_rate", Float64, queue_size=10)
def ImuCallback(self,data):
quat = data.orientation
q = [quat.x, quat.y, quat.z, quat.w]
roll, pitch, yaw = euler_from_quaternion(q)
print "angle: " + "{0:+.4f}".format(yaw) + " rad; "\
+ "{0:+.2f}".format(degrees(yaw)) + " deg"
print "rate: " + "{0:+.2f}".format(data.angular_velocity.z) + " rad/s; "\
+ "{0:+.2f}".format(degrees(data.angular_velocity.z)) + " deg/s"
print '---'
angle = Float64()
angle_rate = Float64()
angle.data = yaw
angle_rate.data = data.angular_velocity.z
self.pub_angle.publish(angle)
self.pub_angle_rate.publish(angle_rate)
if __name__ == '__main__':
try:
instance = Converter()
print
print "It prints angle and angular_velocity from Imu message of single yaw gyro."
print
rospy.spin()
except rospy.ROSInterruptException: pass

6
autonomous_robotics_ros/src/kobuki_node/src/CMakeLists.txt Normal file
View File

@@ -0,0 +1,6 @@
###############################################################################
# Subdirectories
###############################################################################
add_subdirectory(library)
add_subdirectory(nodelet)

17
autonomous_robotics_ros/src/kobuki_node/src/library/CMakeLists.txt Normal file
View File

@@ -0,0 +1,17 @@
##############################################################################
# SOURCES
##############################################################################
file(GLOB SOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.cpp)
##############################################################################
# LIBRARY
##############################################################################
add_library(kobuki_ros ${SOURCES})
add_dependencies(kobuki_ros ${catkin_EXPORTED_TARGETS})
target_link_libraries(kobuki_ros ${catkin_LIBRARIES})
install(TARGETS kobuki_ros
DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)

164
autonomous_robotics_ros/src/kobuki_node/src/library/diagnostics.cpp Normal file
View File

@@ -0,0 +1,164 @@
/**
* @file /src/library/diagnostics.cpp
*
* @brief Robot diagnostics implementation.
*
* License: BSD
* https://raw.github.com/yujinrobot/kobuki/hydro-devel/kobuki_node/LICENSE
**/
/*****************************************************************************
** Includes
*****************************************************************************/
#include "../../include/kobuki_node/diagnostics.hpp"
/*****************************************************************************
** Namespaces
*****************************************************************************/
namespace kobuki {
/*****************************************************************************
** Implementation
*****************************************************************************/
void BatteryTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
switch ( status.level() ) {
case ( Battery::Maximum ) : {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Maximum");
break;
}
case ( Battery::Healthy ) : {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Healthy");
break;
}
case ( Battery::Low ) : {
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Low");
break;
}
case ( Battery::Dangerous ) : {
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Dangerous");
break;
}
}
stat.add("Voltage (V)", status.voltage);
stat.add("Percent", status.percent());
stat.add("Charge (Ah)", (2.2*status.percent())/100.0);
stat.add("Capacity (Ah)", 2.2); // TODO: how can we tell which battery is in use?
switch (status.charging_source ) {
case(Battery::None) : {
stat.add("Source", "None");
break;
}
case(Battery::Adapter) : {
stat.add("Source", "Adapter");
break;
}
case(Battery::Dock) : {
stat.add("Source", "Dock");
break;
}
}
switch ( status.charging_state ) {
case ( Battery::Charged ) : {
stat.add("Charging State", "Trickle Charging"); // i.e. fully charged
stat.add("Current (A)", 3.14); // TODO: what's the real value for our charger?
break;
}
case ( Battery::Charging ) : {
stat.add("Charging State", "Full Charging");
stat.add("Current (A)", 3.14); // TODO: what's the real value for our charger?
break;
}
case ( Battery::Discharging ) : {
stat.add("Charging State", "Not Charging");
stat.add("Current (A)", 0.0);
break;
}
default: break;
}
}
void WatchdogTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
if ( alive ) {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Alive");
} else {
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No Signal");
}
}
void CliffSensorTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
if ( status ) {
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Cliff Detected!");
} else {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All right");
}
stat.addf("Left", "Reading: %d Cliff: %s", values.bottom[0], status & CoreSensors::Flags::LeftCliff?"YES":"NO");
stat.addf("Center", "Reading: %d Cliff: %s", values.bottom[1], status & CoreSensors::Flags::CenterCliff?"YES":"NO");
stat.addf("Right", "Reading: %d Cliff: %s", values.bottom[2], status & CoreSensors::Flags::RightCliff?"YES":"NO");
}
void WallSensorTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
if ( status ) {
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Wall Hit!");
} else {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All right");
}
stat.addf("Left", status & CoreSensors::Flags::LeftBumper?"YES":"NO");
stat.addf("Center", status & CoreSensors::Flags::CenterBumper?"YES":"NO");
stat.addf("Right", status & CoreSensors::Flags::RightBumper?"YES":"NO");
}
void WheelDropTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
if ( status ) {
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Wheel Drop!");
} else {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All right");
}
stat.addf("Left", status & CoreSensors::Flags::LeftWheel?"YES":"NO");
stat.addf("Right", status & CoreSensors::Flags::RightWheel?"YES":"NO");
}
void MotorCurrentTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
if ( std::max(values[0], values[1]) > 6 ) { // TODO not sure about this threshold; should be a parameter?
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Is robot stalled? Motors current is very high");
} else {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All right");
}
stat.addf("Left", "%d", values[0]);
stat.addf("Right", "%d", values[1]);
}
void MotorStateTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
if ( state == true ) {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Motors Enabled");
} else {
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Motors Disabled");
}
stat.addf("State", "%d", int(state));
}
void GyroSensorTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
// Raw data angles are in hundredths of degree
stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "Heading: %.2f degrees", heading/100.0);
}
void DigitalInputTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "[%d, %d, %d, %d]",
status & 0x08?1:0, status & 0x04?1:0,
status & 0x02?1:0, status & 0x01?1:0);
}
void AnalogInputTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "[%d, %d, %d, %d]",
values[0], values[1], values[2], values[3]);
}
} // namespace kobuki

350
autonomous_robotics_ros/src/kobuki_node/src/library/kobuki_ros.cpp Normal file
View File

@@ -0,0 +1,350 @@
/*
* Copyright (c) 2012, Yujin Robot.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Yujin Robot nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file /kobuki_node/src/node/kobuki_node.cpp
*
* @brief Implementation for the ros kobuki node wrapper.
**/
/*****************************************************************************
** Includes
*****************************************************************************/
#include <float.h>
#include <tf/tf.h>
#include <ecl/streams/string_stream.hpp>
#include <kobuki_msgs/VersionInfo.h>
#include "kobuki_node/kobuki_ros.hpp"
/*****************************************************************************
** Namespaces
*****************************************************************************/
namespace kobuki
{
/*****************************************************************************
** Implementation [KobukiRos]
*****************************************************************************/
/**
* @brief Default constructor.
*
* Make sure you call the init() method to fully define this node.
*/
KobukiRos::KobukiRos(std::string& node_name) :
name(node_name), cmd_vel_timed_out_(false), serial_timed_out_(false),
slot_version_info(&KobukiRos::publishVersionInfo, *this),
slot_controller_info(&KobukiRos::publishControllerInfo, *this),
slot_stream_data(&KobukiRos::processStreamData, *this),
slot_button_event(&KobukiRos::publishButtonEvent, *this),
slot_bumper_event(&KobukiRos::publishBumperEvent, *this),
slot_cliff_event(&KobukiRos::publishCliffEvent, *this),
slot_wheel_event(&KobukiRos::publishWheelEvent, *this),
slot_power_event(&KobukiRos::publishPowerEvent, *this),
slot_input_event(&KobukiRos::publishInputEvent, *this),
slot_robot_event(&KobukiRos::publishRobotEvent, *this),
slot_debug(&KobukiRos::rosDebug, *this),
slot_info(&KobukiRos::rosInfo, *this),
slot_warn(&KobukiRos::rosWarn, *this),
slot_error(&KobukiRos::rosError, *this),
slot_named(&KobukiRos::rosNamed, *this),
slot_raw_data_command(&KobukiRos::publishRawDataCommand, *this),
slot_raw_data_stream(&KobukiRos::publishRawDataStream, *this),
slot_raw_control_command(&KobukiRos::publishRawControlCommand, *this)
{
updater.setHardwareID("Kobuki");
updater.add(battery_diagnostics);
updater.add(watchdog_diagnostics);
updater.add(bumper_diagnostics);
updater.add(cliff_diagnostics);
updater.add(wheel_diagnostics);
updater.add(motor_diagnostics);
updater.add(state_diagnostics);
updater.add(gyro_diagnostics);
updater.add(dinput_diagnostics);
updater.add(ainput_diagnostics);
}
/**
* This will wait some time while kobuki internally closes its threads and destructs
* itself.
*/
KobukiRos::~KobukiRos()
{
ROS_INFO_STREAM("Kobuki : waiting for kobuki thread to finish [" << name << "].");
}
bool KobukiRos::init(ros::NodeHandle& nh, ros::NodeHandle& nh_pub)
{
/*********************
** Communications
**********************/
advertiseTopics(nh);
subscribeTopics(nh);
/*********************
** Slots
**********************/
slot_stream_data.connect(name + std::string("/stream_data"));
slot_version_info.connect(name + std::string("/version_info"));
slot_controller_info.connect(name + std::string("/controller_info"));
slot_button_event.connect(name + std::string("/button_event"));
slot_bumper_event.connect(name + std::string("/bumper_event"));
slot_cliff_event.connect(name + std::string("/cliff_event"));
slot_wheel_event.connect(name + std::string("/wheel_event"));
slot_power_event.connect(name + std::string("/power_event"));
slot_input_event.connect(name + std::string("/input_event"));
slot_robot_event.connect(name + std::string("/robot_event"));
slot_debug.connect(name + std::string("/ros_debug"));
slot_info.connect(name + std::string("/ros_info"));
slot_warn.connect(name + std::string("/ros_warn"));
slot_error.connect(name + std::string("/ros_error"));
slot_named.connect(name + std::string("/ros_named"));
slot_raw_data_command.connect(name + std::string("/raw_data_command"));
slot_raw_data_stream.connect(name + std::string("/raw_data_stream"));
slot_raw_control_command.connect(name + std::string("/raw_control_command"));
/*********************
** Driver Parameters
**********************/
Parameters parameters;
nh.param("acceleration_limiter", parameters.enable_acceleration_limiter, false);
nh.param("battery_capacity", parameters.battery_capacity, Battery::capacity);
nh.param("battery_low", parameters.battery_low, Battery::low);
nh.param("battery_dangerous", parameters.battery_dangerous, Battery::dangerous);
parameters.sigslots_namespace = name; // name is automatically picked up by device_nodelet parent.
if (!nh.getParam("device_port", parameters.device_port))
{
ROS_ERROR_STREAM("Kobuki : no device port given on the parameter server (e.g. /dev/ttyUSB0)[" << name << "].");
return false;
}
/*********************
** Joint States
**********************/
std::string robot_description, wheel_left_joint_name, wheel_right_joint_name;
nh.param("wheel_left_joint_name", wheel_left_joint_name, std::string("wheel_left_joint"));
nh.param("wheel_right_joint_name", wheel_right_joint_name, std::string("wheel_right_joint"));
// minimalistic check: are joint names present on robot description file?
if (!nh_pub.getParam("robot_description", robot_description))
{
ROS_WARN("Kobuki : no robot description given on the parameter server");
}
else
{
if (robot_description.find(wheel_left_joint_name) == std::string::npos) {
ROS_WARN("Kobuki : joint name %s not found on robot description", wheel_left_joint_name.c_str());
}
if (robot_description.find(wheel_right_joint_name) == std::string::npos) {
ROS_WARN("Kobuki : joint name %s not found on robot description", wheel_right_joint_name.c_str());
}
}
joint_states.name.push_back(wheel_left_joint_name);
joint_states.name.push_back(wheel_right_joint_name);
joint_states.position.resize(2,0.0);
joint_states.velocity.resize(2,0.0);
joint_states.effort.resize(2,0.0);
/*********************
** Validation
**********************/
if (!parameters.validate())
{
ROS_ERROR_STREAM("Kobuki : parameter configuration failed [" << name << "].");
ROS_ERROR_STREAM("Kobuki : " << parameters.error_msg << "[" << name << "]");
return false;
}
else
{
if (parameters.simulation)
{
ROS_INFO("Kobuki : driver going into loopback (simulation) mode.");
}
else
{
ROS_INFO_STREAM("Kobuki : configured for connection on device_port "
<< parameters.device_port << " [" << name << "].");
ROS_INFO_STREAM("Kobuki : driver running in normal (non-simulation) mode" << " [" << name << "].");
}
}
odometry.init(nh, name);
/*********************
** Driver Init
**********************/
try
{
kobuki.init(parameters);
ros::Duration(0.25).sleep(); // wait for some data to come in.
if ( !kobuki.isAlive() ) {
ROS_WARN_STREAM("Kobuki : no data stream, is kobuki turned on?");
// don't need to return false here - simply turning kobuki on while spin()'ing should resurrect the situation.
}
kobuki.enable();
}
catch (const ecl::StandardException &e)
{
switch (e.flag())
{
case (ecl::OpenError):
{
ROS_ERROR_STREAM("Kobuki : could not open connection [" << parameters.device_port << "][" << name << "].");
break;
}
default:
{
ROS_ERROR_STREAM("Kobuki : initialisation failed [" << name << "].");
ROS_DEBUG_STREAM(e.what());
break;
}
}
return false;
}
// kobuki.printSigSlotConnections();
return true;
}
/**
* This is a worker function that runs in a background thread initiated by
* the nodelet. It gathers diagnostics information from the kobuki driver,
* and broadcasts the results to the rest of the ros ecosystem.
*
* Note that the actual driver data is collected via the slot callbacks in this class.
*
* @return Bool : true/false if successfully updated or not (kobuki driver shutdown).
*/
bool KobukiRos::update()
{
if ( kobuki.isShutdown() )
{
ROS_ERROR_STREAM("Kobuki : Driver has been shutdown. Stopping update loop. [" << name << "].");
return false;
}
if ( (kobuki.isEnabled() == true) && odometry.commandTimeout())
{
if ( !cmd_vel_timed_out_ )
{
kobuki.setBaseControl(0, 0);
cmd_vel_timed_out_ = true;
ROS_WARN("Kobuki : Incoming velocity commands not received for more than %.2f seconds -> zero'ing velocity commands", odometry.timeout().toSec());
}
}
else
{
cmd_vel_timed_out_ = false;
}
bool is_alive = kobuki.isAlive();
if ( watchdog_diagnostics.isAlive() && !is_alive )
{
if ( !serial_timed_out_ )
{
ROS_ERROR_STREAM("Kobuki : Timed out while waiting for serial data stream [" << name << "].");
serial_timed_out_ = true;
}
else
{
serial_timed_out_ = false;
}
}
watchdog_diagnostics.update(is_alive);
battery_diagnostics.update(kobuki.batteryStatus());
cliff_diagnostics.update(kobuki.getCoreSensorData().cliff, kobuki.getCliffData());
bumper_diagnostics.update(kobuki.getCoreSensorData().bumper);
wheel_diagnostics.update(kobuki.getCoreSensorData().wheel_drop);
motor_diagnostics.update(kobuki.getCurrentData().current);
state_diagnostics.update(kobuki.isEnabled());
gyro_diagnostics.update(kobuki.getInertiaData().angle);
dinput_diagnostics.update(kobuki.getGpInputData().digital_input);
ainput_diagnostics.update(kobuki.getGpInputData().analog_input);
updater.update();
return true;
}
/**
* Two groups of publishers, one required by turtlebot, the other for
* kobuki esoterics.
*/
void KobukiRos::advertiseTopics(ros::NodeHandle& nh)
{
/*********************
** Turtlebot Required
**********************/
joint_state_publisher = nh.advertise <sensor_msgs::JointState>("joint_states",100);
/*********************
** Kobuki Esoterics
**********************/
version_info_publisher = nh.advertise < kobuki_msgs::VersionInfo > ("version_info", 100, true); // latched publisher
controller_info_publisher = nh.advertise < kobuki_msgs::ControllerInfo > ("controller_info", 100, true); // latched publisher
button_event_publisher = nh.advertise < kobuki_msgs::ButtonEvent > ("events/button", 100);
bumper_event_publisher = nh.advertise < kobuki_msgs::BumperEvent > ("events/bumper", 100);
cliff_event_publisher = nh.advertise < kobuki_msgs::CliffEvent > ("events/cliff", 100);
wheel_event_publisher = nh.advertise < kobuki_msgs::WheelDropEvent > ("events/wheel_drop", 100);
power_event_publisher = nh.advertise < kobuki_msgs::PowerSystemEvent > ("events/power_system", 100);
input_event_publisher = nh.advertise < kobuki_msgs::DigitalInputEvent > ("events/digital_input", 100);
robot_event_publisher = nh.advertise < kobuki_msgs::RobotStateEvent > ("events/robot_state", 100, true); // also latched
sensor_state_publisher = nh.advertise < kobuki_msgs::SensorState > ("sensors/core", 100);
dock_ir_publisher = nh.advertise < kobuki_msgs::DockInfraRed > ("sensors/dock_ir", 100);
imu_data_publisher = nh.advertise < sensor_msgs::Imu > ("sensors/imu_data", 100);
raw_imu_data_publisher = nh.advertise < sensor_msgs::Imu > ("sensors/imu_data_raw", 100);
raw_data_command_publisher = nh.advertise< std_msgs::String > ("debug/raw_data_command", 100);
raw_data_stream_publisher = nh.advertise< std_msgs::String > ("debug/raw_data_stream", 100);
raw_control_command_publisher = nh.advertise< std_msgs::Int16MultiArray > ("debug/raw_control_command", 100);
}
/**
* Two groups of subscribers, one required by turtlebot, the other for
* kobuki esoterics.
*/
void KobukiRos::subscribeTopics(ros::NodeHandle& nh)
{
velocity_command_subscriber = nh.subscribe(std::string("commands/velocity"), 10, &KobukiRos::subscribeVelocityCommand, this);
led1_command_subscriber = nh.subscribe(std::string("commands/led1"), 10, &KobukiRos::subscribeLed1Command, this);
led2_command_subscriber = nh.subscribe(std::string("commands/led2"), 10, &KobukiRos::subscribeLed2Command, this);
digital_output_command_subscriber = nh.subscribe(std::string("commands/digital_output"), 10, &KobukiRos::subscribeDigitalOutputCommand, this);
external_power_command_subscriber = nh.subscribe(std::string("commands/external_power"), 10, &KobukiRos::subscribeExternalPowerCommand, this);
sound_command_subscriber = nh.subscribe(std::string("commands/sound"), 10, &KobukiRos::subscribeSoundCommand, this);
reset_odometry_subscriber = nh.subscribe("commands/reset_odometry", 10, &KobukiRos::subscribeResetOdometry, this);
motor_power_subscriber = nh.subscribe("commands/motor_power", 10, &KobukiRos::subscribeMotorPower, this);
controller_info_command_subscriber = nh.subscribe(std::string("commands/controller_info"), 10, &KobukiRos::subscribeControllerInfoCommand, this);
}
} // namespace kobuki

159
autonomous_robotics_ros/src/kobuki_node/src/library/odometry.cpp Normal file
View File

@@ -0,0 +1,159 @@
/**
* @file /kobuki_node/src/node/odometry.cpp
*
* @brief File comment
*
* File comment
*
**/
/*****************************************************************************
** Includes
*****************************************************************************/
#include "../../include/kobuki_node/odometry.hpp"
/*****************************************************************************
** Namespaces
*****************************************************************************/
namespace kobuki {
/*****************************************************************************
** Implementation
*****************************************************************************/
Odometry::Odometry () :
odom_frame("odom"),
base_frame("base_footprint"),
use_imu_heading(true),
publish_tf(true)
{};
void Odometry::init(ros::NodeHandle& nh, const std::string& name) {
double timeout;
nh.param("cmd_vel_timeout", timeout, 0.6);
cmd_vel_timeout.fromSec(timeout);
ROS_INFO_STREAM("Kobuki : Velocity commands timeout: " << cmd_vel_timeout << " seconds [" << name << "].");
if (!nh.getParam("odom_frame", odom_frame)) {
ROS_WARN_STREAM("Kobuki : no param server setting for odom_frame, using default [" << odom_frame << "][" << name << "].");
} else {
ROS_INFO_STREAM("Kobuki : using odom_frame [" << odom_frame << "][" << name << "].");
}
if (!nh.getParam("base_frame", base_frame)) {
ROS_WARN_STREAM("Kobuki : no param server setting for base_frame, using default [" << base_frame << "][" << name << "].");
} else {
ROS_INFO_STREAM("Kobuki : using base_frame [" << base_frame << "][" << name << "].");
}
if (!nh.getParam("publish_tf", publish_tf)) {
ROS_WARN_STREAM("Kobuki : no param server setting for publish_tf, using default [" << publish_tf << "][" << name << "].");
} else {
if ( publish_tf ) {
ROS_INFO_STREAM("Kobuki : publishing transforms [" << name << "].");
} else {
ROS_INFO_STREAM("Kobuki : not publishing transforms (see robot_pose_ekf) [" << name << "].");
}
}
if (!nh.getParam("use_imu_heading", use_imu_heading)) {
ROS_WARN_STREAM("Kobuki : no param server setting for use_imu_heading, using default [" << use_imu_heading << "][" << name << "].");
} else {
if ( use_imu_heading ) {
ROS_INFO_STREAM("Kobuki : using imu data for heading [" << name << "].");
} else {
ROS_INFO_STREAM("Kobuki : using encoders for heading (see robot_pose_ekf) [" << name << "].");
}
}
odom_trans.header.frame_id = odom_frame;
odom_trans.child_frame_id = base_frame;
pose.setIdentity();
odom_publisher = nh.advertise<nav_msgs::Odometry>("odom", 50); // topic name and queue size
}
bool Odometry::commandTimeout() const {
if ( (!last_cmd_time.isZero()) && ((ros::Time::now() - last_cmd_time) > cmd_vel_timeout) ) {
return true;
} else {
return false;
}
}
void Odometry::update(const ecl::LegacyPose2D<double> &pose_update, ecl::linear_algebra::Vector3d &pose_update_rates,
double imu_heading, double imu_angular_velocity) {
pose *= pose_update;
if (use_imu_heading == true) {
// Overwite with gyro heading data
pose.heading(imu_heading);
pose_update_rates[2] = imu_angular_velocity;
}
//since all ros tf odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(pose.heading());
if ( ros::ok() ) {
publishTransform(odom_quat);
publishOdometry(odom_quat, pose_update_rates);
}
}
/*****************************************************************************
** Private Implementation
*****************************************************************************/
void Odometry::publishTransform(const geometry_msgs::Quaternion &odom_quat)
{
if (publish_tf == false)
return;
odom_trans.header.stamp = ros::Time::now();
odom_trans.transform.translation.x = pose.x();
odom_trans.transform.translation.y = pose.y();
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
odom_broadcaster.sendTransform(odom_trans);
}
void Odometry::publishOdometry(const geometry_msgs::Quaternion &odom_quat,
const ecl::linear_algebra::Vector3d &pose_update_rates)
{
// Publish as shared pointer to leverage the nodelets' zero-copy pub/sub feature
nav_msgs::OdometryPtr odom(new nav_msgs::Odometry);
// Header
odom->header.stamp = ros::Time::now();
odom->header.frame_id = odom_frame;
odom->child_frame_id = base_frame;
// Position
odom->pose.pose.position.x = pose.x();
odom->pose.pose.position.y = pose.y();
odom->pose.pose.position.z = 0.0;
odom->pose.pose.orientation = odom_quat;
// Velocity
odom->twist.twist.linear.x = pose_update_rates[0];
odom->twist.twist.linear.y = pose_update_rates[1];
odom->twist.twist.angular.z = pose_update_rates[2];
// Pose covariance (required by robot_pose_ekf) TODO: publish realistic values
// Odometry yaw covariance must be much bigger than the covariance provided
// by the imu, as the later takes much better measures
odom->pose.covariance[0] = 0.1;
odom->pose.covariance[7] = 0.1;
odom->pose.covariance[35] = use_imu_heading ? 0.05 : 0.2;
odom->pose.covariance[14] = 1e10; // set a non-zero covariance on unused
odom->pose.covariance[21] = 1e10; // dimensions (z, pitch and roll); this
odom->pose.covariance[28] = 1e10; // is a requirement of robot_pose_ekf
odom_publisher.publish(odom);
}
} // namespace kobuki

485
autonomous_robotics_ros/src/kobuki_node/src/library/slot_callbacks.cpp Normal file
View File

@@ -0,0 +1,485 @@
/*
* Copyright (c) 2012, Yujin Robot.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Yujin Robot nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file src/node/slot_callbacks.cpp
*
* @brief All the slot callbacks for interrupts from the kobuki driver.
*
**/
/*****************************************************************************
** Includes
*****************************************************************************/
#include "kobuki_node/kobuki_ros.hpp"
/*****************************************************************************
** Namespaces
*****************************************************************************/
namespace kobuki
{
void KobukiRos::processStreamData() {
publishWheelState();
publishSensorState();
publishDockIRData();
publishInertia();
publishRawInertia();
}
/*****************************************************************************
** Publish Sensor Stream Workers
*****************************************************************************/
void KobukiRos::publishSensorState()
{
if ( ros::ok() ) {
if (sensor_state_publisher.getNumSubscribers() > 0) {
kobuki_msgs::SensorState state;
CoreSensors::Data data = kobuki.getCoreSensorData();
state.header.stamp = ros::Time::now();
state.time_stamp = data.time_stamp; // firmware time stamp
state.bumper = data.bumper;
state.wheel_drop = data.wheel_drop;
state.cliff = data.cliff;
state.left_encoder = data.left_encoder;
state.right_encoder = data.right_encoder;
state.left_pwm = data.left_pwm;
state.right_pwm = data.right_pwm;
state.buttons = data.buttons;
state.charger = data.charger;
state.battery = data.battery;
state.over_current = data.over_current;
Cliff::Data cliff_data = kobuki.getCliffData();
state.bottom = cliff_data.bottom;
Current::Data current_data = kobuki.getCurrentData();
state.current = current_data.current;
GpInput::Data gp_input_data = kobuki.getGpInputData();
state.digital_input = gp_input_data.digital_input;
for ( unsigned int i = 0; i < gp_input_data.analog_input.size(); ++i ) {
state.analog_input.push_back(gp_input_data.analog_input[i]);
}
sensor_state_publisher.publish(state);
}
}
}
void KobukiRos::publishWheelState()
{
// Take latest encoders and gyro data
ecl::LegacyPose2D<double> pose_update;
ecl::linear_algebra::Vector3d pose_update_rates;
kobuki.updateOdometry(pose_update, pose_update_rates);
kobuki.getWheelJointStates(joint_states.position[0], joint_states.velocity[0], // left wheel
joint_states.position[1], joint_states.velocity[1]); // right wheel
// Update and publish odometry and joint states
odometry.update(pose_update, pose_update_rates, kobuki.getHeading(), kobuki.getAngularVelocity());
if (ros::ok())
{
joint_states.header.stamp = ros::Time::now();
joint_state_publisher.publish(joint_states);
}
}
void KobukiRos::publishInertia()
{
if (ros::ok())
{
if (imu_data_publisher.getNumSubscribers() > 0)
{
// Publish as shared pointer to leverage the nodelets' zero-copy pub/sub feature
sensor_msgs::ImuPtr msg(new sensor_msgs::Imu);
msg->header.frame_id = "gyro_link";
msg->header.stamp = ros::Time::now();
msg->orientation = tf::createQuaternionMsgFromRollPitchYaw(0.0, 0.0, kobuki.getHeading());
// set a non-zero covariance on unused dimensions (pitch and roll); this is a requirement of robot_pose_ekf
// set yaw covariance as very low, to make it dominate over the odometry heading when combined
// 1: fill once, as its always the same; 2: using an invented value; cannot we get a realistic estimation?
msg->orientation_covariance[0] = DBL_MAX;
msg->orientation_covariance[4] = DBL_MAX;
msg->orientation_covariance[8] = 0.05;
// fill angular velocity; we ignore acceleration for now
msg->angular_velocity.z = kobuki.getAngularVelocity();
// angular velocity covariance; useless by now, but robot_pose_ekf's
// roadmap claims that it will compute velocities in the future
msg->angular_velocity_covariance[0] = DBL_MAX;
msg->angular_velocity_covariance[4] = DBL_MAX;
msg->angular_velocity_covariance[8] = 0.05;
imu_data_publisher.publish(msg);
}
}
}
void KobukiRos::publishRawInertia()
{
if ( ros::ok() && (raw_imu_data_publisher.getNumSubscribers() > 0) )
{
// Publish as shared pointer to leverage the nodelets' zero-copy pub/sub feature
sensor_msgs::ImuPtr msg(new sensor_msgs::Imu);
ThreeAxisGyro::Data data = kobuki.getRawInertiaData();
ros::Time now = ros::Time::now();
ros::Duration interval(0.01); // Time interval between each sensor reading.
const double digit_to_dps = 0.00875; // digit to deg/s ratio, comes from datasheet of 3d gyro[L3G4200D].
unsigned int length = data.followed_data_length/3;
for( unsigned int i=0; i<length; i++) {
// Each sensor reading has id, that circulate 0 to 255.
//msg->header.frame_id = std::string("gyro_link_" + boost::lexical_cast<std::string>((unsigned int)data.frame_id+i));
msg->header.frame_id = "gyro_link";
// Update rate of 3d gyro sensor is 100 Hz, but robot's update rate is 50 Hz.
// So, here is some compensation.
// See also https://github.com/yujinrobot/kobuki/issues/216
msg->header.stamp = now - interval * (length-i-1);
// Sensing axis of 3d gyro is not match with robot. It is rotated 90 degree counterclockwise about z-axis.
msg->angular_velocity.x = angles::from_degrees( -digit_to_dps * (short)data.data[i*3+1] );
msg->angular_velocity.y = angles::from_degrees( digit_to_dps * (short)data.data[i*3+0] );
msg->angular_velocity.z = angles::from_degrees( digit_to_dps * (short)data.data[i*3+2] );
raw_imu_data_publisher.publish(msg);
}
}
}
void KobukiRos::publishDockIRData()
{
if (ros::ok())
{
if (dock_ir_publisher.getNumSubscribers() > 0)
{
DockIR::Data data = kobuki.getDockIRData();
// Publish as shared pointer to leverage the nodelets' zero-copy pub/sub feature
kobuki_msgs::DockInfraRedPtr msg(new kobuki_msgs::DockInfraRed);
msg->header.frame_id = "dock_ir_link";
msg->header.stamp = ros::Time::now();
msg->data.push_back( data.docking[0] );
msg->data.push_back( data.docking[1] );
msg->data.push_back( data.docking[2] );
dock_ir_publisher.publish(msg);
}
}
}
/*****************************************************************************
** Non Default Stream Packets
*****************************************************************************/
/**
* @brief Publish fw, hw, sw version information.
*
* The driver will only gather this data when initialising so it is
* important that this publisher is latched.
*/
void KobukiRos::publishVersionInfo(const VersionInfo &version_info)
{
if (ros::ok())
{
kobuki_msgs::VersionInfoPtr msg(new kobuki_msgs::VersionInfo);
msg->firmware = VersionInfo::toString(version_info.firmware);
msg->hardware = VersionInfo::toString(version_info.hardware);
msg->software = VersionInfo::getSoftwareVersion();
msg->udid.resize(3);
msg->udid[0] = version_info.udid0;
msg->udid[1] = version_info.udid1;
msg->udid[2] = version_info.udid2;
// Set available features mask depending on firmware and driver versions
if (version_info.firmware > 65536) // 1.0.0
{
msg->features |= kobuki_msgs::VersionInfo::SMOOTH_MOVE_START;
msg->features |= kobuki_msgs::VersionInfo::GYROSCOPE_3D_DATA;
}
if (version_info.firmware > 65792) // 1.1.0
{
// msg->features |= kobuki_msgs::VersionInfo::SOMETHING_JINCHA_FANCY;
}
// if (msg->firmware > ...
version_info_publisher.publish(msg);
}
}
void KobukiRos::publishControllerInfo()
{
if (ros::ok())
{
kobuki_msgs::ControllerInfoPtr msg(new kobuki_msgs::ControllerInfo);
ControllerInfo::Data data = kobuki.getControllerInfoData();
msg->type = data.type;
msg->p_gain = static_cast<float>(data.p_gain) * 0.001f;;
msg->i_gain = static_cast<float>(data.i_gain) * 0.001f;;
msg->d_gain = static_cast<float>(data.d_gain) * 0.001f;;
controller_info_publisher.publish(msg);
}
}
/*****************************************************************************
** Events
*****************************************************************************/
void KobukiRos::publishButtonEvent(const ButtonEvent &event)
{
if (ros::ok())
{
kobuki_msgs::ButtonEventPtr msg(new kobuki_msgs::ButtonEvent);
switch(event.state) {
case(ButtonEvent::Pressed) : { msg->state = kobuki_msgs::ButtonEvent::PRESSED; break; }
case(ButtonEvent::Released) : { msg->state = kobuki_msgs::ButtonEvent::RELEASED; break; }
default: break;
}
switch(event.button) {
case(ButtonEvent::Button0) : { msg->button = kobuki_msgs::ButtonEvent::Button0; break; }
case(ButtonEvent::Button1) : { msg->button = kobuki_msgs::ButtonEvent::Button1; break; }
case(ButtonEvent::Button2) : { msg->button = kobuki_msgs::ButtonEvent::Button2; break; }
default: break;
}
button_event_publisher.publish(msg);
}
}
void KobukiRos::publishBumperEvent(const BumperEvent &event)
{
if (ros::ok())
{
kobuki_msgs::BumperEventPtr msg(new kobuki_msgs::BumperEvent);
switch(event.state) {
case(BumperEvent::Pressed) : { msg->state = kobuki_msgs::BumperEvent::PRESSED; break; }
case(BumperEvent::Released) : { msg->state = kobuki_msgs::BumperEvent::RELEASED; break; }
default: break;
}
switch(event.bumper) {
case(BumperEvent::Left) : { msg->bumper = kobuki_msgs::BumperEvent::LEFT; break; }
case(BumperEvent::Center) : { msg->bumper = kobuki_msgs::BumperEvent::CENTER; break; }
case(BumperEvent::Right) : { msg->bumper = kobuki_msgs::BumperEvent::RIGHT; break; }
default: break;
}
bumper_event_publisher.publish(msg);
}
}
void KobukiRos::publishCliffEvent(const CliffEvent &event)
{
if (ros::ok())
{
kobuki_msgs::CliffEventPtr msg(new kobuki_msgs::CliffEvent);
switch(event.state) {
case(CliffEvent::Floor) : { msg->state = kobuki_msgs::CliffEvent::FLOOR; break; }
case(CliffEvent::Cliff) : { msg->state = kobuki_msgs::CliffEvent::CLIFF; break; }
default: break;
}
switch(event.sensor) {
case(CliffEvent::Left) : { msg->sensor = kobuki_msgs::CliffEvent::LEFT; break; }
case(CliffEvent::Center) : { msg->sensor = kobuki_msgs::CliffEvent::CENTER; break; }
case(CliffEvent::Right) : { msg->sensor = kobuki_msgs::CliffEvent::RIGHT; break; }
default: break;
}
msg->bottom = event.bottom;
cliff_event_publisher.publish(msg);
}
}
void KobukiRos::publishWheelEvent(const WheelEvent &event)
{
if (ros::ok())
{
kobuki_msgs::WheelDropEventPtr msg(new kobuki_msgs::WheelDropEvent);
switch(event.state) {
case(WheelEvent::Dropped) : { msg->state = kobuki_msgs::WheelDropEvent::DROPPED; break; }
case(WheelEvent::Raised) : { msg->state = kobuki_msgs::WheelDropEvent::RAISED; break; }
default: break;
}
switch(event.wheel) {
case(WheelEvent::Left) : { msg->wheel = kobuki_msgs::WheelDropEvent::LEFT; break; }
case(WheelEvent::Right) : { msg->wheel = kobuki_msgs::WheelDropEvent::RIGHT; break; }
default: break;
}
wheel_event_publisher.publish(msg);
}
}
void KobukiRos::publishPowerEvent(const PowerEvent &event)
{
if (ros::ok())
{
kobuki_msgs::PowerSystemEventPtr msg(new kobuki_msgs::PowerSystemEvent);
switch(event.event) {
case(PowerEvent::Unplugged) :
{ msg->event = kobuki_msgs::PowerSystemEvent::UNPLUGGED; break; }
case(PowerEvent::PluggedToAdapter) :
{ msg->event = kobuki_msgs::PowerSystemEvent::PLUGGED_TO_ADAPTER; break; }
case(PowerEvent::PluggedToDockbase) :
{ msg->event = kobuki_msgs::PowerSystemEvent::PLUGGED_TO_DOCKBASE; break; }
case(PowerEvent::ChargeCompleted) :
{ msg->event = kobuki_msgs::PowerSystemEvent::CHARGE_COMPLETED; break; }
case(PowerEvent::BatteryLow) :
{ msg->event = kobuki_msgs::PowerSystemEvent::BATTERY_LOW; break; }
case(PowerEvent::BatteryCritical) :
{ msg->event = kobuki_msgs::PowerSystemEvent::BATTERY_CRITICAL; break; }
default: break;
}
power_event_publisher.publish(msg);
}
}
void KobukiRos::publishInputEvent(const InputEvent &event)
{
if (ros::ok())
{
kobuki_msgs::DigitalInputEventPtr msg(new kobuki_msgs::DigitalInputEvent);
for (unsigned int i = 0; i < msg->values.size(); i++)
msg->values[i] = event.values[i];
input_event_publisher.publish(msg);
}
}
void KobukiRos::publishRobotEvent(const RobotEvent &event)
{
if (ros::ok())
{
kobuki_msgs::RobotStateEventPtr msg(new kobuki_msgs::RobotStateEvent);
switch(event.state) {
case(RobotEvent::Online) : { msg->state = kobuki_msgs::RobotStateEvent::ONLINE; break; }
case(RobotEvent::Offline) : { msg->state = kobuki_msgs::RobotStateEvent::OFFLINE; break; }
default: break;
}
robot_event_publisher.publish(msg);
}
}
/**
* @brief Prints the raw data stream to a publisher.
*
* This is a lazy publisher, it only publishes if someone is listening. It publishes the
* hex byte values of the raw data commands. Useful for debugging command to protocol
* byte packets to the firmware.
*
* The signal which calls this
* function is sending a copy of the buffer (don't worry about mutexes). Be ideal if we used
* const PacketFinder::BufferType here, but haven't updated PushPop to work with consts yet.
*
* @param buffer
*/
void KobukiRos::publishRawDataCommand(Command::Buffer &buffer)
{
if ( raw_data_command_publisher.getNumSubscribers() > 0 ) { // do not do string processing if there is no-one listening.
std::ostringstream ostream;
Command::Buffer::Formatter format;
ostream << format(buffer); // convert to an easily readable hex string.
std_msgs::String s;
s.data = ostream.str();
if (ros::ok())
{
raw_data_command_publisher.publish(s);
}
}
}
/**
* @brief Prints the raw data stream to a publisher.
*
* This is a lazy publisher, it only publishes if someone is listening. It publishes the
* hex byte values of the raw data (incoming) stream. Useful for checking when bytes get
* mangled.
*
* The signal which calls this
* function is sending a copy of the buffer (don't worry about mutexes). Be ideal if we used
* const PacketFinder::BufferType here, but haven't updated PushPop to work with consts yet.
*
* @param buffer
*/
void KobukiRos::publishRawDataStream(PacketFinder::BufferType &buffer)
{
if ( raw_data_stream_publisher.getNumSubscribers() > 0 ) { // do not do string processing if there is no-one listening.
/*std::cout << "size: [" << buffer.size() << "], asize: [" << buffer.asize() << "]" << std::endl;
std::cout << "leader: " << buffer.leader << ", follower: " << buffer.follower << std::endl;
{
std::ostringstream ostream;
PacketFinder::BufferType::Formatter format;
ostream << format(buffer); // convert to an easily readable hex string.
//std::cout << ostream.str() << std::endl;
std_msgs::String s;
s.data = ostream.str();
if (ros::ok())
{
raw_data_stream_publisher.publish(s);
}
}*/
{
std::ostringstream ostream;
ostream << "{ " ;
ostream << std::setfill('0') << std::uppercase;
for (unsigned int i=0; i < buffer.size(); i++)
ostream << std::hex << std::setw(2) << static_cast<unsigned int>(buffer[i]) << " " << std::dec;
ostream << "}";
//std::cout << ostream.str() << std::endl;
std_msgs::StringPtr msg(new std_msgs::String);
msg->data = ostream.str();
if (ros::ok())
{
raw_data_stream_publisher.publish(msg);
}
}
}
}
void KobukiRos::publishRawControlCommand(const std::vector<short> &velocity_commands)
{
if ( raw_control_command_publisher.getNumSubscribers() > 0 ) {
std_msgs::Int16MultiArrayPtr msg(new std_msgs::Int16MultiArray);
msg->data = velocity_commands;
if (ros::ok())
{
raw_control_command_publisher.publish(msg);
}
}
return;
}
} // namespace kobuki

241
autonomous_robotics_ros/src/kobuki_node/src/library/subscriber_callbacks.cpp Normal file
View File

@@ -0,0 +1,241 @@
/*
* Copyright (c) 2012, Yujin Robot.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Yujin Robot nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file /kobuki_node/src/node/subscriber_callbacks.cpp
*
* @brief Subscriber callbacks for kobuki node.
*
**/
/*****************************************************************************
** Includes
*****************************************************************************/
#include "../../include/kobuki_node/kobuki_ros.hpp"
/*****************************************************************************
** Namespaces
*****************************************************************************/
namespace kobuki
{
/*****************************************************************************
** Implementation
*****************************************************************************/
void KobukiRos::subscribeVelocityCommand(const geometry_msgs::TwistConstPtr msg)
{
if (kobuki.isEnabled())
{
// For now assuming this is in the robot frame, but probably this
// should be global frame and require a transform
//double vx = msg->linear.x; // in (m/s)
//double wz = msg->angular.z; // in (rad/s)
ROS_DEBUG_STREAM("Kobuki : velocity command received [" << msg->linear.x << "],[" << msg->angular.z << "]");
kobuki.setBaseControl(msg->linear.x, msg->angular.z);
odometry.resetTimeout();
}
return;
}
void KobukiRos::subscribeLed1Command(const kobuki_msgs::LedConstPtr msg)
{
switch( msg->value ) {
case kobuki_msgs::Led::GREEN: kobuki.setLed(Led1, Green ); break;
case kobuki_msgs::Led::ORANGE: kobuki.setLed(Led1, Orange ); break;
case kobuki_msgs::Led::RED: kobuki.setLed(Led1, Red ); break;
case kobuki_msgs::Led::BLACK: kobuki.setLed(Led1, Black ); break;
default: ROS_WARN_STREAM("Kobuki : led 1 command value invalid."); break;
}
return;
}
void KobukiRos::subscribeLed2Command(const kobuki_msgs::LedConstPtr msg)
{
switch( msg->value ) {
case kobuki_msgs::Led::GREEN: kobuki.setLed(Led2, Green ); break;
case kobuki_msgs::Led::ORANGE: kobuki.setLed(Led2, Orange ); break;
case kobuki_msgs::Led::RED: kobuki.setLed(Led2, Red ); break;
case kobuki_msgs::Led::BLACK: kobuki.setLed(Led2, Black ); break;
default: ROS_WARN_STREAM("Kobuki : led 2 command value invalid."); break;
}
return;
}
void KobukiRos::subscribeDigitalOutputCommand(const kobuki_msgs::DigitalOutputConstPtr msg)
{
DigitalOutput digital_output;
for ( unsigned int i = 0; i < 4; ++i ) {
digital_output.values[i] = msg->values[i];
digital_output.mask[i] = msg->mask[i];
}
kobuki.setDigitalOutput(digital_output);
return;
}
void KobukiRos::subscribeExternalPowerCommand(const kobuki_msgs::ExternalPowerConstPtr msg)
{
// Validate message
if (!((msg->source == kobuki_msgs::ExternalPower::PWR_3_3V1A) ||
(msg->source == kobuki_msgs::ExternalPower::PWR_5V1A) ||
(msg->source == kobuki_msgs::ExternalPower::PWR_12V5A) ||
(msg->source == kobuki_msgs::ExternalPower::PWR_12V1_5A)))
{
ROS_ERROR_STREAM("Kobuki : Power source " << (unsigned int)msg->source << " does not exist! [" << name << "].");
return;
}
if (!((msg->state == kobuki_msgs::ExternalPower::OFF) ||
(msg->state == kobuki_msgs::ExternalPower::ON)))
{
ROS_ERROR_STREAM("Kobuki : Power source state "
<< (unsigned int)msg->state << " does not exist! [" << name << "].");
return;
}
DigitalOutput digital_output;
for ( unsigned int i = 0; i < 4; ++i )
{
if (i == msg->source)
{
if (msg->state)
{
digital_output.values[i] = true; // turn source on
ROS_INFO_STREAM("Kobuki : Turning on external power source "
<< (unsigned int)msg->source << ". [" << name << "].");
}
else
{
digital_output.values[i] = false; // turn source off
ROS_INFO_STREAM("Kobuki : Turning off external power source "
<< (unsigned int)msg->source << ". [" << name << "].");
}
digital_output.mask[i] = true; // change source state
}
else
{
digital_output.values[i] = false; // values doesn't matter here, since mask is set false, what means ignoring
digital_output.mask[i] = false;
}
}
kobuki.setExternalPower(digital_output);
return;
}
/**
* @brief Play a predefined sound (single sound or sound sequence)
*/
void KobukiRos::subscribeSoundCommand(const kobuki_msgs::SoundConstPtr msg)
{
if ( msg->value == kobuki_msgs::Sound::ON )
{
kobuki.playSoundSequence(On);
}
else if ( msg->value == kobuki_msgs::Sound::OFF )
{
kobuki.playSoundSequence(Off);
}
else if ( msg->value == kobuki_msgs::Sound::RECHARGE )
{
kobuki.playSoundSequence(Recharge);
}
else if ( msg->value == kobuki_msgs::Sound::BUTTON )
{
kobuki.playSoundSequence(Button);
}
else if ( msg->value == kobuki_msgs::Sound::ERROR )
{
kobuki.playSoundSequence(Error);
}
else if ( msg->value == kobuki_msgs::Sound::CLEANINGSTART )
{
kobuki.playSoundSequence(CleaningStart);
}
else if ( msg->value == kobuki_msgs::Sound::CLEANINGEND )
{
kobuki.playSoundSequence(CleaningEnd);
}
else
{
ROS_WARN_STREAM("Kobuki : Invalid sound command! There is no sound stored for value '" << msg->value << "'.");
}
return;
}
/**
* @brief Reset the odometry variables.
*/
void KobukiRos::subscribeResetOdometry(const std_msgs::EmptyConstPtr /* msg */)
{
ROS_INFO_STREAM("Kobuki : Resetting the odometry. [" << name << "].");
joint_states.position[0] = 0.0; // wheel_left
joint_states.velocity[0] = 0.0;
joint_states.position[1] = 0.0; // wheel_right
joint_states.velocity[1] = 0.0;
odometry.resetOdometry();
kobuki.resetOdometry();
return;
}
void KobukiRos::subscribeMotorPower(const kobuki_msgs::MotorPowerConstPtr msg)
{
if (msg->state == kobuki_msgs::MotorPower::ON)
{
ROS_INFO_STREAM("Kobuki : Firing up the motors. [" << name << "]");
kobuki.enable();
odometry.resetTimeout();
}
else if (msg->state == kobuki_msgs::MotorPower::OFF)
{
kobuki.disable();
ROS_INFO_STREAM("Kobuki : Shutting down the motors. [" << name << "]");
odometry.resetTimeout();
}
else
{
ROS_ERROR_STREAM("Kobuki : Motor power command specifies unknown state '" << (unsigned int)msg->state
<< "'. [" << name << "]");
}
}
void KobukiRos::subscribeControllerInfoCommand(const kobuki_msgs::ControllerInfoConstPtr msg)
{
if( msg->p_gain < 0.0f || msg->i_gain < 0.0f || msg->d_gain < 0.0f) {
ROS_ERROR_STREAM("Kobuki : All controller gains should be positive. [" << name << "]");
return;
}
kobuki.setControllerGain(msg->type,
static_cast<unsigned int>(msg->p_gain*1000.0f),
static_cast<unsigned int>(msg->i_gain*1000.0f),
static_cast<unsigned int>(msg->d_gain*1000.0f));
return;
}
} // namespace kobuki

11
autonomous_robotics_ros/src/kobuki_node/src/nodelet/CMakeLists.txt Normal file
View File

@@ -0,0 +1,11 @@
##############################################################################
# NODELET
##############################################################################
add_library(kobuki_nodelet kobuki_nodelet.cpp)
add_dependencies(kobuki_nodelet kobuki_ros)
target_link_libraries(kobuki_nodelet kobuki_ros)
install(TARGETS kobuki_nodelet
DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)

91
autonomous_robotics_ros/src/kobuki_node/src/nodelet/kobuki_nodelet.cpp Normal file
View File

@@ -0,0 +1,91 @@
/*
* Copyright (c) 2012, Yujin Robot.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Yujin Robot nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file /kobuki_node/src/nodelet/kobuki_nodelet.cpp
*
* @brief Implementation for the ROS Kobuki nodelet
**/
/*****************************************************************************
** Includes
*****************************************************************************/
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <ecl/threads/thread.hpp>
#include "kobuki_node/kobuki_ros.hpp"
namespace kobuki
{
class KobukiNodelet : public nodelet::Nodelet
{
public:
KobukiNodelet() : shutdown_requested_(false) {};
~KobukiNodelet()
{
NODELET_DEBUG_STREAM("Kobuki : waiting for update thread to finish.");
shutdown_requested_ = true;
update_thread_.join();
}
virtual void onInit()
{
NODELET_DEBUG_STREAM("Kobuki : initialising nodelet...");
std::string nodelet_name = this->getName();
kobuki_.reset(new KobukiRos(nodelet_name));
// if there are latency issues with callbacks, we might want to move to process callbacks in multiple threads (use MTPrivateNodeHandle)
if (kobuki_->init(this->getPrivateNodeHandle(), this->getNodeHandle()))
{
update_thread_.start(&KobukiNodelet::update, *this);
NODELET_INFO_STREAM("Kobuki : initialised.");
}
else
{
NODELET_ERROR_STREAM("Kobuki : could not initialise! Please restart.");
}
}
private:
void update()
{
ros::Rate spin_rate(10);
while (!shutdown_requested_ && ros::ok() && kobuki_->update())
{
spin_rate.sleep();
}
}
boost::shared_ptr<KobukiRos> kobuki_;
ecl::Thread update_thread_;
bool shutdown_requested_;
};
} // namespace kobuki
PLUGINLIB_EXPORT_CLASS(kobuki::KobukiNodelet, nodelet::Nodelet);

66
autonomous_robotics_ros/src/kobuki_safety_controller/.cproject Normal file
View File

@@ -0,0 +1,66 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?>
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.base.990789000">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.990789000" moduleId="org.eclipse.cdt.core.settings" name="Default">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.base.990789000" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
<folderInfo id="cdt.managedbuild.toolchain.gnu.base.990789000.690998658" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.base.434663521" name="cdt.managedbuild.toolchain.gnu.base" superClass="cdt.managedbuild.toolchain.gnu.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.1565546395" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
<builder id="cdt.managedbuild.target.gnu.builder.base.1035872122" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="cdt.managedbuild.target.gnu.builder.base"/>
<tool id="cdt.managedbuild.tool.gnu.archiver.base.1774405744" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.464412048" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base">
<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.19647080" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
</tool>
<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.1724647528" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base">
<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.393310220" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
</tool>
<tool id="cdt.managedbuild.tool.gnu.c.linker.base.1129461534" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.1354354052" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base">
<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.229380900" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
<additionalInput kind="additionalinput" paths="$(LIBS)"/>
</inputType>
</tool>
<tool id="cdt.managedbuild.tool.gnu.assembler.base.1385482174" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base">
<inputType id="cdt.managedbuild.tool.gnu.assembler.input.2052218308" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
</tool>
</toolChain>
</folderInfo>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
</cconfiguration>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="kobuki_safety_controller.null.679135572" name="kobuki_safety_controller"/>
</storageModule>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
<buildTargets>
<target name="rosmake" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>rosmake</buildCommand>
<buildArguments/>
<buildTarget/>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
</buildTargets>
</storageModule>
</cproject>

1
autonomous_robotics_ros/src/turtlebot/turtlebot_interactions/turtlebot_interactive_markers/.gitignore → autonomous_robotics_ros/src/kobuki_safety_controller/.gitignore vendored Executable file → Normal file
View File

@@ -1,2 +1,3 @@
bin
build
lib

79
autonomous_robotics_ros/src/kobuki_safety_controller/.project Normal file
View File

@@ -0,0 +1,79 @@
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>kobuki_safety_controller</name>
<comment></comment>
<projects>
</projects>
<buildSpec>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
<triggers>clean,full,incremental,</triggers>
<arguments>
<dictionary>
<key>?name?</key>
<value></value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.append_environment</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.autoBuildTarget</key>
<value>all</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildArguments</key>
<value></value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildCommand</key>
<value>make</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>
<value>clean</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.contents</key>
<value>org.eclipse.cdt.make.core.activeConfigSettings</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.enableAutoBuild</key>
<value>false</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.enableCleanBuild</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.enableFullBuild</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.fullBuildTarget</key>
<value>all</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.stopOnError</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
<value>true</value>
</dictionary>
</arguments>
</buildCommand>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
<triggers>full,incremental,</triggers>
<arguments>
</arguments>
</buildCommand>
</buildSpec>
<natures>
<nature>org.eclipse.cdt.core.cnature</nature>
<nature>org.eclipse.cdt.core.ccnature</nature>
<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
</natures>
</projectDescription>

53
autonomous_robotics_ros/src/kobuki_safety_controller/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,53 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package kobuki_safety_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.6.6 (2015-05-27)
------------------
0.6.5 (2014-11-21)
------------------
0.6.4 (2014-08-26)
------------------
0.6.3 (2014-08-25)
------------------
* added initialization for msg_ to kobuki_safety_controller
* added time_to_extend_bump_cliff_events param to kobuki_safety_controller
* Contributors: Kaijen Hsiao
0.6.2 (2014-08-11)
------------------
0.6.1 (2014-08-08)
------------------
0.6.0 (2014-08-08)
------------------
* Add missing run dependency on yocs_cmd_vel_mux
* Contributors: Jorge Santos
0.5.5 (2013-10-11)
------------------
0.5.4 (2013-09-06)
------------------
0.5.3 (2013-08-30)
------------------
0.5.0 (2013-08-29)
------------------
* Added extra url info on all packages.
* Fix URL to the previous changelog wiki.
* Changelogs at package level.
0.4.0 (2013-08-09)
------------------
Previous versions, bugfixing
============================
Available in ROS wiki: http://ros.org/wiki/kobuki/ChangeList

29
autonomous_robotics_ros/src/kobuki_safety_controller/CMakeLists.txt Normal file
View File

@@ -0,0 +1,29 @@
cmake_minimum_required(VERSION 2.8.3)
project(kobuki_safety_controller)
find_package(catkin REQUIRED COMPONENTS roscpp nodelet pluginlib std_msgs geometry_msgs kobuki_msgs yocs_controllers ecl_threads)
catkin_package(
INCLUDE_DIRS include
LIBRARIES kobuki_safety_controller_nodelet
CATKIN_DEPENDS roscpp nodelet pluginlib std_msgs geometry_msgs kobuki_msgs yocs_controllers ecl_threads
)
include_directories(include
${catkin_INCLUDE_DIRS})
add_library(kobuki_safety_controller_nodelet src/nodelet.cpp)
add_dependencies(kobuki_safety_controller_nodelet ${catkin_EXPORTED_TARGETS})
target_link_libraries(kobuki_safety_controller_nodelet ${catkin_LIBRARIES})
install(TARGETS kobuki_safety_controller_nodelet
DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(DIRECTORY plugins
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

359
autonomous_robotics_ros/src/kobuki_safety_controller/include/kobuki_safety_controller/safety_controller.hpp Normal file
View File

@@ -0,0 +1,359 @@
/*
* Copyright (c) 2012, Yujin Robot.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Yujin Robot nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file /kobuki_safety_controller/include/kobuki_safety_controller/safety_controller.hpp
*
* @brief Kobuki-specific safety controller
*
* This controller uses Kobuki's bumper, cliff and wheel drop sensors to ensure safe operation.
*
* @author Marcus Liebhardt, Yujin Robot
*
**/
/*****************************************************************************
** Ifdefs
*****************************************************************************/
#ifndef SAFETY_CONTROLLER_HPP_
#define SAFETY_CONTROLLER_HPP_
/*****************************************************************************
** Includes
*****************************************************************************/
#include <string>
#include <ros/ros.h>
#include <yocs_controllers/default_controller.hpp>
#include <std_msgs/Empty.h>
#include <geometry_msgs/Twist.h>
#include <kobuki_msgs/BumperEvent.h>
#include <kobuki_msgs/CliffEvent.h>
#include <kobuki_msgs/WheelDropEvent.h>
namespace kobuki
{
/**
* @ brief Keeps track of safety-related events and commands Kobuki to move accordingly
*
* The SafetyController keeps track of bumper, cliff and wheel drop events. In case of the first two,
* Kobuki is commanded to move back. In the latter case, Kobuki is stopped. All commands stop when the
* event condition disappears. In the case of lateral bump/cliff, robot also spins a bit, what makes
* easier to escape from the risk.
*
* This controller can be enabled/disabled.
* The safety states (bumper pressed etc.) can be reset. WARNING: Dangerous!
*/
class SafetyController : public yocs::Controller
{
public:
SafetyController(ros::NodeHandle& nh, std::string& name) :
Controller(),
nh_(nh),
name_(name),
wheel_left_dropped_(false),
wheel_right_dropped_(false),
bumper_left_pressed_(false),
bumper_center_pressed_(false),
bumper_right_pressed_(false),
cliff_left_detected_(false),
cliff_center_detected_(false),
cliff_right_detected_(false),
last_event_time_(ros::Time(0)),
msg_(new geometry_msgs::Twist()){};
~SafetyController(){};
/**
* Set-up necessary publishers/subscribers and variables
* @return true, if successful
*/
bool init()
{
//how long to keep sending messages after a bump, cliff, or wheel drop stops
double time_to_extend_bump_cliff_events;
nh_.param("time_to_extend_bump_cliff_events", time_to_extend_bump_cliff_events, 0.0);
time_to_extend_bump_cliff_events_ = ros::Duration(time_to_extend_bump_cliff_events);
enable_controller_subscriber_ = nh_.subscribe("enable", 10, &SafetyController::enableCB, this);
disable_controller_subscriber_ = nh_.subscribe("disable", 10, &SafetyController::disableCB, this);
bumper_event_subscriber_ = nh_.subscribe("events/bumper", 10, &SafetyController::bumperEventCB, this);
cliff_event_subscriber_ = nh_.subscribe("events/cliff", 10, &SafetyController::cliffEventCB, this);
wheel_event_subscriber_ = nh_.subscribe("events/wheel_drop", 10, &SafetyController::wheelEventCB, this);
reset_safety_states_subscriber_ = nh_.subscribe("reset", 10, &SafetyController::resetSafetyStatesCB, this);
velocity_command_publisher_ = nh_.advertise< geometry_msgs::Twist >("cmd_vel", 10);
return true;
};
/**
* @ brief Checks safety states and publishes velocity commands when necessary
*/
void spin();
private:
ros::NodeHandle nh_;
std::string name_;
ros::Subscriber enable_controller_subscriber_, disable_controller_subscriber_;
ros::Subscriber bumper_event_subscriber_, cliff_event_subscriber_, wheel_event_subscriber_;
ros::Subscriber reset_safety_states_subscriber_;
ros::Publisher controller_state_publisher_, velocity_command_publisher_;
bool wheel_left_dropped_, wheel_right_dropped_;
bool bumper_left_pressed_, bumper_center_pressed_, bumper_right_pressed_;
bool cliff_left_detected_, cliff_center_detected_, cliff_right_detected_;
ros::Duration time_to_extend_bump_cliff_events_;
ros::Time last_event_time_;
geometry_msgs::TwistPtr msg_; // velocity command
/**
* @brief ROS logging output for enabling the controller
* @param msg incoming topic message
*/
void enableCB(const std_msgs::EmptyConstPtr msg);
/**
* @brief ROS logging output for disabling the controller
* @param msg incoming topic message
*/
void disableCB(const std_msgs::EmptyConstPtr msg);
/**
* @brief Keeps track of bumps
* @param msg incoming topic message
*/
void bumperEventCB(const kobuki_msgs::BumperEventConstPtr msg);
/**
* @brief Keeps track of cliff detection
* @param msg incoming topic message
*/
void cliffEventCB(const kobuki_msgs::CliffEventConstPtr msg);
/**
* @brief Keeps track of the wheel states
* @param msg incoming topic message
*/
void wheelEventCB(const kobuki_msgs::WheelDropEventConstPtr msg);
/**
* @brief Callback for resetting safety variables
*
* Allows resetting bumper, cliff and wheel drop states.
* DANGEROUS!
*
* @param msg incoming topic message
*/
void resetSafetyStatesCB(const std_msgs::EmptyConstPtr msg);
};
void SafetyController::enableCB(const std_msgs::EmptyConstPtr msg)
{
if (this->enable())
{
ROS_INFO_STREAM("Controller has been enabled. [" << name_ << "]");
}
else
{
ROS_INFO_STREAM("Controller was already enabled. [" << name_ <<"]");
}
};
void SafetyController::disableCB(const std_msgs::EmptyConstPtr msg)
{
if (this->disable())
{
ROS_INFO_STREAM("Controller has been disabled. [" << name_ <<"]");
}
else
{
ROS_INFO_STREAM("Controller was already disabled. [" << name_ <<"]");
}
};
void SafetyController::cliffEventCB(const kobuki_msgs::CliffEventConstPtr msg)
{
if (msg->state == kobuki_msgs::CliffEvent::CLIFF)
{
last_event_time_ = ros::Time::now();
ROS_DEBUG_STREAM("Cliff detected. Moving backwards. [" << name_ << "]");
switch (msg->sensor)
{
case kobuki_msgs::CliffEvent::LEFT: cliff_left_detected_ = true; break;
case kobuki_msgs::CliffEvent::CENTER: cliff_center_detected_ = true; break;
case kobuki_msgs::CliffEvent::RIGHT: cliff_right_detected_ = true; break;
}
}
else // kobuki_msgs::CliffEvent::FLOOR
{
ROS_DEBUG_STREAM("Not detecting any cliffs. Resuming normal operation. [" << name_ << "]");
switch (msg->sensor)
{
case kobuki_msgs::CliffEvent::LEFT: cliff_left_detected_ = false; break;
case kobuki_msgs::CliffEvent::CENTER: cliff_center_detected_ = false; break;
case kobuki_msgs::CliffEvent::RIGHT: cliff_right_detected_ = false; break;
}
}
};
void SafetyController::bumperEventCB(const kobuki_msgs::BumperEventConstPtr msg)
{
if (msg->state == kobuki_msgs::BumperEvent::PRESSED)
{
last_event_time_ = ros::Time::now();
ROS_DEBUG_STREAM("Bumper pressed. Moving backwards. [" << name_ << "]");
switch (msg->bumper)
{
case kobuki_msgs::BumperEvent::LEFT: bumper_left_pressed_ = true; break;
case kobuki_msgs::BumperEvent::CENTER: bumper_center_pressed_ = true; break;
case kobuki_msgs::BumperEvent::RIGHT: bumper_right_pressed_ = true; break;
}
}
else // kobuki_msgs::BumperEvent::RELEASED
{
ROS_DEBUG_STREAM("No bumper pressed. Resuming normal operation. [" << name_ << "]");
switch (msg->bumper)
{
case kobuki_msgs::BumperEvent::LEFT: bumper_left_pressed_ = false; break;
case kobuki_msgs::BumperEvent::CENTER: bumper_center_pressed_ = false; break;
case kobuki_msgs::BumperEvent::RIGHT: bumper_right_pressed_ = false; break;
}
}
};
void SafetyController::wheelEventCB(const kobuki_msgs::WheelDropEventConstPtr msg)
{
if (msg->state == kobuki_msgs::WheelDropEvent::DROPPED)
{
// need to keep track of both wheels separately
if (msg->wheel == kobuki_msgs::WheelDropEvent::LEFT)
{
ROS_DEBUG_STREAM("Left wheel dropped. [" << name_ << "]");
wheel_left_dropped_ = true;
}
else // kobuki_msgs::WheelDropEvent::RIGHT
{
ROS_DEBUG_STREAM("Right wheel dropped. [" << name_ << "]");
wheel_right_dropped_ = true;
}
}
else // kobuki_msgs::WheelDropEvent::RAISED
{
// need to keep track of both wheels separately
if (msg->wheel == kobuki_msgs::WheelDropEvent::LEFT)
{
ROS_DEBUG_STREAM("Left wheel raised. [" << name_ << "]");
wheel_left_dropped_ = false;
}
else // kobuki_msgs::WheelDropEvent::RIGHT
{
ROS_DEBUG_STREAM("Right wheel raised. [" << name_ << "]");
wheel_right_dropped_ = false;
}
if (!wheel_left_dropped_ && !wheel_right_dropped_)
{
ROS_DEBUG_STREAM("Both wheels raised. Resuming normal operation. [" << name_ << "]");
}
}
};
void SafetyController::resetSafetyStatesCB(const std_msgs::EmptyConstPtr msg)
{
wheel_left_dropped_ = false;
wheel_right_dropped_ = false;
bumper_left_pressed_ = false;
bumper_center_pressed_ = false;
bumper_right_pressed_ = false;
cliff_left_detected_ = false;
cliff_center_detected_ = false;
cliff_right_detected_ = false;
ROS_WARN_STREAM("All safety states have been reset to false. [" << name_ << "]");
}
void SafetyController::spin()
{
if (this->getState())
{
if (wheel_left_dropped_ || wheel_right_dropped_)
{
msg_.reset(new geometry_msgs::Twist());
msg_->linear.x = 0.0;
msg_->linear.y = 0.0;
msg_->linear.z = 0.0;
msg_->angular.x = 0.0;
msg_->angular.y = 0.0;
msg_->angular.z = 0.0;
velocity_command_publisher_.publish(msg_);
}
else if (bumper_center_pressed_ || cliff_center_detected_)
{
msg_.reset(new geometry_msgs::Twist());
msg_->linear.x = -0.1;
msg_->linear.y = 0.0;
msg_->linear.z = 0.0;
msg_->angular.x = 0.0;
msg_->angular.y = 0.0;
msg_->angular.z = 0.0;
velocity_command_publisher_.publish(msg_);
}
else if (bumper_left_pressed_ || cliff_left_detected_)
{
// left bump/cliff; also spin a bit to the right to make escape easier
msg_.reset(new geometry_msgs::Twist());
msg_->linear.x = -0.1;
msg_->linear.y = 0.0;
msg_->linear.z = 0.0;
msg_->angular.x = 0.0;
msg_->angular.y = 0.0;
msg_->angular.z = -0.4;
velocity_command_publisher_.publish(msg_);
}
else if (bumper_right_pressed_ || cliff_right_detected_)
{
// right bump/cliff; also spin a bit to the left to make escape easier
msg_.reset(new geometry_msgs::Twist());
msg_->linear.x = -0.1;
msg_->linear.y = 0.0;
msg_->linear.z = 0.0;
msg_->angular.x = 0.0;
msg_->angular.y = 0.0;
msg_->angular.z = 0.4;
velocity_command_publisher_.publish(msg_);
}
//if we want to extend the safety state and we're within the time, just keep sending msg_
else if (time_to_extend_bump_cliff_events_ > ros::Duration(1e-10) &&
ros::Time::now() - last_event_time_ < time_to_extend_bump_cliff_events_) {
velocity_command_publisher_.publish(msg_);
}
}
};
} // namespace kobuki
#endif /* SAFETY_CONTROLLER_HPP_ */

14
autonomous_robotics_ros/src/kobuki_safety_controller/launch/standalone.launch Normal file
View File

@@ -0,0 +1,14 @@
<!-- Example/standalone launcher for Kobuki's safety controller -->
<launch>
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
<node pkg="nodelet" type="nodelet" name="kobuki_safety_controller" args="load kobuki_safety_controller/SafetyControllerNodelet nodelet_manager">
<remap from="kobuki_safety_controller/enable" to="enable"/>
<remap from="kobuki_safety_controller/disable" to="disable"/>
<remap from="kobuki_safety_controller/reset" to="reset"/>
<remap from="kobuki_safety_controller/cmd_vel" to="cmd_vel"/>
<remap from="kobuki_safety_controller/events/bumper" to="events/bumper"/>
<remap from="kobuki_safety_controller/events/cliff" to="events/cliff"/>
<remap from="kobuki_safety_controller/events/wheel_drop" to="events/wheel_drop"/>
</node>
</launch>

43
autonomous_robotics_ros/src/kobuki_safety_controller/package.xml Normal file
View File

@@ -0,0 +1,43 @@
<package>
<name>kobuki_safety_controller</name>
<version>0.7.6</version>
<description>
A controller ensuring the safe operation of Kobuki.
The SafetyController keeps track of bumper, cliff and wheel drop events. In case of the first two,
Kobuki is commanded to move back. In the latter case, Kobuki is stopped.
This controller can be enabled/disabled.
The safety states (bumper pressed etc.) can be reset. WARNING: Dangerous!
</description>
<author email="marcus.liebhardt@yujinrobot.com">Marcus Liebhardt</author>
<maintainer email="marcus.liebhardt@yujinrobot.com">Marcus Liebhardt</maintainer>
<license>BSD</license>
<url type="bugtracker">https://github.com/yujinrobot/kobuki/issues</url>
<url type="repository">https://github.com/yujinrobot/kobuki</url>
<url type="website">http://ros.org/wiki/kobuki_safety_controller</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>pluginlib</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>kobuki_msgs</build_depend>
<build_depend>yocs_controllers</build_depend>
<build_depend>ecl_threads</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>nodelet</run_depend>
<run_depend>pluginlib</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>kobuki_msgs</run_depend>
<run_depend>yocs_controllers</run_depend>
<run_depend>ecl_threads</run_depend>
<export>
<nodelet plugin="${prefix}/plugins/nodelet_plugins.xml"/>
</export>
</package>

8
autonomous_robotics_ros/src/kobuki_safety_controller/plugins/nodelet_plugins.xml Normal file
View File

@@ -0,0 +1,8 @@
<library path="lib/libkobuki_safety_controller_nodelet">
<class name="kobuki_safety_controller/SafetyControllerNodelet" type="kobuki::SafetyControllerNodelet" base_class_type="nodelet::Nodelet">
<description>
Nodelet for Kobuki's safety controller
</description>
</class>
</library>

102
autonomous_robotics_ros/src/kobuki_safety_controller/src/nodelet.cpp Normal file
View File

@@ -0,0 +1,102 @@
/*
* Copyright (c) 2012, Yujin Robot.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Yujin Robot nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file /kobuki_safety_controller/src/nodelet.cpp
*
* @brief Implementation for Kobuki's safety controller nodelet
*
* @author Marcus Liebhardt, Yujin Robot
*
**/
/*****************************************************************************
** Includes
*****************************************************************************/
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <ecl/threads/thread.hpp>
#include "kobuki_safety_controller/safety_controller.hpp"
namespace kobuki
{
class SafetyControllerNodelet : public nodelet::Nodelet
{
public:
SafetyControllerNodelet() : shutdown_requested_(false) { };
~SafetyControllerNodelet()
{
NODELET_DEBUG_STREAM("Waiting for update thread to finish.");
shutdown_requested_ = true;
update_thread_.join();
}
virtual void onInit()
{
ros::NodeHandle nh = this->getPrivateNodeHandle();
// resolve node(let) name
std::string name = nh.getUnresolvedNamespace();
int pos = name.find_last_of('/');
name = name.substr(pos + 1);
NODELET_INFO_STREAM("Initialising nodelet... [" << name << "]");
controller_.reset(new SafetyController(nh, name));
if (controller_->init())
{
NODELET_INFO_STREAM("Kobuki initialised. Spinning up update thread ... [" << name << "]");
update_thread_.start(&SafetyControllerNodelet::update, *this);
NODELET_INFO_STREAM("Nodelet initialised. [" << name << "]");
}
else
{
NODELET_ERROR_STREAM("Couldn't initialise nodelet! Please restart. [" << name << "]");
}
}
private:
void update()
{
ros::Rate spin_rate(10);
controller_->enable(); // enable the controller when loading the nodelet
while (! shutdown_requested_ && ros::ok())
{
controller_->spin();
spin_rate.sleep();
}
}
boost::shared_ptr<SafetyController> controller_;
ecl::Thread update_thread_;
bool shutdown_requested_;
};
} // namespace kobuki
PLUGINLIB_EXPORT_CLASS(kobuki::SafetyControllerNodelet,
nodelet::Nodelet);

2
autonomous_robotics_ros/src/navigation/fake_localization/fake_localization.cpp
View File

@@ -102,7 +102,7 @@ class FakeOdomNode
ros::NodeHandle private_nh("~");
private_nh.param("odom_frame_id", odom_frame_id_, std::string("odom"));
private_nh.param("base_frame_id", base_frame_id_, std::string("base_link"));
private_nh.param("global_frame_id", global_frame_id_, std::string("/map"));
private_nh.param("global_frame_id", global_frame_id_, std::string("map"));
private_nh.param("delta_x", delta_x_, 0.0);
private_nh.param("delta_y", delta_y_, 0.0);
private_nh.param("delta_yaw", delta_yaw_, 0.0);

89
autonomous_robotics_ros/src/navigation/move_base/CHANGELOG.rst
View File

@@ -1,89 +0,0 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package move_base
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.14.2 (2017-08-14)
-------------------
1.14.1 (2017-08-07)
-------------------
* Add a max_planning_retries parameter to move_base [kinetic] (`#539 <https://github.com/ros-planning/navigation/issues/539>`_)
* Fixed deadlock when changing global planner
* Fix CMakeLists + package.xmls (`#548 <https://github.com/ros-planning/navigation/issues/548>`_)
* Added deps to amcl costmap_2d move_base (`#512 <https://github.com/ros-planning/navigation/issues/512>`_)
* move_base: Add move_base_msgs to find_package.
* Contributors: Jorge Santos Simón, Maarten de Vries, Martin Günther, Vincent Rabaud, mryellow, ne0
1.14.0 (2016-05-20)
-------------------
1.13.1 (2015-10-29)
-------------------
* Removes installation of nonexistent directories
* use correct size for clearing window
* full name has been required for eons, this code just adds unneeded complexity
* remove ancient conversion scripts from v0.2 to v0.3
* proper locking during costmap update
* Contributors: Michael Ferguson, Thiago de Freitas Oliveira Araujo
1.13.0 (2015-03-17)
-------------------
* Fixing various memory freeing operations
* Contributors: Alex Bencz
1.12.0 (2015-02-04)
-------------------
* update maintainer email
* Contributors: Michael Ferguson
1.11.15 (2015-02-03)
--------------------
* Disable global planner when resetting state.
* Mark move_base headers for installation
* Add ARCHIVE DESTINATION for move_base
* Break infinite loop when tolerance 0 is used
* remove partial usage of <tab> in the code
* Contributors: Gary Servin, Michael Ferguson, ohendriks, v4hn
1.11.14 (2014-12-05)
--------------------
* use timer rather than rate for immediate wakeup
* adding lock to planner makePlan fail case
* Contributors: Michael Ferguson, phil0stine
1.11.13 (2014-10-02)
--------------------
1.11.12 (2014-10-01)
--------------------
1.11.11 (2014-07-23)
--------------------
* removes trailing spaces and empty lines
* Contributors: Enrique Fernández Perdomo
1.11.10 (2014-06-25)
--------------------
* Remove unnecessary colons
* move_base planService now searches out from desired goal
* Contributors: David Lu!!, Kaijen Hsiao
1.11.9 (2014-06-10)
-------------------
* uses ::hypot(x, y) instead of sqrt(x*x, y*y)
* Contributors: Enrique Fernández Perdomo
1.11.8 (2014-05-21)
-------------------
1.11.7 (2014-05-21)
-------------------
* update build to find eigen using cmake_modules
* Fix classloader warnings on exit of move_base
* Contributors: Michael Ferguson
1.11.4 (2013-09-27)
-------------------
* Package URL Updates
* Reintroduce ClearCostmaps Service
* Add dependencies to recovery behaviors.

15
autonomous_robotics_ros/src/navigation/move_base/CMakeLists.txt Executable file → Normal file
View File

@@ -20,7 +20,8 @@ find_package(catkin REQUIRED
rospy
rotate_recovery
std_srvs
tf
tf2_geometry_msgs
tf2_ros
)
find_package(Eigen3 REQUIRED)
add_definitions(${EIGEN3_DEFINITIONS})
@@ -64,11 +65,14 @@ set_target_properties(move_base_node PROPERTIES OUTPUT_NAME move_base)
install(
TARGETS
move_base
move_base_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(
TARGETS
move_base
DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)
## Mark cpp header files for installation
@@ -76,3 +80,4 @@ install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)

4
autonomous_robotics_ros/src/navigation/move_base/cfg/MoveBase.cfg Executable file → Normal file
View File

@@ -26,5 +26,9 @@ gen.add("shutdown_costmaps", bool_t, 0, "Determines whether or not to shutdown t
gen.add("oscillation_timeout", double_t, 0, "How long in seconds to allow for oscillation before executing recovery behaviors.", 0.0, 0, 60)
gen.add("oscillation_distance", double_t, 0, "How far in meters the robot must move to be considered not to be oscillating.", 0.5, 0, 10)
gen.add("make_plan_clear_costmap", bool_t, 0, "Whether or not to clear the global costmap on make_plan service call.", True)
gen.add("make_plan_add_unreachable_goal", bool_t, 0, "Whether or not to add the original goal to the path if it is unreachable in the make_plan service call.", True)
gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration", False)
exit(gen.generate(PACKAGE, "move_base_node", "MoveBase"))

15
autonomous_robotics_ros/src/navigation/move_base/include/move_base/move_base.h Executable file → Normal file
View File

@@ -53,7 +53,7 @@
#include <costmap_2d/costmap_2d.h>
#include <nav_msgs/GetPlan.h>
#include <pluginlib/class_loader.h>
#include <pluginlib/class_loader.hpp>
#include <std_srvs/Empty.h>
#include <dynamic_reconfigure/server.h>
@@ -87,7 +87,7 @@ namespace move_base {
* @param name The name of the action
* @param tf A reference to a TransformListener
*/
MoveBase(tf::TransformListener& tf);
MoveBase(tf2_ros::Buffer& tf);
/**
* @brief Destructor - Cleans up
@@ -164,6 +164,8 @@ namespace move_base {
bool isQuaternionValid(const geometry_msgs::Quaternion& q);
bool getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap);
double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2);
geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg);
@@ -173,7 +175,7 @@ namespace move_base {
*/
void wakePlanner(const ros::TimerEvent& event);
tf::TransformListener& tf_;
tf2_ros::Buffer& tf_;
MoveBaseActionServer* as_;
@@ -184,18 +186,20 @@ namespace move_base {
std::string robot_base_frame_, global_frame_;
std::vector<boost::shared_ptr<nav_core::RecoveryBehavior> > recovery_behaviors_;
std::vector<std::string> recovery_behavior_names_;
unsigned int recovery_index_;
tf::Stamped<tf::Pose> global_pose_;
geometry_msgs::PoseStamped global_pose_;
double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_;
double planner_patience_, controller_patience_;
int32_t max_planning_retries_;
uint32_t planning_retries_;
double conservative_reset_dist_, clearing_radius_;
ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_;
ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_, recovery_status_pub_;
ros::Subscriber goal_sub_;
ros::ServiceServer make_plan_srv_, clear_costmaps_srv_;
bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_;
bool make_plan_clear_costmap_, make_plan_add_unreachable_goal_;
double oscillation_timeout_, oscillation_distance_;
MoveBaseState state_;
@@ -233,3 +237,4 @@ namespace move_base {
};
#endif

63
autonomous_robotics_ros/src/navigation/move_base/package.xml Executable file → Normal file
View File

@@ -1,6 +1,8 @@
<package>
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>move_base</name>
<version>1.14.2</version>
<version>1.16.7</version>
<description>
The move_base package provides an implementation of an action (see the <a href="http://www.ros.org/wiki/actionlib">actionlib</a> package) that, given a goal in the world, will attempt to reach it with a mobile base. The move_base node links together a global and local planner to accomplish its global navigation task. It supports any global planner adhering to the nav_core::BaseGlobalPlanner interface specified in the <a href="http://www.ros.org/wiki/nav_core">nav_core</a> package and any local planner adhering to the nav_core::BaseLocalPlanner interface specified in the <a href="http://www.ros.org/wiki/nav_core">nav_core</a> package. The move_base node also maintains two costmaps, one for the global planner, and one for a local planner (see the <a href="http://www.ros.org/wiki/costmap_2d">costmap_2d</a> package) that are used to accomplish navigation tasks.
@@ -9,49 +11,38 @@
<author>Eitan Marder-Eppstein</author>
<author>contradict@gmail.com</author>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<maintainer email="mferguson@fetchrobotics.com">Michael Ferguson</maintainer>
<maintainer email="mfergs7@gmail.com">Michael Ferguson</maintainer>
<maintainer email="ahoy@fetchrobotics.com">Aaron Hoy</maintainer>
<license>BSD</license>
<url>http://wiki.ros.org/move_base</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>actionlib</build_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>costmap_2d</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>move_base_msgs</build_depend>
<build_depend>nav_core</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>pluginlib</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>tf</build_depend>
<build_depend>tf2_geometry_msgs</build_depend>
<depend>actionlib</depend>
<depend>costmap_2d</depend>
<depend>dynamic_reconfigure</depend>
<depend>geometry_msgs</depend>
<depend>move_base_msgs</depend>
<depend>nav_core</depend>
<depend>nav_msgs</depend>
<depend>pluginlib</depend>
<depend>roscpp</depend>
<depend>rospy</depend>
<depend>std_srvs</depend>
<depend>tf2_ros</depend>
<depend>visualization_msgs</depend>
<!--These deps aren't strictly needed, but given the default parameters require them to work, we'll enforce that they build -->
<build_depend>base_local_planner</build_depend>
<build_depend>clear_costmap_recovery</build_depend>
<build_depend>navfn</build_depend>
<build_depend>rotate_recovery</build_depend>
<run_depend>base_local_planner</run_depend>
<run_depend>clear_costmap_recovery</run_depend>
<run_depend>navfn</run_depend>
<run_depend>rotate_recovery</run_depend>
<depend>base_local_planner</depend>
<depend>clear_costmap_recovery</depend>
<depend>navfn</depend>
<depend>rotate_recovery</depend>
<run_depend>actionlib</run_depend>
<run_depend>costmap_2d</run_depend>
<run_depend>dynamic_reconfigure</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>move_base_msgs</run_depend>
<run_depend>nav_core</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>pluginlib</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_srvs</run_depend>
<run_depend>tf</run_depend>
<exec_depend>message_runtime</exec_depend>
</package>

0
autonomous_robotics_ros/src/navigation/move_base/planner_test.xml Executable file → Normal file
View File

Some files were not shown because too many files have changed in this diff Show More