RoboBuggy

1. Build TurtleSim

------------------

   Run:

    cd ~/buggyworkspace

    catkin_make

    # We have now build the binaries; we will now overlay these on our 

    #   workspace. 

    cd ~/buggyworkspace/devel    

    source setup.bash

2. Run TurtleSim

----------------

We will now run turtlesim. This is, once again, a paraphrasing of the 

turtlesim tutorial, that can be found at http://wiki.ros.org/ROS/Tutorials/UnderstandingTopics . 

  Run:

   # Make sure you ran /deve/setup.bash, so the right binaries are run. 

   # Start up the core service

   roscore

   # Start up the rostopic viewer. 

   # This will let you watch as the new nodes come online, and you can see

   #   what topics and services they offer. 

   rqt_graph

   #   (i recommend unhiding all topics, to get a better view)

   # In a new terminal

   # This command will start 

   rosrun turtlesim_ref turtlesim_node

   # Run some commands to investigate

   rosnode list

   rosnode info /rosout

   # Read from a running topic

   rostopic hz /turtle1/pose # see update freq.

   rostopic bw /turtle1/pose # see bandwidth

   rostopic echo /turtle1/pose

cmake_minimum_required(VERSION 2.8.3)

project(turtlesim_ref)

#project(turtlesim)

find_package(catkin REQUIRED COMPONENTS message_generation roscpp roslib rosconsole std_msgs std_srvs rostime roscpp_serialization)

find_package(Qt4 4.6 REQUIRED COMPONENTS QtCore QtGui)

include(${QT_USE_FILE})

find_package(Boost REQUIRED COMPONENTS thread)

include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${QT_INCLUDE_DIRS})

link_directories(${catkin_LIBRARY_DIRS})

add_message_files(DIRECTORY msg FILES 

                  Color.msg Pose.msg Velocity.msg)

add_service_files(DIRECTORY srv FILES

                  Kill.srv

                  SetPen.srv

                  Spawn.srv

                  TeleportAbsolute.srv

                  TeleportRelative.srv)

generate_messages(DEPENDENCIES std_msgs std_srvs)

catkin_package(CATKIN_DEPENDS message_runtime std_msgs std_srvs)

# Set up the dependencies for turtlesim_node

set(turtlesim_node_SRCS

  src/turtlesim.cpp

  src/turtle.cpp

  src/turtle_frame.cpp

)

set(turtlesim_node_HDRS

  include/turtlesim/turtle_frame.h

)

# apparently this is just what you do with QT

qt4_wrap_cpp(turtlesim_node_MOCS ${turtlesim_node_HDRS})

# Build the turtlesim node?

add_executable(turtlesim_node ${turtlesim_node_SRCS} ${turtlesim_node_MOCS})

target_link_libraries(turtlesim_node ${QT_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES})

add_dependencies(turtlesim_node turtlesim_gencpp)

# Build the teleop...thing. I thought it was a package, 

#  it obviously isn't

add_executable(turtle_teleop_key tutorials/teleop_turtle_key.cpp)

target_link_libraries(turtle_teleop_key ${catkin_LIBRARIES})

add_dependencies(turtle_teleop_key turtlesim_gencpp)

# Build draw_square. so many words

add_executable(draw_square tutorials/draw_square.cpp)

target_link_libraries(draw_square ${catkin_LIBRARIES} ${Boost_LIBRARIES})

add_dependencies(draw_square turtlesim_gencpp)

# but these words I know, it spawns a turtle that does follor the leader

add_executable(mimic tutorials/mimic.cpp)

target_link_libraries(mimic ${catkin_LIBRARIES})

add_dependencies(mimic turtlesim_gencpp)

install(TARGETS turtlesim_node turtle_teleop_key draw_square mimic

  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

install(DIRECTORY images

  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}

  FILES_MATCHING PATTERN "*.png")

How to build this thing

=======================

Run catkin_make at the top level. 

The binaries will be in devel; 

    - to use them, source the setup.bash file in devel.

       this will drag the things you just built into your packagepath

Change the source, recompile, continue!

Note that if you want any of your stuff built, you have to 

add it to the cmake file. 

ros-groovy-turtlesim (0.3.13-0precise) precise; urgency=high

  * Autogenerated, no changelog for this version found in CHANGELOG.rst.

 -- Dirk Thomas <dthomas@osrfoundation.org>  Thu, 05 Sep 2013 22:29:39 -0000

7

Source: ros-groovy-turtlesim

Section: misc

Priority: extra

Maintainer: Dirk Thomas <dthomas@osrfoundation.org>

Build-Depends: debhelper (>= 7.0.50~), libqt4-dev, qt4-qmake, ros-groovy-catkin, ros-groovy-message-generation, ros-groovy-rosconsole, ros-groovy-roscpp, ros-groovy-roscpp-serialization, ros-groovy-roslib, ros-groovy-rostime, ros-groovy-std-msgs, ros-groovy-std-srvs

Homepage: http://www.ros.org/wiki/turtlesim

Standards-Version: 3.9.2

Package: ros-groovy-turtlesim

Architecture: any

Depends: ${shlibs:Depends}, ${misc:Depends}, libqt4-dbus, libqt4-network, libqt4-script, libqt4-test, libqt4-xml, libqtcore4, ros-groovy-message-runtime, ros-groovy-rosconsole, ros-groovy-roscpp, ros-groovy-roscpp-serialization, ros-groovy-roslib, ros-groovy-rostime, ros-groovy-std-msgs, ros-groovy-std-srvs

Description: turtlesim is a tool made for teaching ROS and ROS packages.

[git-buildpackage]

upstream-branch=release/groovy/turtlesim/0.3.13-0

upstream-tree=branch

#!/usr/bin/make -f

# -*- makefile -*-

# Sample debian/rules that uses debhelper.

# This file was originally written by Joey Hess and Craig Small.

# As a special exception, when this file is copied by dh-make into a

# dh-make output file, you may use that output file without restriction.

# This special exception was added by Craig Small in version 0.37 of dh-make.

# Uncomment this to turn on verbose mode.

export DH_VERBOSE=1

export DH_OPTIONS=-v --buildsystem=cmake

# TODO: remove the LDFLAGS override.  It's here to avoid esoteric problems

# of this sort:

#  https://code.ros.org/trac/ros/ticket/2977

#  https://code.ros.org/trac/ros/ticket/3842

export LDFLAGS=

export PKG_CONFIG_PATH=/opt/ros/groovy/lib/pkgconfig

%:

	dh  $@

override_dh_auto_configure:

	# In case we're installing to a non-standard location, look for a setup.sh

	# in the install tree that was dropped by catkin, and source it.  It will

	# set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH.

	if [ -f "/opt/ros/groovy/setup.sh" ]; then . "/opt/ros/groovy/setup.sh"; fi && \

	dh_auto_configure -- \

		-DCATKIN_BUILD_BINARY_PACKAGE="1" \

		-DCMAKE_INSTALL_PREFIX="/opt/ros/groovy" \

		-DCMAKE_PREFIX_PATH="/opt/ros/groovy"

override_dh_auto_build:

	# In case we're installing to a non-standard location, look for a setup.sh

	# in the install tree that was dropped by catkin, and source it.  It will

	# set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH.

	if [ -f "/opt/ros/groovy/setup.sh" ]; then . "/opt/ros/groovy/setup.sh"; fi && \

	dh_auto_build

override_dh_auto_test:

	# In case we're installing to a non-standard location, look for a setup.sh

	# in the install tree that was dropped by catkin, and source it.  It will

	# set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH.

	echo -- Running tests. Even if one of them fails the build is not canceled.

	if [ -f "/opt/ros/groovy/setup.sh" ]; then . "/opt/ros/groovy/setup.sh"; fi && \

	dh_auto_test || true

override_dh_shlibdeps:

	# In case we're installing to a non-standard location, look for a setup.sh

	# in the install tree that was dropped by catkin, and source it.  It will

	# set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH.

	if [ -f "/opt/ros/groovy/setup.sh" ]; then . "/opt/ros/groovy/setup.sh"; fi && \

	dh_shlibdeps -l$(CURDIR)/debian/ros-groovy-turtlesim//opt/ros/groovy/lib/

3.0 (quilt)

<?xml version="1.0" encoding="utf-8"?>

<!-- Generator: Adobe Illustrator 16.0.4, SVG Export Plug-In . SVG Version: 6.00 Build 0)  -->

<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">

<svg version="1.1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" x="0px" y="0px" width="45px"

	 height="45px" viewBox="0 0 45 45" enable-background="new 0 0 45 45" xml:space="preserve">

<g id="head1">

	<path fill="none" stroke="#A2C880" stroke-width="0.4" stroke-miterlimit="10" d="M17.829,14.402"/>

	<line fill="#A2C880" x1="14.885" y1="16.83" x2="15.303" y2="16.522"/>

	<path fill="#A2C880" d="M19.095,14.533"/>

	<line fill="#A2C880" x1="29.754" y1="16.83" x2="29.336" y2="16.522"/>

	<path fill="#A2C880" d="M25.544,14.533"/>

	<g>

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-width="0.2366" stroke-miterlimit="10" d="M23.826,6.305"/>

		</g>

	</g>

	<path fill="none" stroke="#A2C880" stroke-width="0.4" stroke-miterlimit="10" d="M22.5,0.625"/>

	<path fill="#A2C880" stroke="#375219" stroke-width="1.5" stroke-miterlimit="10" d="M20.071,14.002

		c1.048-3.341-0.434-5.89-0.434-5.89c0.166,0.099-1.743-1.642-1.315-3.514C18.77,2.635,22.3,1.315,22.3,1.315

		s2.999,0.788,3.959,2.89s-1.107,3.981-1.271,3.912c0,0-1.466,3.42-0.276,5.53"/>

	<path fill="#231F20" d="M21.014,3.291c0,0-1.151,0.479-1.072,1.072c0.081,0.596,0.873,0.397,1.072,0.277

		c0.198-0.119,0.794-0.555,0.636-0.993c-0.161-0.435-0.161-0.435-0.161-0.435"/>

	<path fill="#231F20" d="M23.619,3.291c0,0,1.15,0.479,1.072,1.072c-0.078,0.596-0.873,0.397-1.072,0.277

		c-0.199-0.119-0.795-0.555-0.637-0.993c0.162-0.435,0.162-0.435,0.162-0.435"/>

	<g>

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-width="0.2366" stroke-miterlimit="10" d="M22.04,4.491"/>

		</g>

	</g>

	<g>

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-width="0.2366" stroke-miterlimit="10" d="M22.631,4.176"/>

		</g>

	</g>

	<g>

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-width="0.2366" stroke-miterlimit="10" d="M25.036,4.491"/>

		</g>

	</g>

	<g>

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-width="0.2366" stroke-miterlimit="10" d="M25.47,5.359"/>

		</g>

	</g>

	<path fill="none" stroke="#A2C880" stroke-width="0.4" stroke-miterlimit="10" d="M34.05,4.562"/>

	<path fill="#A2C880" stroke="#375219" stroke-width="1.5" stroke-miterlimit="10" d="M25.271,14.436

		c2.6-2.347,2.616-5.295,2.616-5.295c0.093,0.169-0.667-2.299,0.652-3.694c1.382-1.464,5.095-0.809,5.095-0.809

		s2.183,2.201,1.943,4.5s-2.976,2.868-3.082,2.725c0,0-3,2.202-3.046,4.624"/>

	<g>

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-width="0.2366" stroke-miterlimit="10" d="M31.689,7.659"/>

		</g>

	</g>

	<g>

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-width="0.2366" stroke-miterlimit="10" d="M32.359,7.688"/>

		</g>

	</g>

	<g>

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-width="0.2366" stroke-miterlimit="10" d="M34.271,9.181"/>

		</g>

	</g>

	<g>

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-width="0.2366" stroke-miterlimit="10" d="M34.205,10.148"/>

		</g>

	</g>

	<path fill="none" stroke="#A2C880" stroke-width="0.4" stroke-miterlimit="10" d="M10.817,4.562"/>

	<path fill="#A2C880" stroke="#375219" stroke-width="1.5" stroke-miterlimit="10" d="M19.597,14.436

		c-2.6-2.347-2.616-5.295-2.616-5.295c-0.093,0.169,0.667-2.299-0.652-3.694c-1.382-1.464-5.095-0.809-5.095-0.809

		s-2.183,2.201-1.943,4.5s2.976,2.868,3.082,2.725c0,0,3,2.202,3.046,4.624"/>

	<path fill="#231F20" d="M13.375,5.737c0,0,1.234-0.174,1.467,0.379c0.233,0.554-0.551,0.786-0.781,0.783

		c-0.232-0.002-0.967-0.075-1.054-0.533C12.925,5.91,12.925,5.91,12.925,5.91"/>

	<path fill="#231F20" d="M10.937,7.108c0,0-0.75,0.996-0.38,1.467c0.368,0.474,0.954-0.101,1.063-0.305

		c0.109-0.203,0.402-0.882,0.044-1.179c-0.36-0.293-0.36-0.293-0.36-0.293"/>

	<path fill="#231F20" d="M31.464,5.737c0,0-1.234-0.174-1.467,0.379c-0.233,0.554,0.551,0.786,0.781,0.783

		c0.232-0.002,0.967-0.075,1.054-0.533c0.082-0.456,0.082-0.456,0.082-0.456"/>

	<path fill="#231F20" d="M33.902,7.108c0,0,0.75,0.996,0.38,1.467c-0.368,0.474-0.954-0.101-1.063-0.305

		c-0.109-0.203-0.402-0.882-0.044-1.179c0.36-0.293,0.36-0.293,0.36-0.293"/>

	<g>

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-width="0.2366" stroke-miterlimit="10" d="M13.178,7.659"/>

		</g>

	</g>

	<g>

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-width="0.2366" stroke-miterlimit="10" d="M12.508,7.688"/>

		</g>

	</g>

	<g>

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-width="0.2366" stroke-miterlimit="10" d="M10.596,9.181"/>

		</g>

	</g>

	<g>

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-width="0.2366" stroke-miterlimit="10" d="M10.662,10.148"/>

		</g>

	</g>

</g>

<g id="head2" display="none">

	<path display="inline" fill="#A2C880" d="M19.542,13.598c0.175,0.104-1.841-1.735-1.39-3.713c0.473-2.075,4.205-3.47,4.205-3.47

		s3.168,0.833,4.184,3.054c1.016,2.221-1.17,4.208-1.344,4.134"/>

	<path display="inline" fill="#231F20" d="M19.781,8.012c0,0-0.81,0.336-0.754,0.755s0.614,0.279,0.754,0.195

		s0.56-0.391,0.448-0.699c-0.113-0.308-0.113-0.308-0.113-0.308"/>

	<path display="inline" fill="#231F20" d="M24.956,8.012c0,0,0.811,0.336,0.756,0.755c-0.057,0.419-0.617,0.279-0.756,0.195

		c-0.141-0.084-0.561-0.391-0.449-0.699c0.113-0.308,0.113-0.308,0.113-0.308"/>

	<path display="inline" fill="#A2C880" d="M18.388,14.193c-0.043,0.127,0.19-1.66-0.879-2.465c-1.123-0.846-3.6,0.038-3.6,0.038

		s-1.243,1.77-0.811,3.323s2.381,1.625,2.438,1.514"/>

	<path display="inline" fill="#231F20" d="M15.185,11.696c0,0,0.552-0.175,0.701,0.061c0.15,0.235-0.189,0.403-0.298,0.42

		c-0.106,0.017-0.448,0.042-0.522-0.162c-0.074-0.203-0.074-0.203-0.074-0.203"/>

	<path display="inline" fill="#231F20" d="M13.381,13.309c0,0-0.269,0.515-0.062,0.702c0.206,0.188,0.43-0.12,0.465-0.223

		c0.035-0.102,0.115-0.436-0.071-0.543c-0.189-0.107-0.189-0.107-0.189-0.107"/>

	<path display="inline" fill="#A2C880" d="M26.507,14.193c0.043,0.127-0.19-1.66,0.879-2.465c1.123-0.846,3.6,0.038,3.6,0.038

		s1.243,1.77,0.811,3.323s-2.381,1.625-2.438,1.514"/>

	<path display="inline" fill="#231F20" d="M29.702,11.686c0,0-0.558-0.154-0.698,0.087c-0.142,0.241,0.205,0.396,0.313,0.408

		c0.106,0.013,0.45,0.025,0.517-0.182c0.066-0.206,0.066-0.206,0.066-0.206"/>

	<path display="inline" fill="#231F20" d="M31.514,13.309c0,0,0.269,0.515,0.062,0.702c-0.206,0.188-0.43-0.12-0.465-0.223

		c-0.035-0.102-0.115-0.436,0.071-0.543c0.189-0.107,0.189-0.107,0.189-0.107"/>

	<g display="inline">

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-miterlimit="10" d="M22.625,10.5"/>

		</g>

	</g>

	<g display="inline">

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-miterlimit="10" d="M21.375,10.583"/>

		</g>

	</g>

</g>

<g id="Layer_1">

	<path fill="#A2C880" stroke="#375219" stroke-width="1.5" stroke-miterlimit="10" d="M13.501,32.946c0,0-1.134,1.152-1.606,2.804

		c-0.327,1.144-0.157,2.62,0.192,3.734c0,0,1.07,0.341,1.415-0.638c0,0,0.763-0.213,0.703-0.808c0,0,0.833,0.17,0.918-0.681

		c0.084-0.851,0.374-0.895,0.374-0.895"/>

	<path fill="#A2C880" stroke="#375219" stroke-width="1.5" stroke-miterlimit="10" d="M31.215,32.612c0,0,1.135,1.211,1.607,2.945

		c0.326,1.202,0.156,2.753-0.192,3.924c0,0-1.069,0.358-1.415-0.67c0,0-0.762-0.224-0.703-0.849c0,0-0.832,0.179-0.918-0.715

		c-0.084-0.894-0.374-0.94-0.374-0.94"/>

	<path fill="#A2C880" stroke="#375219" stroke-width="1.5" stroke-miterlimit="10" d="M15.262,16.801c0,0-3.549-0.879-5.784,0.031

		c0,0-2.184,1.795-3.488,4.573c-1.304,2.778-1.672,5.624-1.686,5.799c0,0-0.181,1.326,0.218,1.991c0,0,0.271,0.771,0.811,0.788

		c0,0,0.81,0.329,1.667-1.57c0,0,0.798-0.896,1.115-0.977c0,0,0.759-0.683,1.233-1.528c0,0,0.393-0.864,1.281-1.127

		c0,0,0.692-0.291,1.192-1.336c0,0,1.905-0.482,2.011-0.815c0,0,0.083-2.085,0.522-2.77"/>

	<path fill="#A2C880" stroke="#375219" stroke-width="1.5" stroke-miterlimit="10" d="M29.764,16.801c0,0,3.549-0.879,5.783,0.031

		c0,0,2.184,1.795,3.488,4.573c1.305,2.778,1.672,5.624,1.686,5.799c0,0,0.181,1.326-0.218,1.991c0,0-0.271,0.771-0.811,0.788

		c0,0-0.81,0.329-1.667-1.57c0,0-0.799-0.896-1.115-0.977c0,0-0.76-0.683-1.233-1.528c0,0-0.394-0.864-1.281-1.127

		c0,0-0.692-0.291-1.192-1.336c0,0-1.904-0.482-2.011-0.815c0,0-0.083-2.085-0.522-2.77"/>

	<path fill="#A2C880" stroke="#375219" stroke-width="1.5" stroke-miterlimit="10" d="M20.612,39.122l-0.675-0.185

		c0,0,0.912,3,2.3,2.938s2.251-2.787,2.251-2.787"/>

	<g>

		<path fill="#231F20" d="M11.529,27.122c1.292,8.792,5.583,11.667,9.083,12.5c0,0,2.071,0.563,3.775,0

			c3.416-1.13,7.791-3.708,9.084-12.5"/>

		<path fill="#231F20" d="M11.529,27.122c-1.292-8.792,6.708-12.958,6.708-12.958c-0.643,0.374,3.453-2.621,8.525,0

			c0,0,8,4.166,6.709,12.958"/>

	</g>

	<g>

		<polygon fill="#0C5A85" points="19.688,27.9 17.537,23.66 19.75,19.188 25.5,19 26.938,23.544 24.881,27.9 		"/>

	</g>

	<path fill="#0C5A85" d="M25.607,18.083l1.629-2.458c0,0-2.021-1.792-4.7-1.625h-0.299c-2.68-0.167-4.701,1.625-4.701,1.625

		l1.63,2.458C19.167,18.083,20.84,17.417,25.607,18.083z"/>

	<path fill="#0C5A85" d="M32.583,24.292c0.375-3.792-4.5-8.083-4.5-8.083l-2,2.542l1.979,4.312"/>

	<path fill="#0C5A85" d="M12.104,24.292c-0.375-3.792,4.5-8.083,4.5-8.083l2,2.542l-1.979,4.312"/>

	<path fill="#0C5A85" d="M19.875,29.25h5.069c0,0,0.747,4.892-2.821,4.938C18.556,34.233,19.875,29.25,19.875,29.25z"/>

	<path fill="#0C5A85" d="M27.875,24.459l-1.792,3.692c0,0,0.271,1.474,0,2.474s4.75,0.793,5,0.709s1.75-4.334,1.167-5.959"/>

	<path fill="#0C5A85" d="M16.312,25l1.098,0.884l0.69,1.27l0.733,1.347c0,0-0.524,1.43-0.271,2.366s-4.691,0.087-4.925,0.009

		c-0.234-0.079-1.997-3.919-1.451-5.441"/>

	<path fill="#0C5A85" d="M25.573,32.625c0,0,0.734-0.842,5.359-0.259c0,0-2.942,5.717-7.225,6.384c0,0-0.875-2.042-0.458-3.042"/>

	<path fill="#0C5A85" d="M18.997,32.625c0,0-0.734-0.842-5.359-0.259c0,0,2.942,5.717,7.225,6.384c0,0,0.875-2.042,0.458-3.042"/>

	<path fill="#06334B" d="M16.802,23.372"/>

	<g opacity="0.7">

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-width="0.4" stroke-miterlimit="10" d="M40.125,27.875"/>

		</g>

	</g>

	<g opacity="0.9">

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-width="0.4" stroke-miterlimit="10" d="M22,41"/>

		</g>

	</g>

	<g>

		<g>

			<path fill="none" stroke="#C7E0AC" stroke-width="0.4" stroke-miterlimit="10" d="M13,37.062"/>

		</g>

	</g>

	<g>

	</g>

	<g>

	</g>

	<g>

	</g>

	<g>

	</g>

	<g>

	</g>

</g>

</svg>

/*

 * Copyright (c) 2009, 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.

 */

#ifndef TURTLESIM_TURTLE_H

#define TURTLESIM_TURTLE_H

// This prevents a MOC error with versions of boost >= 1.48

#ifndef Q_MOC_RUN  // See: https://bugreports.qt-project.org/browse/QTBUG-22829

# include <ros/ros.h>

# include <boost/shared_ptr.hpp>

# include <turtlesim/Pose.h>

#include <turtlesim/Velocity.h>

# include <turtlesim/SetPen.h>

# include <turtlesim/TeleportRelative.h>

# include <turtlesim/TeleportAbsolute.h>

# include <turtlesim/Color.h>

#endif

#include <QImage>

#include <QPainter>

#include <QPen>

#include <QPointF>

#define PI 3.14159265

namespace turtlesim

{

class Turtle

{

public:

  Turtle(const ros::NodeHandle& nh, const QImage& turtle_image, const QPointF& pos, float orient);

  bool update(double dt, QPainter& path_painter, const QImage& path_image, qreal canvas_width, qreal canvas_height);

  void paint(QPainter &painter);

private:

  void velocityCallback(const VelocityConstPtr& vel);

  bool setPenCallback(turtlesim::SetPen::Request&, turtlesim::SetPen::Response&);

  bool teleportRelativeCallback(turtlesim::TeleportRelative::Request&, turtlesim::TeleportRelative::Response&);

  bool teleportAbsoluteCallback(turtlesim::TeleportAbsolute::Request&, turtlesim::TeleportAbsolute::Response&);

  void rotateImage();

  ros::NodeHandle nh_;

  QImage turtle_image_;

  QImage turtle_rotated_image_;

  QPointF pos_;

  qreal orient_;

  qreal lin_vel_;

  qreal ang_vel_;

  bool pen_on_;

  QPen pen_;

  ros::Subscriber velocity_sub_;

  ros::Publisher pose_pub_;

  ros::Publisher color_pub_;

  ros::ServiceServer set_pen_srv_;

  ros::ServiceServer teleport_relative_srv_;

  ros::ServiceServer teleport_absolute_srv_;

  ros::WallTime last_command_time_;

  float meter_;

  struct TeleportRequest

  {

    TeleportRequest(float x, float y, qreal _theta, qreal _linear, bool _relative)

    : pos(x, y)

    , theta(_theta)

    , linear(_linear)

    , relative(_relative)

    {}

    QPointF pos;

    qreal theta;

    qreal linear;

    bool relative;

  };

  typedef std::vector<TeleportRequest> V_TeleportRequest;

  V_TeleportRequest teleport_requests_;

};

typedef boost::shared_ptr<Turtle> TurtlePtr;

}

#endif

/*

 * Copyright (c) 2009, 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 <QFrame>

#include <QImage>

#include <QPainter>

#include <QPaintEvent>

#include <QTimer>

#include <QVector>

// This prevents a MOC error with versions of boost >= 1.48

#ifndef Q_MOC_RUN  // See: https://bugreports.qt-project.org/browse/QTBUG-22829

# include <ros/ros.h>

# include <std_srvs/Empty.h>

# include <turtlesim/Spawn.h>

# include <turtlesim/Kill.h>

# include <map>

# include "turtle.h"

#endif

namespace turtlesim

{

class TurtleFrame : public QFrame

{

  Q_OBJECT

public:

  TurtleFrame(QWidget* parent = 0, Qt::WindowFlags f = 0);

  ~TurtleFrame();

  std::string spawnTurtle(const std::string& name, float x, float y, float angle);

protected:

  void paintEvent(QPaintEvent* event);

private slots:

  void onUpdate();

private:

  void updateTurtles();

  void clear();

  bool hasTurtle(const std::string& name);

  bool clearCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);

  bool resetCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);

  bool spawnCallback(turtlesim::Spawn::Request&, turtlesim::Spawn::Response&);

  bool killCallback(turtlesim::Kill::Request&, turtlesim::Kill::Response&);

  ros::NodeHandle nh_;

  QTimer* update_timer_;

  QImage path_image_;

  QPainter path_painter_;

  uint64_t frame_count_;

  ros::WallTime last_turtle_update_;

  ros::ServiceServer clear_srv_;

  ros::ServiceServer reset_srv_;

  ros::ServiceServer spawn_srv_;

  ros::ServiceServer kill_srv_;

  typedef std::map<std::string, TurtlePtr> M_Turtle;

  M_Turtle turtles_;

  uint32_t id_counter_;

  QVector<QImage> turtle_images_;

  float meter_;

  float width_in_meters_;

  float height_in_meters_;

};

}

<launch>

	<group ns="turtlesim1">

	       <node pkg="turtlesim" name="sim" type="turtlesim_node"/>

	</group>

	<group ns="turtlesim2">

	       <node pkg="turtlesim" name="sim" type="turtlesim_node"/>

	</group>

</launch>

uint8 r

uint8 g

uint8 b

float32 x

float32 y

float32 theta

float32 linear_velocity

float32 angular_velocity

float32 linear

float32 angular

<?xml version="1.0"?>

<package>

  <name>turtlesim_ref</name>

  <version>0.1.1</version>

  <description>

    buggysim is derived from turtlesim, and is used for slightly-realistic

        buggy simulation for Robobuggy. 

  </description>

  <maintainer email="msebek@andrew.cmu.edu">Matthew Sebek</maintainer>

  <license>BSD</license>

  <!-- Lol just gonna leave these here for now... -->

  <url type="website">http://www.ros.org/wiki/turtlesim</url>

  <url type="bugtracker">https://github.com/ros/ros_tutorials/issues</url>

  <url type="repository">https://github.com/ros/ros_tutorials</url>

  <author>Josh Faust</author>

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>message_generation</build_depend>

  <build_depend>libqt4-dev</build_depend>

  <build_depend>qt4-qmake</build_depend>

  <build_depend>rosconsole</build_depend>

  <build_depend>roscpp</build_depend>

  <build_depend>roscpp_serialization</build_depend>

  <build_depend>roslib</build_depend>

  <build_depend>rostime</build_depend>

  <build_depend>std_msgs</build_depend>

  <build_depend>std_srvs</build_depend>

  <run_depend>libqt4</run_depend>

  <run_depend>message_runtime</run_depend>

  <run_depend>rosconsole</run_depend>

  <run_depend>roscpp</run_depend>

  <run_depend>roscpp_serialization</run_depend>

  <run_depend>roslib</run_depend>

  <run_depend>rostime</run_depend>

  <run_depend>std_msgs</run_depend>

  <run_depend>std_srvs</run_depend>

</package>

/*

 * Copyright (c) 2009, 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 "turtlesim/turtle.h"

#include <QColor>

#include <QRgb>

#define DEFAULT_PEN_R 0xb3

#define DEFAULT_PEN_G 0xb8

#define DEFAULT_PEN_B 0xff

namespace turtlesim

{

Turtle::Turtle(const ros::NodeHandle& nh, const QImage& turtle_image, const QPointF& pos, float orient)

: nh_(nh)

, turtle_image_(turtle_image)

, pos_(pos)

, orient_(orient)

, lin_vel_(0.0)

, ang_vel_(0.0)

, pen_on_(true)

, pen_(QColor(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))

{

  pen_.setWidth(3);

  velocity_sub_ = nh_.subscribe("command_velocity", 1, &Turtle::velocityCallback, this);

  pose_pub_ = nh_.advertise<Pose>("pose", 1);

  color_pub_ = nh_.advertise<Color>("color_sensor", 1);

  set_pen_srv_ = nh_.advertiseService("set_pen", &Turtle::setPenCallback, this);

  teleport_relative_srv_ = nh_.advertiseService("teleport_relative", &Turtle::teleportRelativeCallback, this);

  teleport_absolute_srv_ = nh_.advertiseService("teleport_absolute", &Turtle::teleportAbsoluteCallback, this);

  meter_ = turtle_image_.height();

  rotateImage();

}

void Turtle::velocityCallback(const VelocityConstPtr& vel)

{

  last_command_time_ = ros::WallTime::now();

  lin_vel_ = vel->linear;

  ang_vel_ = vel->angular;

}

bool Turtle::setPenCallback(turtlesim::SetPen::Request& req, turtlesim::SetPen::Response&)

{

  pen_on_ = !req.off;

  if (req.off)

  {

    return true;

  }

  QPen pen(QColor(req.r, req.g, req.b));

  if (req.width != 0)

  {

    pen.setWidth(req.width);

  }

  pen_ = pen;

  return true;

}

bool Turtle::teleportRelativeCallback(turtlesim::TeleportRelative::Request& req, turtlesim::TeleportRelative::Response&)

{

  teleport_requests_.push_back(TeleportRequest(0, 0, req.angular, req.linear, true));

  return true;

}

bool Turtle::teleportAbsoluteCallback(turtlesim::TeleportAbsolute::Request& req, turtlesim::TeleportAbsolute::Response&)

{

  teleport_requests_.push_back(TeleportRequest(req.x, req.y, req.theta, 0, false));

  return true;

}

void Turtle::rotateImage()

{

  QTransform transform;

  transform.rotate(-orient_ * 180.0 / PI + 90.0);

  turtle_rotated_image_ = turtle_image_.transformed(transform);

}

bool Turtle::update(double dt, QPainter& path_painter, const QImage& path_image, qreal canvas_width, qreal canvas_height)

{

  bool modified = false;

  qreal old_orient = orient_;

  // first process any teleportation requests, in order

  V_TeleportRequest::iterator it = teleport_requests_.begin();

  V_TeleportRequest::iterator end = teleport_requests_.end();

  for (; it != end; ++it)

  {

    const TeleportRequest& req = *it;

    QPointF old_pos = pos_;

    if (req.relative)

    {

      orient_ += req.theta;

      pos_.rx() += std::sin(orient_ + PI/2.0) * req.linear;

      pos_.ry() += std::cos(orient_ + PI/2.0) * req.linear;

    }

    else

    {

      pos_.setX(req.pos.x());

      pos_.setY(std::max(0.0, static_cast<double>(canvas_height - req.pos.y())));

      orient_ = req.theta;

    }

    path_painter.setPen(pen_);

    path_painter.drawLine(pos_ * meter_, old_pos * meter_);

    modified = true;

  }

  teleport_requests_.clear();

  // Stop moving the turtle if there hasn't been a command sent recently.

  if (ros::WallTime::now() - last_command_time_ > ros::WallDuration(1.0))

  {

    lin_vel_ = 0.0;

    ang_vel_ = 0.0;

  }

  QPointF old_pos = pos_;

  orient_ = std::fmod(orient_ + ang_vel_ * dt, 2*PI);

  pos_.rx() += std::sin(orient_ + PI/2.0) * lin_vel_ * dt;

  pos_.ry() += std::cos(orient_ + PI/2.0) * lin_vel_ * dt;

  // Clamp to screen size

  if (pos_.x() < 0 || pos_.x() > canvas_width ||

      pos_.y() < 0 || pos_.y() > canvas_height)

  {

    ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos_.x(), pos_.y());

  }

  pos_.setX(std::min(std::max(static_cast<double>(pos_.x()), 0.0), static_cast<double>(canvas_width)));

  pos_.setY(std::min(std::max(static_cast<double>(pos_.y()), 0.0), static_cast<double>(canvas_height)));

  // Publish pose of the turtle

  Pose p;

  p.x = pos_.x();

  p.y = canvas_height - pos_.y();

  p.theta = orient_;

  p.linear_velocity = lin_vel_;

  p.angular_velocity = ang_vel_;

  pose_pub_.publish(p);

  // Figure out (and publish) the color underneath the turtle

  {

    Color color;

    QRgb pixel = path_image.pixel((pos_ * meter_).toPoint());

    color.r = qRed(pixel);

    color.g = qGreen(pixel);

    color.b = qBlue(pixel);

    color_pub_.publish(color);

  }

  ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f", nh_.getNamespace().c_str(), pos_.x(), pos_.y(), orient_);

  if (orient_ != old_orient)

  {

    rotateImage();

    modified = true;

  }

  if (pos_ != old_pos)

  {

    if (pen_on_)

    {

      path_painter.setPen(pen_);

      path_painter.drawLine(pos_ * meter_, old_pos * meter_);

    }

    modified = true;

  }

  return modified;

}

  // Do the actual painting operations.

void Turtle::paint(QPainter& painter)

{

  QPointF p = pos_ * meter_;

  p.rx() -= 0.5 * turtle_rotated_image_.width();

  p.ry() -= 0.5 * turtle_rotated_image_.height();

  painter.drawImage(p, turtle_rotated_image_);

}

}

/*

 * Copyright (c) 2009, 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 "turtlesim/turtle_frame.h"

#include <QPointF>

#include <ros/package.h>

#include <cstdlib>

#include <ctime>

#define DEFAULT_BG_R 0x45

#define DEFAULT_BG_G 0x56

#define DEFAULT_BG_B 0xff

namespace turtlesim

{

TurtleFrame::TurtleFrame(QWidget* parent, Qt::WindowFlags f)

: QFrame(parent, f)

, path_image_(500, 500, QImage::Format_ARGB32)

, path_painter_(&path_image_)

, frame_count_(0)

, id_counter_(0)

{

  setFixedSize(500, 500);

  setWindowTitle("TurtleSim");

  srand(time(NULL));

  update_timer_ = new QTimer(this);

  update_timer_->setInterval(16);

  update_timer_->start();

  connect(update_timer_, SIGNAL(timeout()), this, SLOT(onUpdate()));

  nh_.setParam("background_r", DEFAULT_BG_R);

  nh_.setParam("background_g", DEFAULT_BG_G);

  nh_.setParam("background_b", DEFAULT_BG_B);

  QVector<QString> turtles;

  turtles.append("box-turtle.png");

  turtles.append("robot-turtle.png");

  turtles.append("sea-turtle.png");

  turtles.append("diamondback.png");

  turtles.append("electric.png");

  turtles.append("fuerte.png");

  turtles.append("groovy.png");

  turtles.append("hydro.svg");

  QString images_path = (ros::package::getPath("turtlesim") + "/images/").c_str();

  for (size_t i = 0; i < turtles.size(); ++i)

  {

    QImage img;

    img.load(images_path + turtles[i]);

    turtle_images_.append(img);

  }

  meter_ = turtle_images_[0].height();

  clear();

  clear_srv_ = nh_.advertiseService("clear", &TurtleFrame::clearCallback, this);

  reset_srv_ = nh_.advertiseService("reset", &TurtleFrame::resetCallback, this);

  spawn_srv_ = nh_.advertiseService("spawn", &TurtleFrame::spawnCallback, this);

  kill_srv_ = nh_.advertiseService("kill", &TurtleFrame::killCallback, this);

  ROS_INFO("Starting turtlesim with node name %s", ros::this_node::getName().c_str()) ;

  width_in_meters_ = (width() - 1) / meter_;

  height_in_meters_ = (height() - 1) / meter_;

  spawnTurtle("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0);

}

TurtleFrame::~TurtleFrame()

{

  delete update_timer_;

}

bool TurtleFrame::spawnCallback(turtlesim::Spawn::Request& req, turtlesim::Spawn::Response& res)

{

  std::string name = spawnTurtle(req.name, req.x, req.y, req.theta);

  if (name.empty())

  {

    ROS_ERROR("A turtled named [%s] already exists", req.name.c_str());

    return false;

  }

  res.name = name;

  return true;

}

bool TurtleFrame::killCallback(turtlesim::Kill::Request& req, turtlesim::Kill::Response&)

{

  M_Turtle::iterator it = turtles_.find(req.name);

  if (it == turtles_.end())

  {

    ROS_ERROR("Tried to kill turtle [%s], which does not exist", req.name.c_str());

    return false;

  }

  turtles_.erase(it);

  update();

  return true;

}

bool TurtleFrame::hasTurtle(const std::string& name)

{

  return turtles_.find(name) != turtles_.end();

}

std::string TurtleFrame::spawnTurtle(const std::string& name, float x, float y, float angle)

{

  std::string real_name = name;

  if (real_name.empty())

  {

    do

    {

      std::stringstream ss;

      ss << "turtle" << ++id_counter_;

      real_name = ss.str();

    } while (hasTurtle(real_name));

  }

  else

  {

    if (hasTurtle(real_name))

    {

      return "";

    }

  }

  TurtlePtr t(new Turtle(ros::NodeHandle(real_name), turtle_images_[rand() % turtle_images_.size()], QPointF(x, y), angle));

  turtles_[real_name] = t;

  update();

  ROS_INFO("Spawning turtle [%s] at x=[%f], y=[%f], theta=[%f]", real_name.c_str(), x, y, angle);

  return real_name;

}

void TurtleFrame::clear()

{

  int r = DEFAULT_BG_R;

  int g = DEFAULT_BG_G;

  int b = DEFAULT_BG_B;

  nh_.param("background_r", r, r);

  nh_.param("background_g", g, g);

  nh_.param("background_b", b, b);

  path_image_.fill(qRgb(r, g, b));

  update();

}

void TurtleFrame::onUpdate()

{

  ros::spinOnce();

  updateTurtles();

  if (!ros::ok())

  {

    close();

  }

}

void TurtleFrame::paintEvent(QPaintEvent* event)

{

  QPainter painter(this);

  painter.drawImage(QPoint(0, 0), path_image_);

  M_Turtle::iterator it = turtles_.begin();

  M_Turtle::iterator end = turtles_.end();

  for (; it != end; ++it)

  {

    it->second->paint(painter);

  }

}

void TurtleFrame::updateTurtles()

{

  if (last_turtle_update_.isZero())

  {

    last_turtle_update_ = ros::WallTime::now();

    return;

  }

  bool modified = false;

  M_Turtle::iterator it = turtles_.begin();

  M_Turtle::iterator end = turtles_.end();

  for (; it != end; ++it)

  {

    modified |= it->second->update(0.001 * update_timer_->interval(), path_painter_, path_image_, width_in_meters_, height_in_meters_);

  }

  if (modified)

  {

    update();

  }

  ++frame_count_;

}

bool TurtleFrame::clearCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)

{

  ROS_INFO("Clearing turtlesim.");

  clear();

  return true;

}

bool TurtleFrame::resetCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)

{

  ROS_INFO("Resetting turtlesim.");

  turtles_.clear();

  id_counter_ = 0;

  spawnTurtle("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0);

  clear();

  return true;

}

}

/*

 * Copyright (c) 2009, 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 <QApplication>

#include <ros/ros.h>

#include "turtlesim/turtle_frame.h"

class TurtleApp : public QApplication

{

public:

  ros::NodeHandlePtr nh_;

  TurtleApp(int& argc, char** argv)

    : QApplication(argc, argv)

  {

    ros::init(argc, argv, "turtlesim", ros::init_options::NoSigintHandler);

    nh_.reset(new ros::NodeHandle);

  }

  int exec()

  {

    turtlesim::TurtleFrame frame;

    frame.show();

    return QApplication::exec();

  }

};

int main(int argc, char** argv)

{

  TurtleApp app(argc, argv);

  return app.exec();

}

string name

---

uint8 r

uint8 g

uint8 b

uint8 width

uint8 off

---

float32 x

float32 y

float32 theta

string name # Optional.  A unique name will be created and returned if this is empty

---

string name

float32 x

float32 y

float32 theta

---

float32 linear

float32 angular

---

#include <boost/bind.hpp>

#include <ros/ros.h>

#include <turtlesim/Pose.h>

#include <turtlesim/Velocity.h>

#include <std_srvs/Empty.h>

turtlesim::PoseConstPtr g_pose;

turtlesim::Pose g_goal;

enum State

{

  FORWARD,

  STOP_FORWARD,

  TURN,

  STOP_TURN,

};

State g_state = FORWARD;

State g_last_state = FORWARD;

bool g_first_goal_set = false;

#define PI 3.141592

void poseCallback(const turtlesim::PoseConstPtr& pose)

{

  g_pose = pose;

}

bool hasReachedGoal()

{

  return fabsf(g_pose->x - g_goal.x) < 0.1 && fabsf(g_pose->y - g_goal.y) < 0.1 && fabsf(g_pose->theta - g_goal.theta) < 0.01;

}

bool hasStopped()

{

  return g_pose->angular_velocity < 0.0001 && g_pose->linear_velocity < 0.0001;

}

void printGoal()

{

  ROS_INFO("New goal [%f %f, %f]", g_goal.x, g_goal.y, g_goal.theta);

}

void commandTurtle(ros::Publisher vel_pub, float linear, float angular)

{

  turtlesim::Velocity vel;

  vel.linear = linear;

  vel.angular = angular;

  vel_pub.publish(vel);

}

void stopForward(ros::Publisher vel_pub)

{

  if (hasStopped())

  {

    ROS_INFO("Reached goal");

    g_state = TURN;

    g_goal.x = g_pose->x;

    g_goal.y = g_pose->y;

    g_goal.theta = fmod(g_pose->theta + PI/2.0, 2*PI);

    printGoal();

  }

  else

  {

    commandTurtle(vel_pub, 0, 0);

  }

}