From 9e8ea3f81f22bd46a5f1c1770f15b1fc0423dcfc Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com>
Date: Mon, 9 May 2022 13:58:38 +0200
Subject: [PATCH] working coimunication
---
CMakeLists.txt | 22 +-
config/omniwheels_config.yaml | 6 +
launch/launch_controller.launch | 6 +
msg/motorCommand.msg | 1 +
package.xml | 4 +
scripts/crazzyflie_logger.py | 0
scripts/omni_wheel_controller_node.py | 220 ++++++++++++++++++
scripts/track_path (1).py | 306 ++++++++++++++++++++++++++
src/talker.cpp | 137 ------------
9 files changed, 561 insertions(+), 141 deletions(-)
create mode 100644 config/omniwheels_config.yaml
create mode 100644 launch/launch_controller.launch
create mode 100644 msg/motorCommand.msg
create mode 100644 scripts/crazzyflie_logger.py
create mode 100755 scripts/omni_wheel_controller_node.py
create mode 100644 scripts/track_path (1).py
delete mode 100644 src/talker.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 53cb85a..8c87e83 100644
--- a/CMakeLists.txt
+++ b/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}
+)
+
diff --git a/config/omniwheels_config.yaml b/config/omniwheels_config.yaml
new file mode 100644
index 0000000..0bdf958
--- /dev/null
+++ b/config/omniwheels_config.yaml
@@ -0,0 +1,6 @@
+# configuration file for all the omniwheels
+name: "omniwheel1"
+id: 1
+channel: 90 # change to the correct one
+type: default
+
diff --git a/launch/launch_controller.launch b/launch/launch_controller.launch
new file mode 100644
index 0000000..b6446ab
--- /dev/null
+++ b/launch/launch_controller.launch
@@ -0,0 +1,6 @@
+<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>
diff --git a/msg/motorCommand.msg b/msg/motorCommand.msg
new file mode 100644
index 0000000..22a1a5e
--- /dev/null
+++ b/msg/motorCommand.msg
@@ -0,0 +1 @@
+int32[3] data
diff --git a/package.xml b/package.xml
index 8cf772f..ac21299 100644
--- a/package.xml
+++ b/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>
diff --git a/scripts/crazzyflie_logger.py b/scripts/crazzyflie_logger.py
new file mode 100644
index 0000000..e69de29
diff --git a/scripts/omni_wheel_controller_node.py b/scripts/omni_wheel_controller_node.py
new file mode 100755
index 0000000..3691c1a
--- /dev/null
+++ b/scripts/omni_wheel_controller_node.py
@@ -0,0 +1,220 @@
+#!/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
diff --git a/scripts/track_path (1).py b/scripts/track_path (1).py
new file mode 100644
index 0000000..fae9aa5
--- /dev/null
+++ b/scripts/track_path (1).py
@@ -0,0 +1,306 @@
+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()
+
diff --git a/src/talker.cpp b/src/talker.cpp
deleted file mode 100644
index f0899ac..0000000
--- a/src/talker.cpp
+++ /dev/null
@@ -1,137 +0,0 @@
-/*
- * 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)%
--
GitLab