turtlebot · jihoonl · Mar 11, 2015
diff --git a/turtlebot_apps/package.xml b/turtlebot_apps/package.xml
@@ -25,6 +25,7 @@
   <run_depend>turtlebot_follower</run_depend>
   <run_depend>turtlebot_navigation</run_depend>
   <run_depend>turtlebot_panorama</run_depend>
+  <run_depend>turtlebot_interactiver_markers</run_depend>
 
   <export>
     <metapackage/> 

diff --git a/turtlebot_interactive_markers/.cproject b/turtlebot_interactive_markers/.cproject
@@ -0,0 +1,43 @@
+<?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.1828323608">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.1828323608" 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.1828323608" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
+					<folderInfo id="cdt.managedbuild.toolchain.gnu.base.1828323608.1545079297" name="/" resourcePath="">
+						<toolChain id="cdt.managedbuild.toolchain.gnu.base.1674793227" 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.278061" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
+							<builder id="cdt.managedbuild.target.gnu.builder.base.47023294" managedBuildOn="false" name="Gnu Make Builder.Default" superClass="cdt.managedbuild.target.gnu.builder.base"/>
+							<tool id="cdt.managedbuild.tool.gnu.archiver.base.1239360010" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
+							<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.1923256535" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base"/>
+							<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.1064903291" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base"/>
+							<tool id="cdt.managedbuild.tool.gnu.c.linker.base.1588740816" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
+							<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.1386957786" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base"/>
+							<tool id="cdt.managedbuild.tool.gnu.assembler.base.948799480" 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="cdtBuildSystem" version="4.0.0">
+		<project id="turtlebot_interactive_markers.null.43598649" name="turtlebot_interactive_markers"/>
+	</storageModule>
+	<storageModule moduleId="scannerConfiguration">
+		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+	</storageModule>
+</cproject>
diff --git a/turtlebot_interactive_markers/.gitignore b/turtlebot_interactive_markers/.gitignore
@@ -0,0 +1,2 @@
+bin
+build
diff --git a/turtlebot_interactive_markers/.project b/turtlebot_interactive_markers/.project
@@ -0,0 +1,79 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>turtlebot_interactive_markers</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>
diff --git a/turtlebot_interactive_markers/CHANGELOG.rst b/turtlebot_interactive_markers/CHANGELOG.rst
@@ -0,0 +1,39 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package turtlebot_interactive_markers
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.3.2 (2015-01-21)
+------------------
+
+2.3.0 (2014-12-30)
+------------------
+
+2.2.2 (2013-10-25)
+------------------
+* Correct reference to renamed launcher.
+* Add missing install rule for the server.
+
+2.2.1 (2013-09-11)
+------------------
+
+2.2.0 (2013-08-30)
+------------------
+* Add bugtracker and repo info URLs.
+* Changelogs at package level.
+
+
+2.1.x - hydro, unstable
+=======================
+
+2.1.1 (2013-07-23)
+------------------
+
+2.1.0 (2013-07-16)
+------------------
+* Catkinized
+
+
+Previous versions, bugfixing
+============================
+
+Available in ROS wiki: http://ros.org/wiki/turtlebot_viz/ChangeList
diff --git a/turtlebot_interactive_markers/CMakeLists.txt b/turtlebot_interactive_markers/CMakeLists.txt
@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(turtlebot_interactive_markers)
+
+find_package(catkin REQUIRED COMPONENTS roscpp
+                                        visualization_msgs
+                                        interactive_markers)
+
+catkin_package()
+
+include_directories(${catkin_INCLUDE_DIRS})
+
+add_executable(turtlebot_marker_server src/turtlebot_marker_server.cpp)
+
+target_link_libraries(turtlebot_marker_server ${catkin_LIBRARIES})
+
+install(TARGETS turtlebot_marker_server DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+
+
diff --git a/turtlebot_interactive_markers/launch/interactive_markers.launch b/turtlebot_interactive_markers/launch/interactive_markers.launch
@@ -0,0 +1,15 @@
+<launch>
+  <!-- turtlebot_teleop_key already has its own built in velocity smoother -->
+  <node pkg="turtlebot_interactive_markers" type="turtlebot_marker_server" name="turtlebot_marker_server">
+    <remap from="cmd_vel" to="interactive_marker_velocity_smoother/raw_cmd_vel"/>
+  </node>
+
+  <!-- velocity smoother for the interactive marker server output -->
+  <node pkg="nodelet" type="nodelet" name="interactive_marker_velocity_smoother"
+        args="load yocs_velocity_smoother/VelocitySmootherNodelet /mobile_base_nodelet_manager">
+    <rosparam file="$(find turtlebot_bringup)/param/defaults/smoother.yaml" command="load"/>
+    <remap from="interactive_marker_velocity_smoother/odometry" to="/odom"/>
+    <remap from="interactive_marker_velocity_smoother/robot_cmd_vel" to="/mobile_base/commands/velocity"/>
+    <remap from="interactive_marker_velocity_smoother/smooth_cmd_vel" to="/cmd_vel_mux/input/teleop"/>
+  </node>
+</launch>
diff --git a/turtlebot_interactive_markers/package.xml b/turtlebot_interactive_markers/package.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package>
+  <name>turtlebot_interactive_markers</name>
+  <version>2.3.2</version>
+  <description>Interactive control for the TurtleBot using RViz and interactive markers</description>
+  <maintainer email="[email protected]">Daniel Stonier</maintainer>
+  <license>BSD</license>
+  <url type="website">http://ros.org/wiki/turtlebot_interactive_markers</url>
+  <url type="repository">https://github.com/turtlebot/turtlebot_apps</url>
+  <url type="bugtracker">https://github.com/turtlebot/turtlebot_apps/issues</url>
+  <author>Helen Oleynikova</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>interactive_markers</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>interactive_markers</run_depend>
+  <run_depend>turtlebot_bringup</run_depend>
+</package>
diff --git a/turtlebot_interactive_markers/src/turtlebot_marker_server.cpp b/turtlebot_interactive_markers/src/turtlebot_marker_server.cpp
@@ -0,0 +1,141 @@
+/*
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 the Willow Garage, Inc. 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.
+ */
+
+#include <ros/ros.h>
+#include <interactive_markers/interactive_marker_server.h>
+#include <string.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Pose.h>
+#include <tf/tf.h>
+
+using namespace visualization_msgs;
+
+class TurtlebotMarkerServer
+{
+  public:
+    TurtlebotMarkerServer()
+      : nh("~"), server("turtlebot_marker_server")
+    {
+      std::string cmd_vel_topic;
+
+      nh.param<std::string>("link_name", link_name, "/base_link");
+      nh.param<double>("linear_scale", linear_scale, 1.0);
+      nh.param<double>("angular_scale", angular_scale, 2.2);
+
+      vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
+      createInteractiveMarkers();
+
+      ROS_INFO("[turtlebot_marker_server] Initialized.");
+    }
+
+    void processFeedback(
+        const InteractiveMarkerFeedbackConstPtr &feedback );
+
+  private:
+    void createInteractiveMarkers();
+
+    ros::NodeHandle nh;
+    ros::Publisher vel_pub;
+    interactive_markers::InteractiveMarkerServer server;
+
+    double linear_scale;
+    double angular_scale;
+
+    std::string link_name;
+};
+
+void TurtlebotMarkerServer::processFeedback(
+    const InteractiveMarkerFeedbackConstPtr &feedback )
+{
+  // Handle angular change (yaw is the only direction in which you can rotate)
+  double yaw = tf::getYaw(feedback->pose.orientation);
+
+  geometry_msgs::Twist vel;
+  vel.angular.z = angular_scale*yaw;
+  vel.linear.x = linear_scale*feedback->pose.position.x;
+
+  vel_pub.publish(vel);    
+
+  // Make the marker snap back to turtlebot
+  server.setPose("turtlebot_marker", geometry_msgs::Pose());
+
+  server.applyChanges();
+}
+
+void TurtlebotMarkerServer::createInteractiveMarkers()
+{ 
+  // create an interactive marker for our server
+  InteractiveMarker int_marker;
+  int_marker.header.frame_id = link_name;
+  int_marker.name = "turtlebot_marker";
+  //int_marker.description = "Move the turtlebot";
+
+  InteractiveMarkerControl control;
+
+  control.orientation_mode = InteractiveMarkerControl::FIXED;
+  control.orientation.w = 1;
+  control.orientation.x = 1;
+  control.orientation.y = 0;
+  control.orientation.z = 0;
+  control.name = "move_x";
+  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
+  int_marker.controls.push_back(control);
+
+  control.orientation.w = 1;
+  control.orientation.x = 0;
+  control.orientation.y = 1;
+  control.orientation.z = 0;
+  control.name = "rotate_z";
+
+  control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
+  //control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
+  int_marker.controls.push_back(control);
+
+  // Commented out for non-holonomic turtlebot. If holonomic, can move in y.
+  /*control.orientation.w = 1;
+  control.orientation.x = 0;
+  control.orientation.y = 0;
+  control.orientation.z = 1;
+  control.name = "move_y";
+  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
+  int_marker.controls.push_back(control);*/
+
+  server.insert(int_marker, boost::bind( &TurtlebotMarkerServer::processFeedback, this, _1 ));
+
+  server.applyChanges();
+}
+
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "turtlebot_marker_server");
+  TurtlebotMarkerServer turtleserver;
+
+  ros::spin();
+}
diff --git a/turtlebot_rapps/package.xml b/turtlebot_rapps/package.xml
@@ -19,6 +19,7 @@
   <run_depend>turtlebot_navigation</run_depend>
   <run_depend>turtlebot_panorama</run_depend>
   <run_depend>turtlebot_teleop</run_depend>
+  <run_depend>turtlebot_interactive_markers</run_depend>
   <run_depend>robot_pose_publisher</run_depend>
 
   <!-- for navigation apps -->
@@ -46,6 +47,7 @@
     <rocon_app>rapps/panorama/panorama.rapp</rocon_app>
     <rocon_app>rapps/follower/follower.rapp</rocon_app>
     <rocon_app>rapps/3dsensor/3dsensor.rapp</rocon_app>
+    <rocon_app>rapps/interactive_markers_control/interactive_markers_control.rapp</rocon_app>
   </export>
 
 </package>