Scope target turtlesim

io_turtlesim/io_turtlesim_webserver/Dockerfile

@@ -9,7 +9,7 @@ RUN chmod -R g+w /var/log/nginx /var/cache/nginx /var/run
 COPY env.sh /usr/bin/
 COPY src/ /usr/share/nginx/html/
 
-ENV WS="ws://localhost:9090"
+ENV WS_ROSBRIDGE="ws://localhost:9090"
 
 EXPOSE 8080
 ENTRYPOINT /bin/bash -c "bash /usr/bin/env.sh && nginx -g 'daemon off;'"

io_turtlesim/io_turtlesim_webserver/env.sh

@@ -2,8 +2,8 @@
 
 WS_URL='wss://localhost:9090'
 
-if [ ! -z "$WS" ]; then
-    WS_URL='wss://'${WS##*/}
+if [ ! -z "$WS_ROSBRIDGE" ]; then
+    WS_URL='wss://'${WS_ROSBRIDGE##*/}
 fi
 
 # write env object to env.js

io_turtlesim_scoped_targetted/LICENSE

@@ -0,0 +1,22 @@
+Copyright (c) 2017-2018, Rapyuta Robotics Co., Ltd.
+All Rights Reserved.
+
+All information contained herein is, and remains the property of Rapyuta
+Robotics Co., Ltd. and its suppliers, if any. The intellectual and technical
+concepts contained herein are proprietary to Rapyuta Robotics Co., Ltd. and its
+suppliers and may be covered by patents, patents in process, and are protected
+by trade secret or copyright law.
+Dissemination of this information or reproduction of this material is strictly
+forbidden unless prior written permission is obtained from Rapyuta
+Robotics Co., Ltd.
+
+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 Rapyuta Robotics Co., Ltd. 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.

io_turtlesim_scoped_targetted/README.md

@@ -0,0 +1,24 @@
+# IO Turtle Simulation
+
+### Setup (optional)
+Set the following environment variables if you want to override the default settings of port 9090 and address 0.0.0.0:
+```
+export WS_PORT=9090
+export WS_ADDR="0.0.0.0"
+```
+
+### Building the packages
+Clone or symlink this directory into the /src directory of your catkin workspace, then build and source.
+Assuming your current directory is the root of your catkin workspace:
+```
+catkin build io_turtle_sim_env
+source devel/setup.bash
+```
+
+### Launching simulation locally
+After successfully building and sourcing the project, launch it:
+```
+roslaunch io_turtle_sim_env demo_sim.launch
+```
+
+The UI can be found in the [_io\_turtlesim\_webserver_](io_turtlesim_webserver/src/index.html) package. 

io_turtlesim_scoped_targetted/io_turtle/CMakeLists.txt

@@ -0,0 +1,71 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(io_turtle_scoped_targetted)
+
+##################
+## Dependencies ##
+##################
+
+add_compile_options(-std=c++11)
+
+set(PKG_DEPS
+    roscpp
+    geometry_msgs
+    dynamic_reconfigure
+    io_turtle_scoped_targetted_action
+    io_turtle_scoped_targetted_msgs
+    io_turtle_scoped_targetted_services
+)
+
+find_package(catkin REQUIRED COMPONENTS ${PKG_DEPS})
+
+generate_dynamic_reconfigure_options(
+    cfg/Control.cfg
+)
+
+catkin_package(
+    CATKIN_DEPENDS ${PKG_DEPS}
+    INCLUDE_DIRS include
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(${PROJECT_NAME}_node
+    src/io_turtle.cpp
+    src/io_turtle_node.cpp  
+)
+
+add_dependencies(${PROJECT_NAME}_node
+    ${catkin_EXPORTED_TARGETS}
+    ${PROJECT_NAME}_gencfg
+)
+
+target_link_libraries(${PROJECT_NAME}_node
+    ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+install(TARGETS ${PROJECT_NAME}_node
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(DIRECTORY cfg
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY launch
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

io_turtlesim_scoped_targetted/io_turtle/cfg/Control.cfg

@@ -0,0 +1,18 @@
+#! /usr/bin/env python
+
+# Control parameters for turtle simulation
+PACKAGE='io_turtle'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#       Name         Type        Level   Description                Default Min  Max
+gen.add("k_rep",     double_t,   0,      "Repulsive Gain",          1.0,    0,   10)
+gen.add("k_att",     double_t,   0,      "Attractive Gain",         2,      0,   10)
+gen.add("k_theta",   double_t,   0,      "Angle Gain - P",          5.0,    0,   10)
+gen.add("f_att_max", double_t,   0,      "Max Attractive Force",    4,      0,   10)
+gen.add("v_max",     double_t,   0,      "Max Velocity",            5,      0,   10)
+gen.add("omega_max", double_t,   0,      "Max Angular velocity",    2,      0,   10)
+
+exit(gen.generate(PACKAGE, "io_turtle", "Control"))

io_turtlesim_scoped_targetted/io_turtle/include/io_turtle_scoped_targetted/io_turtle.hpp

@@ -0,0 +1,55 @@
+#ifndef IO_TURTLE_IO_TURTLE_HPP
+#define IO_TURTLE_IO_TURTLE_HPP
+
+#include <ros/ros.h>
+#include <dynamic_reconfigure/server.h>
+
+#include <io_turtle_scoped_targetted/turtle.hpp>
+#include <io_turtle_scoped_targetted/ControlConfig.h>
+
+namespace turtlesim {
+
+class IOTurtle {
+public:
+    IOTurtle(ros::NodeHandle& priv_nh);
+
+    void dynamic_parameters_callback(io_turtle::ControlConfig& config, uint32_t level);
+    void register_turtle(ros::NodeHandle& nh, bool sim, int queue_size);
+    void set_goal(float x, float y);
+    void move_towards_goal();
+    void stop_turtle();
+    void reset();
+
+    bool is_moving() const;
+    bool stop_requested() const;
+    bool reached_goal() const;
+    bool check_collision() const;
+
+private:
+    void publish_velocity();
+    void velocity_callback(const geometry_msgs::Twist& vel);
+    void sim_pose_callback(const geometry_msgs::Pose2D& pose);
+    void sim_sensors_callback(const io_turtle_scoped_targetted_msgs::DistanceSensor& sensor);
+
+    ros::Subscriber _velocity_sub;
+    ros::Subscriber _sim_pose_sub;
+    ros::Subscriber _sim_sensors_sub;
+    ros::Publisher _sim_velocity_pub;
+    ros::Publisher _pose_pub;
+
+    Turtle _turtle;
+    bool _sim;
+    bool _reached_goal;
+    bool _stop_requested;
+    float _goal_x;
+    float _goal_y;
+    float _d_0;
+    float _collision_range;
+    float _goal_tolerance;
+
+    io_turtle::ControlConfig _control_config;
+};
+
+}  // namespace turtlesim
+
+#endif /* IO_TURTLE_IO_TURTLE_HPP */

io_turtlesim_scoped_targetted/io_turtle/include/io_turtle_scoped_targetted/turtle.hpp

@@ -0,0 +1,26 @@
+#ifndef IO_TURTLE_TURTLE_HPP
+#define IO_TURTLE_TURTLE_HPP
+
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/Twist.h>
+#include <io_turtle_scoped_targetted_msgs/DistanceSensor.h>
+
+namespace turtlesim {
+
+struct Turtle {
+    Turtle() {}
+
+    void set_cmd_vel(float linear, float angular) {
+        _cmd_vel.linear.x = linear;
+        _cmd_vel.angular.z = angular;
+    }
+
+    geometry_msgs::Pose2D _pose;
+    geometry_msgs::Twist _current_vel;
+    geometry_msgs::Twist _cmd_vel;
+    io_turtle_scoped_targetted_msgs::DistanceSensor _sensors;
+};
+
+}  // namespace turtlesim
+
+#endif /* IO_TURTLE_TURTLE_HPP */

io_turtlesim_scoped_targetted/io_turtle/launch/turtle.launch

@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+    <arg name="use_rapyuta_io" default="false" doc="use rapyuta.io to get robot name"/>
+    <rosparam param="use_rapyuta_io" subst_value="True">$(arg use_rapyuta_io)</rosparam>
+
+    <arg name="robot_name" default="turtle0" doc="not used if use_rapyuta_io=true" />
+    <node name="$(anon turtle)" pkg="io_turtle_scoped_targetted" type="io_turtle_scoped_targetted_node" output="screen">
+        <rosparam file="$(find io_turtle_scoped_targetted_config)/config/config.yaml" />
+        <param name="name" value="$(arg robot_name)" />
+        <!-- remap topics. these namespace separation are handled by rapyuta.io -->
+        <remap from="sim/pose" to="$(arg robot_name)/sim/pose" unless="$(arg use_rapyuta_io)" />
+        <remap from="sim/cmd_vel" to="$(arg robot_name)/sim/cmd_vel" unless="$(arg use_rapyuta_io)" />
+        <remap from="sim/sensors" to="$(arg robot_name)/sim/sensors" unless="$(arg use_rapyuta_io)" />
+        <remap from="pose" to="$(arg robot_name)/pose" unless="$(arg use_rapyuta_io)" />
+        <remap from="cmd_vel" to="$(arg robot_name)/cmd_vel" unless="$(arg use_rapyuta_io)" />
+        <!-- todo action and service -->
+        <remap from="goto_action" to="$(arg robot_name)/goto_action" unless="$(arg use_rapyuta_io)" />
+
+    </node>
+</launch>

io_turtlesim_scoped_targetted/io_turtle/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>io_turtle_scoped_targetted</name>
+  <version>1.0.0</version>
+  <description>
+    IO Turtle Node
+  </description>
+  <maintainer email="[email protected]">Gautham Manoharan</maintainer>
+  <maintainer email="[email protected]">Yu Okamoto</maintainer>
+  <license>Commercial</license>
+
+  <author>Gautham Manoharan</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>roscpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>dynamic_reconfigure</depend>
+
+  <depend>io_turtle_scoped_targetted_action</depend>
+  <depend>io_turtle_scoped_targetted_msgs</depend>
+  <depend>io_turtle_scoped_targetted_services</depend>
+
+  <exec_depend>io_turtle_scoped_targetted_config</exec_depend>
+</package>