working coimunication

CMakeLists.txt

@@ -2,15 +2,29 @@ cmake_minimum_required(VERSION 2.8.3)
 project(omniwheels_controller)
 
 ## Find catkin and any catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)
-
+find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg geometry_msgs message_generation)
+add_message_files(
+ FILES
+ motorCommand.msg
+#   Message2.msg
+)
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
 ## Declare a catkin package
 catkin_package()
 
 ## Build talker and listener
 include_directories(include ${catkin_INCLUDE_DIRS})
 
-add_executable(talker src/talker.cpp)
-target_link_libraries(talker ${catkin_LIBRARIES})
+
+#add_executable(talker src/talker.cpp)
+#target_link_libraries(talker ${catkin_LIBRARIES})
+
+catkin_install_python(PROGRAMS scripts/omni_wheel_controller_node.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 
 

config/omniwheels_config.yaml

+# configuration file for all the omniwheels 
+name: "omniwheel1"
+id: 1
+channel: 90 # change to the correct one
+type: default
+

launch/launch_controller.launch

+<launch>
+	<rosparam command="load" file="$(find omniwheels_controller)/config/omniwheels_config.yaml"/>
+	<group ns = "omniwheel1">	
+		<node name="omniwheel_controller" pkg="omniwheels_controller" type="omni_wheel_controller_node.py" output="screen"/>
+	</group>
+</launch>

msg/motorCommand.msg

+int32[3] data

package.xml

@@ -23,15 +23,19 @@
   <build_depend>message_generation</build_depend>
   <build_depend>rosconsole</build_depend>
   <build_depend>roscpp</build_depend>
+  <build_depend>geometry_msgs</build_depend>
   <build_depend>roscpp_serialization</build_depend>
   <build_depend>rostime</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <run_depend>message_runtime</run_depend>
 
   <run_depend>libboost-date-time-dev</run_depend>
   <run_depend>libboost-thread-dev</run_depend>
   <run_depend>message_runtime</run_depend>
   <run_depend>rosconsole</run_depend>
   <run_depend>roscpp</run_depend>
+  <run_depend>geometry_msgs</run_depend>
   <run_depend>roscpp_serialization</run_depend>
   <run_depend>rostime</run_depend>
   <run_depend>std_msgs</run_depend>

scripts/omni_wheel_controller_node.py

+#!/usr/bin/env python3
+import rospy
+import time
+from std_msgs.msg import String
+from omniwheels_controller.msg import motorCommand
+from geometry_msgs.msg import Twist
+
+
+# Dynamixel packages
+from dynamixel.model.xm430_w210_t_r import XM430_W210_T_R
+import dynamixel.channel
+import time
+
+
+
+
+class CrazyLogger:
+    
+    """
+    Simple logging class that logs the Crazyflie Stabilizer from a supplied
+    link uri and disconnects after 60s.
+    """
+    # Define which states to log:
+    namelink = {'stateEstimate.x': 0, 'stateEstimate.y': 1,'stateEstimate.z': 2, 'stabilizer.yaw':3}
+    #_Offset for crazyflie position on robot to center
+    position_offset = 0.11
+
+    def __init__(self, link_uri):
+        """ Initialize and run the example with the specified link_uri """
+
+        self._cf = Crazyflie(rw_cache='./cache')
+        self._state = np.array([0.0,0.0,0.0,0.0])
+
+        # Connect some callbacks from the Crazyflie API
+        self._cf.connected.add_callback(self._connected)
+        self._cf.disconnected.add_callback(self._disconnected)
+        self._cf.connection_failed.add_callback(self._connection_failed)
+        self._cf.connection_lost.add_callback(self._connection_lost)
+
+        print('Connecting to %s' % link_uri)
+
+        # Try to connect to the Crazyflie
+        self._cf.open_link(link_uri)
+
+        # Variable used to keep main loop occupied until disconnect
+        self.is_connected = True
+
+    def _connected(self, link_uri):
+        """ This callback is called form the Crazyflie API when a Crazyflie
+        has been connected and the TOCs have been downloaded."""
+        print('Connected to %s' % link_uri)
+
+        # The definition of the logconfig can be made before connecting
+        self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=100)
+        self._lg_stab.add_variable('stateEstimate.x', 'float')
+        self._lg_stab.add_variable('stateEstimate.y', 'float')
+        self._lg_stab.add_variable('stateEstimate.z', 'float')
+        self._lg_stab.add_variable('stabilizer.yaw', 'float')
+        
+        # The fetch-as argument can be set to FP16 to save space in the log packet
+        #self._lg_stab.add_variable('pm.vbat', 'FP16')
+
+        # Adding the configuration cannot be done until a Crazyflie is
+        # connected, since we need to check that the variables we
+        # would like to log are in the TOC.
+        try:
+            self._cf.log.add_config(self._lg_stab)
+            # This callback will receive the data
+            self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
+            # This callback will be called on errors
+            self._lg_stab.error_cb.add_callback(self._stab_log_error)
+            #      # Start a timer to disconnect in 10s
+        t = Timer(60, self._cf.close_link)
+        t.start()
+
+    def _stab_log_error(self, logconf, msg):
+        """Callback from the log API when an error occurs"""
+        print('Error when logging %s: %s' % (logconf.name, msg))
+
+    def _stab_log_data(self, timestamp, data, logconf):
+        """Callback from a the log API when data arrives"""
+        #print(f'[{timestamp}][{logconf.name}]: ', end='')
+        for name, value in data.items():
+            self._state[self._namelink[name]] = value
+            #print(f'{name}: {value:3.3f} ', end='\n')
+        #print()
+
+    def _connection_failed(self, link_uri, msg):
+        """Callback when connection initial connection fails (i.e no Crazyflie
+        at the specified address)"""
+        print('Connection to %s failed: %s' % (link_uri, msg))
+        self.is_connected = False
+
+    def _connection_lost(self, link_uri, msg):
+        """Callback when disconnected after a connection has been made (i.e
+        Crazyflie moves out of range)"""
+        print('Connection to %s lost: %s' % (link_uri, msg))
+
+    def _disconnected(self, link_uri):
+        """Callback when the Crazyflie is disconnected (called in all cases)"""
+        print('Disconnected from %s' % link_uri)
+        self.is_connected = False
+        
+    def x(self):
+		""" Return x-position of robot center"""
+		# Note that the offset of the crazyflie mount position is included
+        return self._state[0] - np.sin(self._state[3])*self._position_offset 
+    
+    def y(self):
+		""" Return y-position of robot center"""
+		# Note that the offset of the crazyflie mount position is included
+        return self._state[1] + np.cos(self._state[3])*self._position_offset 
+    
+    def z(self):
+		""" Return z-position of robot center"""
+        return self._state[2]
+    
+    def theta(self):
+		""" Return direction of robot center, measured in radians between -pi and pi."""
+		# Note that the offset of the crazyflie mount position is included
+        return np.mod(self._state[3]*np.pi/180 + 11*np.pi/6,2*np.pi)-np.pi
+    
+    def states(self):
+        return self._state
+    
+    def close(self):
+        self._cf.close_link()
+        
+        
+
+class omniWheelvelocityController:
+    def __init__(self):
+        # Load the servos 
+        self.servos = self.load_servos()
+        self.motor_command = [0,0,0];
+        self.send_command = False;
+        self.vel_cmd_arrived = False;
+        self.motor_command[0] = 0;
+        self.motor_command[1] = 0;
+        self.motor_command[2] = 0; 
+        self.vel_cmd = Twist()
+        self.vel_cmd.linear.x = 0;
+        self.vel_cmd.linear.y = 0;
+        self.vel_cmd.angular.z = 0;
+        
+        # get ros param value 
+        robot_name = rospy.get_param("/name");
+
+
+      
+        
+        # subscriber 
+        #rospy.Subscriber("motor_command", motorCommand, self.motorVelocityCallback)
+        # todo add grounp ns 
+        rospy.Subscriber("vel_cmd", Twist, self.velocityCommandCallback)
+
+        
+        time.sleep(1) # Wait for connection to work
+        rospy.loginfo("Starting controller: " +  robot_name)
+        
+        # main loop 
+        while not rospy.is_shutdown():
+            
+            
+            if self.vel_cmd_arrived == True:
+                # convert velocity command to motor command 
+                self.motor_command[0] = self.vel_cmd.linear.x
+                self.motor_command[1] = self.vel_cmd.linear.y
+                self.motor_command[2] = self.vel_cmd.angular.z
+                self.vel_cmd_arrived = False
+                
+                
+            else:
+                self.motor_command[0] = 0
+                self.motor_command[1] = 0
+                self.motor_command[2] = 0
+                
+            # Send command to the motors
+            for (j,s) in enumerate(self.servos):
+                s.goal_velocity.write(round(self.motor_command[j]))
+        
+
+    def load_servos(self):
+        """
+        Servo load function, based on the dynamixel implementation of Anders Blomdell.
+        """
+        # The speed and channel numbers can be found and checked via the dynamixel software "dynamixel wizard".
+        # The device can be found by e.g. lsusb, and is currently adapted for the three-color robots.
+
+        channel = dynamixel.channel.Channel(speed=57600,device='/dev/ttyACM0')
+        servos = [ XM430_W210_T_R(channel, 1),
+                   XM430_W210_T_R(channel, 2),
+                   XM430_W210_T_R(channel, 3) ]
+        for s in servos:
+            s.torque_enable.write(0)
+            print(s.model_number.read(), s.id.read())
+            s.operating_mode.write(1)
+            s.bus_watchdog.write(0) # Clear old watchdog error
+            s.bus_watchdog.write(100) # 2 second timeout
+            s.torque_enable.write(1)
+            
+        return servos
+
+    def motorVelocityCallback(self, data):
+        self.send_command = True
+        self.motor_command[0] = data.data[0];
+        self.motor_command[1] = data.data[1];
+        self.motor_command[2] = data.data[2];
+    def velocityCommandCallback(self,data):
+        self.vel_cmd_arrived = True
+        self.vel_cmd = data
+
+        
+
+if __name__ == '__main__':
+    rospy.init_node("omni_wheel_controller_node")
+    try:
+        omniwheel1 = omniWheelvelocityController()
+    except rospy.ROSInterruptException:
+        pass

scripts/track_path (1).py

+import numpy as np
+import pandas as pd
+import sys
+
+# Dynamixel packages
+from dynamixel.model.xm430_w210_t_r import XM430_W210_T_R
+import dynamixel.channel
+import time
+
+#Crazy packages
+import logging
+from threading import Timer
+
+import cflib.crtp  # noqa
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+from cflib.utils import uri_helper
+
+logging.basicConfig(level=logging.ERROR)
+
+
+########################################################################
+# 				SUPPORTING CLASSES AND FUNCTIONS
+########################################################################
+
+def load_servos():
+"""
+Servo load function, based on the dynamixel implementation of Anders Blomdell.
+"""
+
+# The speed and channel numbers can be found and checked via the dynamixel software "dynamixel wizard".
+# The device can be found by e.g. lsusb, and is currently adapted for the three-color robots.
+
+    channel = dynamixel.channel.Channel(speed=57600,device='/dev/ttyACM0')
+    servos = [ XM430_W210_T_R(channel, 1),
+               XM430_W210_T_R(channel, 2),
+               XM430_W210_T_R(channel, 3) ]
+    for s in servos:
+        s.torque_enable.write(0)
+        print(s.model_number.read(), s.id.read())
+        s.operating_mode.write(1)
+        s.bus_watchdog.write(0) # Clear old watchdog error
+        s.bus_watchdog.write(100) # 2 second timeout
+        s.torque_enable.write(1)
+        
+    return servos
+
+class SimplePI:
+    """ 
+    Simple PI implementation with basic anti-windup. The _saturation - variable sets the saturation of the anti-windup.
+    """
+    
+    _saturation = 0.1
+    
+    def __init__(self,kp,ki,dt):
+        """ Initialize the controller """
+
+        self.e = np.zeros(3)
+        self.dt = dt # Sample rate
+        self.kp = kp # P-part gain
+        self.ki = ki # I-part gain
+    
+    def update_control(self,e):
+        self.e += e*self.dt # Update cumulative error
+        
+        # Saturate internal error for anti-windup
+        self.e = np.sign(self.e)*np.array([min(ei,self._saturation) for ei in abs(self.e)])
+        
+        # Return control signal
+        return self.kp*e + self.ki*self.e
+     
+
+class CrazyLogger:
+    
+    """
+    Simple logging class that logs the Crazyflie Stabilizer from a supplied
+    link uri and disconnects after 60s.
+    """
+    # Define which states to log:
+    namelink = {'stateEstimate.x': 0, 'stateEstimate.y': 1,'stateEstimate.z': 2, 'stabilizer.yaw':3}
+    #_Offset for crazyflie position on robot to center
+    position_offset = 0.11
+
+    def __init__(self, link_uri):
+        """ Initialize and run the example with the specified link_uri """
+
+        self._cf = Crazyflie(rw_cache='./cache')
+        self._state = np.array([0.0,0.0,0.0,0.0])
+
+        # Connect some callbacks from the Crazyflie API
+        self._cf.connected.add_callback(self._connected)
+        self._cf.disconnected.add_callback(self._disconnected)
+        self._cf.connection_failed.add_callback(self._connection_failed)
+        self._cf.connection_lost.add_callback(self._connection_lost)
+
+        print('Connecting to %s' % link_uri)
+
+        # Try to connect to the Crazyflie
+        self._cf.open_link(link_uri)
+
+        # Variable used to keep main loop occupied until disconnect
+        self.is_connected = True
+
+    def _connected(self, link_uri):
+        """ This callback is called form the Crazyflie API when a Crazyflie
+        has been connected and the TOCs have been downloaded."""
+        print('Connected to %s' % link_uri)
+
+        # The definition of the logconfig can be made before connecting
+        self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=100)
+        self._lg_stab.add_variable('stateEstimate.x', 'float')
+        self._lg_stab.add_variable('stateEstimate.y', 'float')
+        self._lg_stab.add_variable('stateEstimate.z', 'float')
+        self._lg_stab.add_variable('stabilizer.yaw', 'float')
+        
+        # The fetch-as argument can be set to FP16 to save space in the log packet
+        #self._lg_stab.add_variable('pm.vbat', 'FP16')
+
+        # Adding the configuration cannot be done until a Crazyflie is
+        # connected, since we need to check that the variables we
+        # would like to log are in the TOC.
+        try:
+            self._cf.log.add_config(self._lg_stab)
+            # This callback will receive the data
+            self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
+            # This callback will be called on errors
+            self._lg_stab.error_cb.add_callback(self._stab_log_error)
+            # Start the logging
+            self._lg_stab.start()
+        except KeyError as e:
+            print('Could not start log configuration,'
+                  '{} not found in TOC'.format(str(e)))
+        except AttributeError:
+            print('Could not add Stabilizer log config, bad configuration.')
+
+        # Start a timer to disconnect in 10s
+        t = Timer(60, self._cf.close_link)
+        t.start()
+
+    def _stab_log_error(self, logconf, msg):
+        """Callback from the log API when an error occurs"""
+        print('Error when logging %s: %s' % (logconf.name, msg))
+
+    def _stab_log_data(self, timestamp, data, logconf):
+        """Callback from a the log API when data arrives"""
+        #print(f'[{timestamp}][{logconf.name}]: ', end='')
+        for name, value in data.items():
+            self._state[self._namelink[name]] = value
+            #print(f'{name}: {value:3.3f} ', end='\n')
+        #print()
+
+    def _connection_failed(self, link_uri, msg):
+        """Callback when connection initial connection fails (i.e no Crazyflie
+        at the specified address)"""
+        print('Connection to %s failed: %s' % (link_uri, msg))
+        self.is_connected = False
+
+    def _connection_lost(self, link_uri, msg):
+        """Callback when disconnected after a connection has been made (i.e
+        Crazyflie moves out of range)"""
+        print('Connection to %s lost: %s' % (link_uri, msg))
+
+    def _disconnected(self, link_uri):
+        """Callback when the Crazyflie is disconnected (called in all cases)"""
+        print('Disconnected from %s' % link_uri)
+        self.is_connected = False
+        
+    def x(self):
+		""" Return x-position of robot center"""
+		# Note that the offset of the crazyflie mount position is included
+        return self._state[0] - np.sin(self._state[3])*self._position_offset 
+    
+    def y(self):
+		""" Return y-position of robot center"""
+		# Note that the offset of the crazyflie mount position is included
+        return self._state[1] + np.cos(self._state[3])*self._position_offset 
+    
+    def z(self):
+		""" Return z-position of robot center"""
+        return self._state[2]
+    
+    def theta(self):
+		""" Return direction of robot center, measured in radians between -pi and pi."""
+		# Note that the offset of the crazyflie mount position is included
+        return np.mod(self._state[3]*np.pi/180 + 11*np.pi/6,2*np.pi)-np.pi
+    
+    def states(self):
+        return self._state
+    
+    def close(self):
+        self._cf.close_link()
+          
+########################################################################
+# 					MAIN SCRIPT
+########################################################################        
+if __name__ == '__main__':
+    
+    # Read input trajectory filename
+    if len(sys.argv) > 1:
+        print("Input file name:",sys.argv[1])
+        filename = sys.argv[1]
+    else:
+        print("No file name inupt. Using default file name: trajectory.csv")
+        filename = "trajectory.csv"
+
+	# Load trajectory
+    print("Attempting to load trajectory file",filename)
+    trajectory = pd.read_csv(filename)
+    print("\nTrajectory loaded.")
+    x = trajectory.x.to_numpy()
+    y = trajectory.y.to_numpy()
+    # Update theta to be in correct range (-pi,pi)
+    theta = ((trajectory.theta + np.pi).mod(2*np.pi)-np.pi).to_numpy() # Put theta in range -pi -> pi instead of 0 -> 2pi
+    
+    #Find derivatives
+    trajectory[["xdot","ydot","thetadot"]] = trajectory[["x","y","theta"]].diff(axis = 0).fillna(0)/0.1
+        
+    #FUDGE FACTOR TO DECREASE SPEED FOR TESTING!!!!
+    # Should probably be removed in future iterations. Was included because
+    # the robot was exceeding servo speed settings.
+    fudgefactor = 3.0
+    velref = trajectory[['xdot','ydot','thetadot']].to_numpy()/fudgefactor
+    
+    # Create time vector
+    dt = trajectory.t.diff().mean()*fudgefactor
+    t = trajectory.t.to_numpy()*fudgefactor
+    tend = t[-1]
+    n = t.size
+    print("Sample time:",dt)
+    print("Experiment time:",tend)
+    print("Time steps:",n)
+
+	# Read controller parameters
+    if len(sys.argv) > 3:
+        print("\nController parameters:")
+        kp = float(sys.argv[2])
+        ki = float(sys.argv[3])
+    else:
+        print("\nNo controller paramater input. Using default values:")
+        kp = 0.1
+        ki = 0.01
+
+    print("kp =",kp)
+    print("ki =",ki)
+
+	
+	# Robot parameters
+    R = 0.16 		# Distance between center of robot and wheels
+    a1 = 2*np.pi/3 	# Angle between x axis and first wheel
+    a2 = 4*np.pi/3  # Angle between x axis and second wheel
+    r = 0.028*0.45/18 # Wheel radius. Has been fudge-factored because the actual velocity of the wheels did not align with the set-points.
+
+    print("Bot dimensions:")
+    print("R =",R)
+    print("R =",r)
+    
+    def phidot(xdot,ang):
+        """Returns reference velocities for the wheels, given current system state and reference velocities"""
+        M = -1/r*np.array([[-np.sin(ang), np.cos(ang), R ],[-np.sin(ang+a1), np.cos(ang+a1), R],[-np.sin(ang+a2), np.cos(ang+a2), R]])
+        return M.dot(xdot)    
+    
+    
+    # Initiate experiment
+    print('Initiating experiment')
+    servos = load_servos()			# Use predefined funtion to initiate servo connection
+    cflib.crtp.init_drivers()       # Initiate drivers for crazyflie
+    uri = uri_helper.uri_from_env(default='usb://0') # Connection-uri for crazyflie via USB
+    cl = CrazyLogger(uri)           # Create a crazyflie-based logger
+    pi = SimplePI(kp,ki,dt)         # Create a PI-controller 
+
+
+    time.sleep(1) # Wait for connection to work
+    t0 = time.time()
+    
+    # Main control loop
+    for i in range(n-1):
+		
+		# Read current position error:
+        e = np.array([trajectory.x.iloc[i]-cl.x(),trajectory.y.iloc[i]-cl.y(),trajectory.theta.iloc[i]-cl.theta()])
+        
+        # Create reference speed through feed-forward (velref) and feedback (pi):
+        xdot = velref[i,:] + pi.update_control(e)
+        
+        # Transform from x,y,theta-speeds to wheel rotational speeds.
+        ph = phidot(xdot,cl.theta())
+        
+        # Set all servospeeds (enact control signal)
+		for (j,s) in enumerate(servos):
+            s.goal_velocity.write(round(ph[j]))
+        
+        # Printing position and tracking error
+        print("x:",cl.x(),"\t y:",cl.y(),"\t theta:",cl.theta())
+        print("Error:",e,"\n")
+            
+		# Wait until next loop-iteration
+        time.sleep(max(t[i+1]+t0-time.time(),0))
+        
+        
+        
+    # Shutdown
+    print("Experiment done, shutting down servos and logger")
+    for s in servos:
+        s.goal_velocity.write(0)
+    print("Finishing position: x =",cl.x(),"\t y= ",cl.y(),"\t theta =",cl.theta())
+    cl.close()    
+

src/talker.cpp

-/*
- * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
- *
- * 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 names of Stanford University or 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.
- */
-// %Tag(FULLTEXT)%
-// %Tag(ROS_HEADER)%
-#include "ros/ros.h"
-// %EndTag(ROS_HEADER)%
-// %Tag(MSG_HEADER)%
-#include "std_msgs/String.h"
-// %EndTag(MSG_HEADER)%
-
-#include <sstream>
-
-/**
- * This tutorial demonstrates simple sending of messages over the ROS system.
- */
-int main(int argc, char **argv)
-{
-  /**
-   * The ros::init() function needs to see argc and argv so that it can perform
-   * any ROS arguments and name remapping that were provided at the command line.
-   * For programmatic remappings you can use a different version of init() which takes
-   * remappings directly, but for most command-line programs, passing argc and argv is
-   * the easiest way to do it.  The third argument to init() is the name of the node.
-   *
-   * You must call one of the versions of ros::init() before using any other
-   * part of the ROS system.
-   */
-// %Tag(INIT)%
-  ros::init(argc, argv, "talker");
-// %EndTag(INIT)%
-
-  /**
-   * NodeHandle is the main access point to communications with the ROS system.
-   * The first NodeHandle constructed will fully initialize this node, and the last
-   * NodeHandle destructed will close down the node.
-   */
-// %Tag(NODEHANDLE)%
-  ros::NodeHandle n;
-// %EndTag(NODEHANDLE)%
-
-  /**
-   * The advertise() function is how you tell ROS that you want to
-   * publish on a given topic name. This invokes a call to the ROS
-   * master node, which keeps a registry of who is publishing and who
-   * is subscribing. After this advertise() call is made, the master
-   * node will notify anyone who is trying to subscribe to this topic name,
-   * and they will in turn negotiate a peer-to-peer connection with this
-   * node.  advertise() returns a Publisher object which allows you to
-   * publish messages on that topic through a call to publish().  Once
-   * all copies of the returned Publisher object are destroyed, the topic
-   * will be automatically unadvertised.
-   *
-   * The second parameter to advertise() is the size of the message queue
-   * used for publishing messages.  If messages are published more quickly
-   * than we can send them, the number here specifies how many messages to
-   * buffer up before throwing some away.
-   */
-// %Tag(PUBLISHER)%
-  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
-// %EndTag(PUBLISHER)%
-
-// %Tag(LOOP_RATE)%
-  ros::Rate loop_rate(10);
-// %EndTag(LOOP_RATE)%
-
-  /**
-   * A count of how many messages we have sent. This is used to create
-   * a unique string for each message.
-   */
-// %Tag(ROS_OK)%
-  int count = 0;
-  while (ros::ok())
-  {
-// %EndTag(ROS_OK)%
-    /**
-     * This is a message object. You stuff it with data, and then publish it.
-     */
-// %Tag(FILL_MESSAGE)%
-    std_msgs::String msg;
-
-    std::stringstream ss;
-    ss << "hello world " << count;
-    msg.data = ss.str();
-// %EndTag(FILL_MESSAGE)%
-
-// %Tag(ROSCONSOLE)%
-    ROS_INFO("%s", msg.data.c_str());
-// %EndTag(ROSCONSOLE)%
-
-    /**
-     * The publish() function is how you send messages. The parameter
-     * is the message object. The type of this object must agree with the type
-     * given as a template parameter to the advertise<>() call, as was done
-     * in the constructor above.
-     */
-// %Tag(PUBLISH)%
-    chatter_pub.publish(msg);
-// %EndTag(PUBLISH)%
-
-// %Tag(SPINONCE)%
-    ros::spinOnce();
-// %EndTag(SPINONCE)%
-
-// %Tag(RATE_SLEEP)%
-    loop_rate.sleep();
-// %EndTag(RATE_SLEEP)%
-    ++count;
-  }
-
-
-  return 0;
-}
-// %EndTag(FULLTEXT)%