diff --git a/DockerFiles/ROS1.0/ROS1.0.df b/DockerFiles/ROS1.0/ROS1.0.df new file mode 100644 index 0000000..75d310f --- /dev/null +++ b/DockerFiles/ROS1.0/ROS1.0.df @@ -0,0 +1,132 @@ +FROM ros:melodic-robot-bionic as base + +MAINTAINER Sunny + +# This Dockerfile is modified from https://github.com/fcwu/docker-ubuntu-vnc-desktop + +ENV ROS_DISTRO melodic + +# Install essential packages +RUN apt update && \ + DEBIAN_FRONTEND=noninteractive \ + apt install --no-install-recommends -y \ + vim \ + nano \ + tmux \ + curl \ + apache2-utils \ + locales \ + build-essential \ + wget \ + net-tools \ + unzip \ + git \ + sudo \ + xz-utils \ + gpg-agent \ + supervisor \ + software-properties-common && \ + rm -rf /var/lib/apt/lists/* + +RUN echo "en_US.UTF-8 UTF-8" > /etc/locale.gen +RUN locale-gen + +# Install x11vnc and relatives +RUN apt update && \ + DEBIAN_FRONTEND=noninteractive \ + apt install --no-install-recommends -y \ + zenity \ + dbus-x11 \ + x11-utils \ + alsa-utils \ + mesa-utils \ + libgl1-mesa-dri \ + xvfb \ + x11vnc && \ + rm -rf /var/lib/apt/lists/* + +# Install the GUI +RUN apt update && \ + DEBIAN_FRONTEND=noninteractive \ + apt install --no-install-recommends -y \ + lxde \ + gtk2-engines-murrine \ + gnome-themes-standard \ + gtk2-engines-pixbuf \ + gtk2-engines-murrine \ + arc-theme && \ + rm -rf /var/lib/apt/lists/* + +# Kill the bell! +RUN echo "set bell-style none" >> /etc/inputrc + +# Install some applications +RUN apt update -y && \ + DEBIAN_FRONTEND=noninteractive \ + apt install --no-install-recommends -y \ + ttf-ubuntu-font-family \ + firefox && \ + rm -rf /var/lib/apt/lists/* + +# sublime +RUN apt update -y && \ + wget -qO - https://download.sublimetext.com/sublimehq-pub.gpg | apt-key add - && \ + apt-add-repository "deb https://download.sublimetext.com/ apt/stable/" && \ + DEBIAN_FRONTEND=noninteractive \ + sudo apt install -y sublime-text && \ + rm -rf /var/lib/apt/lists/* + +# Install tini as init +ARG TINI_VERSION=v0.18.0 +ADD https://github.com/krallin/tini/releases/download/${TINI_VERSION}/tini /bin/tini +RUN chmod +x /bin/tini + +FROM base as robot + +# Install the ros packages +RUN apt-get update -y && \ + DEBIAN_FRONTEND=noninteractive \ + apt-get install -y \ + ros-${ROS_DISTRO}-tf2-geometry-msgs \ + ros-${ROS_DISTRO}-ackermann-msgs \ + ros-${ROS_DISTRO}-teleop-twist-keyboard \ + ros-${ROS_DISTRO}-map-server \ + ros-${ROS_DISTRO}-navigation \ + ros-${ROS_DISTRO}-robot-localization \ + ros-${ROS_DISTRO}-cv-bridge \ + ros-${ROS_DISTRO}-image-transport \ + ros-${ROS_DISTRO}-tf \ + ros-${ROS_DISTRO}-diagnostic-updater \ + ros-${ROS_DISTRO}-desktop-full \ + ros-${ROS_DISTRO}-ddynamic-reconfigure && \ + rm -rf /var/lib/apt/lists/* + +# Setup chickenbot_ws +RUN mkdir /chickenbot_ws && \ + cd /chickenbot_ws + +# Setup catkin_ws +COPY catkin_ws /catkin_ws/ +RUN /bin/bash -c 'source /opt/ros/$ROS_DISTRO/setup.bash; cd /catkin_ws; catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release' + +# locale and user +RUN locale-gen en_US.UTF-8 && \ + ln -fs /usr/share/zoneinfo/Asia/Taipei /etc/localtime && \ + dpkg-reconfigure --frontend noninteractive tzdata && \ + useradd -ms /bin/bash ubuntu && \ + adduser ubuntu sudo && \ + chown -R ubuntu:ubuntu /home/ubuntu/ + +COPY ./etc/ /etc/ +COPY ./bin/xvfb.sh /usr/local/bin/xvfb.sh +COPY ./bin/.gtkrc-2.0 /home/ubuntu/ + +RUN chmod +x /usr/local/bin/xvfb.sh + +ENV HOME /home/ubuntu +WORKDIR "/home/ubuntu" + +CMD ["bash", "/home/ubuntu/.startup.sh"] + +# Expose the VNC port +EXPOSE 5900 diff --git a/DockerFiles/ROS1.0/bin/.gtkrc-2.0 b/DockerFiles/ROS1.0/bin/.gtkrc-2.0 new file mode 100644 index 0000000..e41091c --- /dev/null +++ b/DockerFiles/ROS1.0/bin/.gtkrc-2.0 @@ -0,0 +1,20 @@ +# DO NOT EDIT! This file will be overwritten by LXAppearance. +# Any customization should be done in ~/.gtkrc-2.0.mine instead. +include "/usr/share/themes/Arc/gtk-2.0/gtkrc" + +gtk-theme-name="Arc" +gtk-icon-theme-name="nuoveXT2" +gtk-font-name="Sans 10" +gtk-cursor-theme-name="DMZ-White" +gtk-cursor-theme-size=18 +gtk-toolbar-style=GTK_TOOLBAR_BOTH_HORIZ +gtk-toolbar-icon-size=GTK_ICON_SIZE_LARGE_TOOLBAR +gtk-button-images=1 +gtk-menu-images=1 +gtk-enable-event-sounds=1 +gtk-enable-input-feedback-sounds=1 +gtk-xft-antialias=1 +gtk-xft-hinting=1 +gtk-xft-hintstyle="hintslight" +gtk-xft-rgba="rgb" +include "/root/.gtkrc-2.0.mine" diff --git a/DockerFiles/ROS1.0/bin/xvfb.sh b/DockerFiles/ROS1.0/bin/xvfb.sh new file mode 100644 index 0000000..e4b7106 --- /dev/null +++ b/DockerFiles/ROS1.0/bin/xvfb.sh @@ -0,0 +1,3 @@ +#!/bin/sh + +exec /usr/bin/Xvfb :1 -screen 0 1600x900x24 diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/CMakeLists.txt b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/CMakeLists.txt new file mode 100644 index 0000000..744de97 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(chickenbot) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/chickenbot.md b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/chickenbot.md new file mode 100644 index 0000000..e69de29 diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/config/costmap_common_params.yaml b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/config/costmap_common_params.yaml new file mode 100644 index 0000000..3018909 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/config/costmap_common_params.yaml @@ -0,0 +1,21 @@ +obstacle_range: 4.0 +raytrace_range: 4.0 +footprint: [[-0.25, -0.15], [-0.25, 0.15], [0.25, 0.15], [0.25, -0.15]] +#robot_radius: 0.25 +inflation_radius: 0.3 + +inf_is_valid: true + +observation_sources: laser_scan_sensor + +laser_scan_sensor: {sensor_frame: laser_link, data_type: LaserScan, topic: scan, marking: true, clearing: true} + + +plugins: + - {name: static_layer, type: "costmap_2d::StaticLayer"} + - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} + - {name: inflation_layer, type: "costmap_2d::InflationLayer"} + +obstacle_layer: + observation_sources: base_scan + base_scan: {data_type: LaserScan, sensor_frame: /laser_link, clearing: true, marking: true, topic: /scan} diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/config/dwa_planner_params.yaml b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/config/dwa_planner_params.yaml new file mode 100644 index 0000000..9bd3fd0 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/config/dwa_planner_params.yaml @@ -0,0 +1,36 @@ +DWAPlannerROS: + xy_goal_tolerance: 0.12 + yaw_goal_tolerance: 0.1 + + path_distance_bias: 100.0 + goal_distance_bias: 24.0 + occdist_scale: 0.01 + forward_point_distance: 0.2 + publish_cost_grid_pc: true + publish_traj_pc: true + + max_trans_vel: 2.0 + min_trans_vel: 0.01 + max_vel_x: 2.0 + min_vel_x: -2.0 + max_vel_y: 0.0 + min_vel_y: 0.0 + max_rot_vel: 2.0 + min_rot_vel: 0.01 + acc_lim_x: 2.0 + acc_lim_y: 0.0 + acc_lim_theta: 2.5 + acc_limit_trans: 5.0 + sim_time: 1.5 + sim_granularity: 0.05 + global_frame_id: "map" + vx_samples: 10 + vy_samples: 1 + vth_samples: 10 + oscillation_reset_angle: 0.01 + valid_trajectory_ratio: 0.0 + max_opposite_angle: 2.35619445 + + cheat_factor: 25.0 +latch_xy_goal_tolerance: true + diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/config/global_costmap_params.yaml b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/config/global_costmap_params.yaml new file mode 100644 index 0000000..ef4fdc5 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/config/global_costmap_params.yaml @@ -0,0 +1,7 @@ +global_costmap: + global_frame: map + robot_base_frame: base_link + update_frequency: 1 + static_map: true + resolution: 0.05 + transform_tolerance: 2.0 diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/config/local_costmap_params.yaml b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/config/local_costmap_params.yaml new file mode 100644 index 0000000..45862f7 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/config/local_costmap_params.yaml @@ -0,0 +1,13 @@ +local_costmap: + global_frame: map + robot_base_frame: base_link + update_frequency: 15 + publish_frequency: 15 + static_map: false + rolling_window: true + + width: 6.0 + height: 6.0 + resolution: 0.01 + + transform_tolerance: 2.0 diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/chicken.launch b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/chicken.launch new file mode 100644 index 0000000..db1e1a6 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/chicken.launch @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/chicken_sim.launch b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/chicken_sim.launch new file mode 100644 index 0000000..d2b3407 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/chicken_sim.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/chickenbot.launch b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/chickenbot.launch new file mode 100644 index 0000000..6e5e240 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/chickenbot.launch @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/fake_laser.launch b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/fake_laser.launch new file mode 100644 index 0000000..5d6706b --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/fake_laser.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/rtls.launch b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/rtls.launch new file mode 100644 index 0000000..e7cb94d --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/rtls.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/teleop_keyboard.launch b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/teleop_keyboard.launch new file mode 100644 index 0000000..5725aa9 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/launch/teleop_keyboard.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/map/chicken.bmp b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/map/chicken.bmp new file mode 100644 index 0000000..021a34b Binary files /dev/null and b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/map/chicken.bmp differ diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/map/chicken.jpg b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/map/chicken.jpg new file mode 100644 index 0000000..aa9abf9 Binary files /dev/null and b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/map/chicken.jpg differ diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/map/chicken.yaml b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/map/chicken.yaml new file mode 100644 index 0000000..961b2ab --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/map/chicken.yaml @@ -0,0 +1,6 @@ +image: chicken.bmp +resolution: 0.01 +origin: [-56.60, -7.5, 0.] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/map/lab.jpg b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/map/lab.jpg new file mode 100644 index 0000000..efdd5bf Binary files /dev/null and b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/map/lab.jpg differ diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/package.xml b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/package.xml new file mode 100644 index 0000000..4c9dc31 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/package.xml @@ -0,0 +1,12 @@ + + chickenbot + 0.0.7 + + This package provides chickenbot's configurations and launch scripts. + + Sam Kuo + MIT + + catkin + rospy + \ No newline at end of file diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/src/motor_controller.py b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/src/motor_controller.py new file mode 100755 index 0000000..ad1e729 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/src/motor_controller.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +from abc import ABCMeta, abstractmethod +import rospy +from geometry_msgs.msg import Twist +import smbus + + +__author__ = "Sam Kuo" +__maintainer__ = "Sam Kuo" +__email__ = "kuo77122@gmail.com" +__version__ = "0.1" + + +class API(): + __metaclass__ = ABCMeta + @abstractmethod + def transform(self, linear_velocity, angular_velocity): + pass + + @abstractmethod + def send_cmd(self): + pass + +# class SerialAPI(API): +# """Provide serial interface to communiting with the arduino motor controller +# """ + +class SMBusAPI(API): + """Provide I2C interface to communiting with the arduino motor controller + """ + def __init__(self, turning_threshold, speed_threshold, address, bus_idx=1): + # validate parameters + self.bus_idx = bus_idx + if address is None: + raise ValueError('For SMBUS, device address must be provided') + self.address = address + if len(turning_threshold) != 2: + raise ValueError('Should give turning_threshold two value. (1st_th, 2nd_th)') + self.turning_threshold = tuple(sorted(turning_threshold)) + if len(speed_threshold) != 2: + raise ValueError('Should give speed_threshold two value. (1st_th, 2nd_th)') + self.speed_threshold = tuple(sorted(speed_threshold)) + # SMBus Init. + self.bus = smbus.SMBus(self.bus_idx) + rospy.loginfo("Init SMBus({} with device address: {}".format(self.bus_idx, self.address)) + rospy.loginfo("Setting speed_threshold={} and turning_threshold={}".format(speed_threshold, turning_threshold)) + # TODO: check smbus device is exist + + def transform(self, linear_velocity, angular_velocity): + """ 20202-05-25 by 葉逸新 + 從Rpi到Arduino的指令是 ~三位數~ 數字: _ _ _ + 分別是： linear x, angular z, speed + 0停, 1前進, 2後退 | 0 直行, 1小左轉, 3大左轉 ,2小右轉, 4大右轉 | 速度 1～3段 + ex:101代表以第一段速度直行 + 另外它每個指令會執行兩秒，兩秒內沒收到新指令就車會停住 + """ + cmd = [0, 0, 0] + # 1st cmd + if linear_velocity > 0: + cmd[0] = 1 + elif linear_velocity < 0: + cmd[0] = 2 + else: + cmd[0] = 0 + # 2nd cmd + if abs(angular_velocity) < self.turning_threshold[0]: + cmd[1] = 0 + else: + if angular_velocity > 0: + if abs(angular_velocity) < self.turning_threshold[1]: + cmd[1] = 1 + else: # abs(angular_velocity) > self.turning_threshold[1]: + cmd[1] = 3 + else: # angular_velocity < 0 + if abs(angular_velocity) < self.turning_threshold[1]: + cmd[1] = 2 + else: # abs(angular_velocity) > self.turning_threshold[1]: + cmd[1] = 4 + # 3rd cmd + if abs(linear_velocity) < self.speed_threshold[0]: + cmd[2] = 1 + elif abs(linear_velocity) < self.speed_threshold[1]: + cmd[2] = 2 + else: + cmd[2] = 3 + rospy.logdebug("Gernerate {} from (linear.x, angular.z) = ({}, {})".format(cmd, linear_velocity, angular_velocity)) + return sum(d * 10**i for i, d in enumerate(cmd[::-1])) # [1,2,3] -> 123 + + def send_cmd(self, cmd): + rospy.logdebug("Send '{}' to device".format(cmd)) + self.bus.write_byte(self.address, cmd) + + +class MotorController(object): + protocol_mapping = {'i2c': SMBusAPI, + 'smbus': SMBusAPI} + + def __init__(self, cmd_vel_topic, arduino_protocol, **kwargs): + self.cmd_vel_subscriber = rospy.Subscriber(cmd_vel_topic, Twist, self.callback) + if self.protocol_mapping.get(arduino_protocol) is None: + raise Exception("Protocol {} can not be recognized.".format(arduino_protocol)) + # init arduino interface + self.arduino_api_instance = self.protocol_mapping.get(arduino_protocol)(**kwargs) + + def callback(self, msg): + rospy.logdebug("Received a /cmd_vel message!") + rospy.logdebug("Linear Components: [{}, {}, {}]".format(msg.linear.x, msg.linear.y, msg.linear.z)) + rospy.logdebug("Angular Components: [{}, {}, {}]".format(msg.angular.x, msg.angular.y, msg.angular.z)) + api_cmd = self.arduino_api_instance.transform(msg.linear.x, msg.angular.z) + rospy.logdebug("Received api_cmd: {}".format(api_cmd)) + self.arduino_api_instance.send_cmd(api_cmd) + + +if __name__ == '__main__': + rospy.init_node('motor_controller') + cmd_vel_topic = rospy.get_param('~cmd_vel_topic', '/cmd_vel') + arduino_protocol = rospy.get_param('~arduino_protocol') + arduino_address = rospy.get_param('~arduino_addresss', None) + turning_threshold = rospy.get_param('~turning_threshold') + speed_threshold = rospy.get_param('~speed_threshold') + bus_idx = rospy.get_param('~bus_idx', None) + # rosrun chickenbot motor_controller.py _arduino_protocol:=i2c _arduino_addresss:=4 _turning_threshold:=[1,2] _speed_threshold:=[1,2] _bus_idx:=1 + MotorController(cmd_vel_topic=cmd_vel_topic, + arduino_protocol=arduino_protocol, + address=arduino_address, + turning_threshold=turning_threshold, + speed_threshold=speed_threshold, + bus_idx=bus_idx) + rospy.spin() diff --git a/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/src/rtls.py b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/src/rtls.py new file mode 100755 index 0000000..f19db1a --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/chickenbot/src/rtls.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +from abc import ABCMeta, abstractmethod +import rospy +from geometry_msgs.msg import PoseWithCovarianceStamped +import requests + +__author__ = "Sam Kuo" +__maintainer__ = "Sam Kuo" +__email__ = "kuo77122@gmail.com" +__version__ = "0.1" + + +class RTLS(object): + + def __init__(self, topic, server_url, frame_id, **kwargs): + self.frame_id = frame_id + self.pose_pub = rospy.Publisher(topic, PoseWithCovarianceStamped, queue_size=10) + self.server_url = server_url + try: + response = requests.get(self.server_url) + response.raise_for_status() + except Exception as e: + rospy.logfatal("Cannot build connection to server {}. {}".format(self.server_url, e)) + + def decode_msg(self, raw_msg): + pose_stamped = PoseWithCovarianceStamped() + pose_stamped.header.frame_id = self.frame_id + pose_stamped.header.stamp = rospy.Time.now() + + pose_stamped.pose.pose.position.x = raw_msg.get("x") + pose_stamped.pose.pose.position.y = raw_msg.get("y") + # pose_stamped.pose.pose.position.z = position.get("z") + + # RTLS donot contain orientation, skip + # pose_stamped.pose.pose.orientation.y = orientation.get("x") + # pose_stamped.pose.pose.orientation.x = orientation.get("y") + # pose_stamped.pose.pose.orientation.z = orientation.get("z") + # pose_stamped.pose.pose.orientation.w = orientation.get("w") + + # RTLS may provide convariance in the future + # pose_stamped.pose.covariance = covariance + return pose_stamped + + def get_position(self): + """ + curl http://192.168.49.3:5000/api/v1/tagPosition + {"name":"Tag-01","x":141.01423987174698,"y":440.1414863167405 + """ + try: + response = requests.get(self.server_url) + response.raise_for_status() + except Exception as e: + rospy.logwarn("loss connection with server {}. {}".format(self.server_url, e)) + try: + data = response.json() + if "x" not in data.keys() or "y" not in data.keys(): + rospy.logerror("Decode Error. cannot find x, y in response") + return + return data + except Exception as e: + rospy.logerror(e) + + def send2topic(self, pose_stamped): + self.pose_pub.publish(pose_stamped) + + +if __name__ == '__main__': + rospy.init_node('rtls') + server_url = rospy.get_param('~server_url') + localization_topic = rospy.get_param('~pose_topic', '/rtls_pose') + query_rate = rospy.get_param('~query_rate', 1) + frame_id = rospy.get_param('~frame_id', 'rtls') + rate = rospy.Rate(query_rate) + + rtls_api = RTLS(topic=localization_topic, + server_url=server_url, + frame_id=frame_id) + + while not rospy.is_shutdown(): + raw_msg = rtls_api.get_position() + if raw_msg is not None: + pose_stamped = rtls_api.decode_msg(raw_msg) + rtls_api.send2topic(pose_stamped) + rate.sleep() diff --git a/DockerFiles/ROS1.0/catkin_ws/src/fake_laser/CMakeLists.txt b/DockerFiles/ROS1.0/catkin_ws/src/fake_laser/CMakeLists.txt new file mode 100644 index 0000000..524a95b --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/fake_laser/CMakeLists.txt @@ -0,0 +1,183 @@ +cmake_minimum_required(VERSION 2.8.3) +project(fake_laser) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + tf + roscpp +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES fake_laser +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(fake_laser +# src/${PROJECT_NAME}/fake_laser_node.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(fake_laser ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +add_executable(fake_laser_node src/fake_laser_node.cpp) +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(fake_laser_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(fake_laser_node ${catkin_LIBRARIES}) +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS fake_laser fake_laser_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_fake_laser.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/DockerFiles/ROS1.0/catkin_ws/src/fake_laser/package.xml b/DockerFiles/ROS1.0/catkin_ws/src/fake_laser/package.xml new file mode 100644 index 0000000..7cbcc57 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/fake_laser/package.xml @@ -0,0 +1,49 @@ + + + fake_laser + 0.0.0 + The fake_laser package + + + + + jonathan + + + + + + TODO + + + + + + + + + + + + + + + + + + tf + + + + + + + catkin + + + + + + + + diff --git a/DockerFiles/ROS1.0/catkin_ws/src/fake_laser/src/fake_laser_node.cpp b/DockerFiles/ROS1.0/catkin_ws/src/fake_laser/src/fake_laser_node.cpp new file mode 100644 index 0000000..f7d42ba --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/fake_laser/src/fake_laser_node.cpp @@ -0,0 +1,109 @@ + +#include "ros/ros.h" +#include "sensor_msgs/LaserScan.h" + +#define DEG2RAD(x) ((x)*M_PI/180.) +#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) + +void publish_scan(ros::Publisher& pub, + size_t node_count, ros::Time start, + double scan_time, bool inverted, + float angle_min, float angle_max, + std::string frame_id) +{ + static int scan_count = 0; + sensor_msgs::LaserScan scan_msg; + + scan_msg.header.stamp = start; + scan_msg.header.frame_id = frame_id; + scan_count++; + + scan_msg.angle_min = M_PI - angle_min; + scan_msg.angle_max = M_PI - angle_max; + scan_msg.angle_increment = + (scan_msg.angle_max - scan_msg.angle_min) / (double)(node_count-1); + + scan_msg.scan_time = scan_time; + scan_msg.time_increment = scan_time / (double)(node_count-1); + + scan_msg.range_min = 0.15; + scan_msg.range_max = 6.; + + scan_msg.intensities.resize(node_count); + scan_msg.ranges.resize(node_count); + if (!inverted) { // assumes scan window at the top + for (size_t i = 0; i < node_count; i++) { + float read_value = (float) 0.0; + if (read_value == 0.0) + scan_msg.ranges[i] = std::numeric_limits::infinity(); + else + scan_msg.ranges[i] = read_value; + scan_msg.intensities[i] = (float) 0.0; + } + } else { + for (size_t i = 0; i < node_count; i++) { + float read_value = (float)0.0; + if (read_value == 0.0) + scan_msg.ranges[node_count-1-i] = std::numeric_limits::infinity(); + else + scan_msg.ranges[node_count-1-i] = read_value; + scan_msg.intensities[node_count-1-i] = (float) 0.0; + } + } + + pub.publish(scan_msg); +} + +static const std::string kFrameIdKey_ = "frame_id"; +static const std::string kMaxAngleKey_ = "angle_max"; +static const std::string kMinAngleKey_ = "angle_min"; +static const std::string kIsRadKey_ = "is_rad"; + +int main(int argc, char * argv[]) { + + ros::init(argc, argv, "rplidar_node"); + + ros::NodeHandle nh; + ros::NodeHandle prv_nh("~"); + ros::Publisher scan_pub = nh.advertise("scan", 1000); + std::string _frame_id; + bool _is_rad; + float angle_min; + float angle_max; + prv_nh.param(kFrameIdKey_, _frame_id, "laser"); + prv_nh.param(kIsRadKey_, _is_rad, false); + if (_is_rad) { + prv_nh.param(kFrameIdKey_, angle_max, 0.0); + prv_nh.param(kFrameIdKey_, angle_min, 3.14); + } else { + prv_nh.param(kFrameIdKey_, angle_max, 0.0); + prv_nh.param(kFrameIdKey_, angle_min, 359.0); + angle_min = DEG2RAD(angle_min); + angle_max = DEG2RAD(angle_max); + } + + size_t count = 360; + + bool inverted = false; + + ros::Time start_scan_time; + ros::Time end_scan_time; + double scan_duration; + + ros::Rate rate(15); + + while (ros::ok()) { + + start_scan_time = ros::Time::now(); + end_scan_time = ros::Time::now(); + scan_duration = (end_scan_time - start_scan_time).toSec() * 1e-3; + + publish_scan(scan_pub, count, + start_scan_time, scan_duration, inverted, + angle_min, angle_max, + _frame_id); + rate.sleep(); + } + + return 0; +} diff --git a/DockerFiles/ROS1.0/catkin_ws/src/my_odom/CHANGELOG.rst b/DockerFiles/ROS1.0/catkin_ws/src/my_odom/CHANGELOG.rst new file mode 100644 index 0000000..113e8c6 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/my_odom/CHANGELOG.rst @@ -0,0 +1,11 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for my_odom +^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.2 2019-06-27 +* 1) change to receive several encoder for calculating once +* 2) add static_period feature + +1.0.1 2018-03-23 +------------------- +* This process uses the encoder topic published from motor node to calculate + the odometry and publish ther result out to other process. diff --git a/DockerFiles/ROS1.0/catkin_ws/src/my_odom/CMakeLists.txt b/DockerFiles/ROS1.0/catkin_ws/src/my_odom/CMakeLists.txt new file mode 100644 index 0000000..016cefb --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/my_odom/CMakeLists.txt @@ -0,0 +1,189 @@ +cmake_minimum_required(VERSION 2.8.3) +project(my_odom) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + tf + std_msgs + roscpp +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES my_odom +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(my_odom +# src/${PROJECT_NAME}/my_odom.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(my_odom ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +add_executable(my_odom_node src/my_odom_node.cpp) +add_executable(my_odom_sim src/my_odom_sim.cpp) +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(my_odom_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +#version_helper_lib +target_link_libraries(my_odom_node ${catkin_LIBRARIES}) +target_link_libraries(my_odom_sim ${catkin_LIBRARIES}) +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation + install(TARGETS + my_odom_node + my_odom_sim + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_my_odom.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/DockerFiles/ROS1.0/catkin_ws/src/my_odom/include/compal_agv/version.h b/DockerFiles/ROS1.0/catkin_ws/src/my_odom/include/compal_agv/version.h new file mode 100644 index 0000000..e03de03 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/my_odom/include/compal_agv/version.h @@ -0,0 +1,51 @@ +/* + * Copyright (c) 2016, Compal Electronics, 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: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of + * conditions and the following disclaimer. + * + * 2. 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. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 INCLUDE_COMPAL_AGV_VERSION_H_ +#define INCLUDE_COMPAL_AGV_VERSION_H_ + +#include + +namespace compal_common { + +class VersionHelper { + public: + VersionHelper(std::string package_name, std::string version_number); + ~VersionHelper(); + void PrintVersion(); + void SetParameter(); + private: + static const std::string kRootString_; + std::string package_name_; + std::string version_number_; + ros::NodeHandle nh_private_; +}; + +}; // namespace compal_common +#endif // INCLUDE_COMPAL_AGV_VERSION_H_ diff --git a/DockerFiles/ROS1.0/catkin_ws/src/my_odom/package.xml b/DockerFiles/ROS1.0/catkin_ws/src/my_odom/package.xml new file mode 100644 index 0000000..892f901 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/my_odom/package.xml @@ -0,0 +1,49 @@ + + + my_odom + 0.0.0 + The my_odom package + + + + + jonathan + + + + + + TODO + + + + + + + + + + + + + + + + + + tf + + + + + + + catkin + + + + + + + + diff --git a/DockerFiles/ROS1.0/catkin_ws/src/my_odom/past_log.txt b/DockerFiles/ROS1.0/catkin_ws/src/my_odom/past_log.txt new file mode 100644 index 0000000..1ada310 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/my_odom/past_log.txt @@ -0,0 +1,202 @@ +commit f484cd9c0fd13efcb7f5cbb07ab6ccd795c74114 +Author: Peter_Tsai +Date: Wed Oct 5 11:24:17 2016 +0800 + + Marge KSP3 changes + The major changes: app_server , add KSP3 paths and fine tune some planner parameters + +commit 953aeb0603492308f09ee725af2c23a3f84efd70 +Author: Peter_Tsai +Date: Fri Aug 19 15:10:10 2016 +0800 + + [DEVELOP DWA] Add another trajectory rviz topic + 1. [DEVELOP DWA] Add another trajectory rviz topic. Make sure the publish_traj_pc is true in the dwa config file + 2. [DEVELOP STAGE] Add Stage launch file (Stage is a robot simulator) + 3. [DEVELOP GLOBAL_PLANNER] Add using the nearest point as the middle point when obstacle is true + 4. [DEVELOP MAP_SERVER] Add supporting original map yaml tag. + +commit d9dd9c1a5e44bb06169db015e6c9cf0c7139eeee +Author: jonathan +Date: Tue Aug 16 14:57:44 2016 +0800 + + [DEVELOP MY_ODOM] Save the robot position in a file(amcl_pose.yaml) for next time to re-location. + +commit 9003177c268177a37f84414677cb87dd22487488 +Author: steven_hsieh +Date: Fri Jul 29 13:48:14 2016 +0800 + + modified my_odom + +commit 3fd848b715a97b70d5bc1c6de1fbfdb82460ebe0 +Author: Peter_Tsai +Date: Wed Jul 20 14:54:32 2016 +0800 + + [DEVELOP MY_ODOM] Add setting frame_id from launch + frame_id_odom(Default:"odom") + frame_id_base_link_tf(Default:"base_link") + frame_id_base_link_pub(Default:"base_link") + +commit d3402fd7fe67c78e3a780ac3a35d9942948ad2ca +Author: jonathan +Date: Wed Jul 6 17:56:22 2016 +0800 + + [DEVELOP MY_ODOM] Remove some verbose printf message. + +commit bda738289b97a2b1569137022e6a31149f814a43 +Author: jonathan +Date: Thu Jun 30 20:05:09 2016 +0800 + + [ENHANCE MY_ODOM] To avoid warning message, change polling interval of printing message. + +commit 69ec8eee3b4b1c7125091f27d527c4fe18af81f3 +Author: jonathan +Date: Mon Jun 27 14:36:59 2016 +0800 + + [DEVLEOP MY_ODOM] + 1. Add argument to enable encode simulation + 2. Fixed in-place rotation calculation formula. + +commit 35d9764bc14655a6d106ba89ec2236ad213f0d69 +Author: jonathan +Date: Tue Jun 21 10:41:34 2016 +0800 + + [ENHANCE MY_ODOM] Rename my_odom_simulation.cpp -> my_odom_sim.cpp + +commit 7943ea34f019ef750c005fa2e98dfdbaf24dfcca +Author: jonathan +Date: Tue Jun 21 10:36:38 2016 +0800 + + [ENHANCE MY_ODOM] Remove unused MACRO. + +commit 3426b3f14cb09ab63d3fda544ddfafa4fee22f1b +Author: jonathan +Date: Tue Jun 21 10:33:39 2016 +0800 + + [DEVELOP MY_ODOM] 1. Add encode simulation to my_odom. 2. Add config to enable amcl. + +commit a2af23ac9f48b08d44a6327d8814d9db4d1b49a1 +Author: jonathan +Date: Fri Jun 17 15:22:08 2016 +0800 + + [FIXBUG MY_ODOM] my_odom shouldn't start calculating the encode when it hasn't publish value. + +commit 504b1930e33f762c7b186b4a5e74db6418895f8e +Author: jonathan +Date: Mon Jun 6 16:57:32 2016 +0800 + + [FIXBUG MY_ODOM] Some ROS distribution cannot get parameters with non-private nodeHandler. + +commit d4e533e666deda135eec71b6988a74754f06b707 +Author: jonathan +Date: Mon Jun 6 15:44:39 2016 +0800 + + [FIXBUG MY_ODOM] Fixed topic namespace problem. + +commit ccba869755f3db542ca0e9bae447bc24ef4b71e5 +Author: jonathan +Date: Fri Jun 3 15:28:57 2016 +0800 + + [DEVELOP MY_ODOM] Assign my_odom setting by node parameter. + +commit f86ba463190b4bdb2037c568871c9d51195712d0 +Author: Steven_Hsieh +Date: Thu May 19 10:48:52 2016 +0800 + + parameters modified + +commit 3c5d0c8d8e53bdb0ee93cf4370894cc98b95db52 +Author: jonathan +Date: Thu May 12 13:37:10 2016 +0800 + + [DEVLEOP MY_ODOM] Add simulation function to my_odom for observating path control. + +commit 00bee45d719c72733d623b55e581fe508c02e50c +Author: jonathan +Date: Tue May 10 09:26:16 2016 +0800 + + [FIXBUG MY_ODOM] Fixed encoder counter loses. + +commit 7331c0f35fdf8a5528f6bb40317870e43e286bfc +Author: jonathan +Date: Tue May 10 09:18:50 2016 +0800 + + [FIXBUG MY_ODOM] my_odom should publish velocity in /odom, rather than moving distance. + +commit 98a83ded5921399ec8e0e351a94c1c4313133ef6 +Author: Steven_Hsieh +Date: Thu Apr 21 10:43:39 2016 +0800 + + add AGV odom node + +commit 2a56b7389ea7510dcef85fb0df2c95d69caa7ed4 +Author: Steven_Hsieh +Date: Thu Apr 21 10:35:17 2016 +0800 + + modify my_odom_node backword + +commit a2edea730e2431bb0feb305d932be96cce165b74 +Merge: fa0a0f5 957f731 +Author: Steven_Hsieh +Date: Mon Apr 18 15:50:28 2016 +0800 + + Merge branch 'master' of 192.168.100.104:/SrcGit/compal_agv_ros_git + + Conflicts: + src/my_odom/src/my_odom_node.cpp + +commit fa0a0f56a5a67c1e20b3412b55837ff080e5976e +Author: Steven_Hsieh +Date: Mon Apr 18 15:49:04 2016 +0800 + + my_odom modify + +commit 957f7319f09b451b25f9f3e4bebaaf044a13bf89 +Author: jonathan +Date: Fri Apr 15 16:57:09 2016 +0800 + + [FIXBUG ODOM] Fixed incompatible data type, which can cause motor counter error. + +commit e0b84ea050df4d6cfccfcf6452908245037cc58c +Author: Grace +Date: Fri Mar 25 18:12:49 2016 +0800 + + add back function + add find_circle, modified my_odom, back_and_forth + +commit 9632f9643f20df88eacd57f78d81ce9135dd7174 +Author: Grace +Date: Fri Mar 18 17:46:18 2016 +0800 + + 1. modified my_odom_node (deltax,deltay define error) + 2. modified params about amcl & base_move + 3. new map + +commit 4efaf3d83fa60a20612fb14f2a033ab47c2e7697 +Author: Jonathan Tseng +Date: Thu Mar 10 21:23:36 2016 -0800 + + [DEVELOP ODOM] Update count per rotation to 714. + +commit 4db6109eccf657b521171cd6eef332359a067547 +Author: Jonathan Tseng +Date: Thu Mar 10 20:56:46 2016 -0800 + + [DEVELOP ODOM] Since the detect on motor decode chip, the count per rotation is changed to 357. + +commit c73cb465a8174d56c15d7eb2178d86bd32146ea7 +Author: Jonathan Tseng +Date: Thu Feb 25 01:20:58 2016 -0800 + + [DEVELOP] Improve odometry accuracy. + +commit b0c22a8633452d3fdbe869f0973dd6e5a6b5f545 +Author: Jonathan Tseng +Date: Tue Feb 23 03:33:02 2016 -0800 + + [DEVELOP SLAM] Add wheel encoder as a reference for tf and odometry. + +commit 9e4be803e564b80280814dda5df1054674a2620d +Author: jonathan +Date: Mon Feb 22 14:10:11 2016 +0800 + + [DEVELOP] Add package my_odom to improve localization when using a map. diff --git a/DockerFiles/ROS1.0/catkin_ws/src/my_odom/src/my_odom_node.cpp b/DockerFiles/ROS1.0/catkin_ws/src/my_odom/src/my_odom_node.cpp new file mode 100644 index 0000000..37d7ac0 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/my_odom/src/my_odom_node.cpp @@ -0,0 +1,375 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include +#include + +#define ROBOT_POSE_FILE "/tmp/amcl_pose.yaml" + +#define PUB_HZ 40.0 + +#define MAX_VX 1.0 +#define MAX_VY 1.0 +#define MAX_VTH M_PI + +#define LEFT_ENCODE_NAME "motor/odom_L" +#define RIGHT_ENCODE_NAME "motor/odom_R" +#define MILEAGE "compal_debug/mileage" +#define RECORD_MAP_FLOOR "map_floor" +#define VERSION_PACKAGE_NAME "MY_ODOM" +#define VERSIOM_NUMBER "1.0.2" + +#define MIN(a,b) (((a)<(b))?(a):(b)) +#define MAX(a,b) (((a)>(b))?(a):(b)) + +#define ENCODE_IS_INIT() (g_left_encode_is_init && g_right_encode_is_init) + +#define INVALID_VELOCITY(vx, vy, vth) (abs(vx) > MAX_VX || abs(vy) > MAX_VY || abs(vth) > MAX_VTH) + +boost::mutex mtx_left; +boost::mutex mtx_right; + +/* PARAMETERS */ +double g_cnt_per_cycle = 0; +double g_wheel_radius = 0; +double g_wheel_distance = 0; + +double g_distance_per_cnt = 0; +double g_angle_per_cnt_r = 0; +double g_angle_per_cnt_R = 0; + +std_msgs::Float64 g_pub_mileage; +std::string g_map_floor = "1"; + +std::string g_frame_id_odom = "odom"; +std::string g_frame_id_base_link_tf = "base_link"; +std::string g_frame_id_base_link_pub = "base_link"; + +int32_t g_left_encode = 0; +int32_t g_right_encode = 0; + +int g_left_encode_recv_count = 0; +int g_right_encode_recv_count = 0; + +bool g_left_encode_is_init = false; +bool g_right_encode_is_init = false; +bool g_enable_write_pose = true; +bool g_enable_static_period = true; +int g_encode_recv_count = 0; +double g_static_period_num = 0.1; + +tf::StampedTransform g_transform; +std::string g_pose_file; + +ros::Time current_time, last_time; + +void write_position(void) { + + std::ofstream pos_file; + + std::string temp_pose_file = g_pose_file + "~"; + + pos_file.open(temp_pose_file.c_str()); + pos_file << "initial_pose_x: " << g_transform.getOrigin().x() << "\n"; + pos_file << "initial_pose_y: " << g_transform.getOrigin().y() << "\n"; + pos_file << "initial_pose_a: " << tf::getYaw(g_transform.getRotation()) << "\n"; + pos_file << "total_milege_distance: " << g_pub_mileage.data << "\n"; + pos_file << "floor: " << g_map_floor << "\n"; + pos_file.close(); + + if (std::rename(temp_pose_file.c_str(),g_pose_file.c_str()) != 0) + ROS_ERROR( "Error renaming file %s -> %s",temp_pose_file.c_str(),g_pose_file.c_str() ); +} + +void sigint_handler(int sig) +{ + write_position(); + + ROS_INFO_STREAM("[MY_ODOM] Save AGV pose(" + << g_transform.getOrigin().x() << ", " + << g_transform.getOrigin().y() << ", " + << tf::getYaw(g_transform.getRotation()) << ") , map_floor: " + << g_map_floor << " and total_mileage: " + << g_pub_mileage.data << " to file: " << g_pose_file); + + ros::shutdown(); +} + +void get_map_floor(const std_msgs::String::ConstPtr& sMapfloor) +{ + g_map_floor = sMapfloor->data; +} + +void encode_left_recv(const std_msgs::Int32& value) +{ + //mtx_left.lock(); + g_left_encode = value.data; + //mtx_left.unlock(); + + g_left_encode_is_init = true; + g_left_encode_recv_count ++; +} + + +void encode_right_recv(const std_msgs::Int32& value) +{ + //mtx_right.lock(); + g_right_encode = value.data; + //mtx_right.unlock(); + + g_right_encode_is_init = true; + g_right_encode_recv_count++; +} + +void calculate_robot_move(int left_encode_diff, int right_encode_diff, double &dx, double &dy, double &dth) { + + int32_t count = 0; + int diff_count = 0; + double concentric_radius=0.0; + double ratio = 0.0; + + double left_move_distance = 0.0; + double right_move_distance = 0.0; + + left_move_distance = left_encode_diff * g_distance_per_cnt; + right_move_distance = right_encode_diff * g_distance_per_cnt; + + if (left_move_distance == right_move_distance) + { + dx = left_move_distance; + dy = 0.0; + dth = 0.0; + } + else + { + dth = (right_move_distance - left_move_distance)/g_wheel_distance; + concentric_radius = (right_move_distance + left_move_distance)/(2*dth); + dx = concentric_radius * sin(dth); + dy = concentric_radius * (cos(dth)-1); + } + +} + + +int main(int argc, char** argv){ + + ros::init(argc, argv, "odometry_publisher", ros::init_options::NoSigintHandler); + signal(SIGINT, sigint_handler); + + ros::NodeHandle n; + ros::Publisher odom_pub = n.advertise("odom", 50); + tf::TransformBroadcaster odom_broadcaster; + tf::TransformListener listener; + + ros::Subscriber encoder_left_sub = n.subscribe(LEFT_ENCODE_NAME, 1000, &encode_left_recv); + ros::Subscriber encoder_right_sub = n.subscribe(RIGHT_ENCODE_NAME, 1000, &encode_right_recv); + ros::Subscriber record_map_floor_sub = n.subscribe(RECORD_MAP_FLOOR, 1 , get_map_floor); + ros::Publisher milege_pub = n.advertise(MILEAGE , 50); + + double total_x = 0.0; + double x = 0.0; + double y = 0.0; + double th = 0.0; + + double dx = 0.0; + double dy = 0.0; + double dth = 0.0; + + double vx = 0.0; + double vy = 0.0; + double vth = 0.0; + + int32_t last_left_encode; + int32_t last_right_encode; + + int left_encode_diff = 0; + int right_encode_diff = 0; + + uint32_t tick = 0; + + current_time = ros::Time::now(); + last_time = ros::Time::now(); + + ros::Rate r(PUB_HZ); + ros::Rate r_wait(200); + + ros::NodeHandle nh_private("~"); + + /* Get parameters */ + nh_private.param("cnt_per_cycle", g_cnt_per_cycle, 714.0); + nh_private.param("wheel_radius", g_wheel_radius, 0.04); + nh_private.param("wheel_distance", g_wheel_distance, 0.29); + + nh_private.param("frame_id_odom", g_frame_id_odom, std::string("odom")); + nh_private.param("frame_id_base_link_tf", g_frame_id_base_link_tf, std::string("base_link")); + nh_private.param("frame_id_base_link_pub", g_frame_id_base_link_pub, std::string("base_link")); + nh_private.param("pose_file_param", g_pose_file, std::string(ROBOT_POSE_FILE)); + nh_private.param("total_milege_distance" , g_pub_mileage.data , 0.0 ); + nh_private.param("enable_write_pose", g_enable_write_pose, true); + nh_private.param("enable_static_period", g_enable_static_period, false); + nh_private.param("static_period_num", g_static_period_num, 0.1); + nh_private.param("encoder_recv_num", g_encode_recv_count, 2); + //compal_common::VersionHelper version(VERSION_PACKAGE_NAME, VERSIOM_NUMBER); + + g_distance_per_cnt = ( ( 2 * g_wheel_radius * M_PI ) / g_cnt_per_cycle ); + g_angle_per_cnt_R = ( g_distance_per_cnt / g_wheel_distance ); + g_angle_per_cnt_r = ( g_distance_per_cnt / (g_wheel_distance/2) ); + + ROS_INFO_STREAM( "MY_ODOM parameter list:"); + ROS_INFO_STREAM( " - cnt_per_cycle = " << g_cnt_per_cycle); + ROS_INFO_STREAM( " - wheel_radius = " << g_wheel_radius); + ROS_INFO_STREAM( " - wheel_distance = " << g_wheel_distance); + + ROS_INFO_STREAM( " - frame_id_odom = " << g_frame_id_odom); + ROS_INFO_STREAM( " - frame_id_base_link_tf = " << g_frame_id_base_link_tf); + ROS_INFO_STREAM( " - frame_id_base_link_pub = " << g_frame_id_base_link_pub); + ROS_INFO_STREAM( " - g_enable_write_pose = " << g_enable_write_pose); + ROS_INFO_STREAM(" - enable_static_period = " << g_enable_static_period); + ROS_INFO_STREAM(" - static_period_num = " << g_enable_static_period); + ROS_INFO_STREAM( " - encoder_recv_num = " << g_encode_recv_count); + + ros::spinOnce(); + + while (!ENCODE_IS_INIT() && n.ok()) { + ros::Duration(2.0).sleep(); + ROS_WARN_STREAM( "[MY_ODOM] Encode hasn't been updated yet...plz check topic " + << RIGHT_ENCODE_NAME << " and " + << LEFT_ENCODE_NAME); + ros::spinOnce(); + } + + last_left_encode = g_left_encode; + last_right_encode = g_right_encode; + + ROS_INFO_STREAM( "[MY_ODOM] Encode is recv!! MY_ODOM is ready to go."); + + while (n.ok()) + { + dx = 0.0; + dy = 0.0; + dth = 0.0; + + vx = 0.0; + vy = 0.0; + vth = 0.0; + + while (!(g_left_encode_recv_count >=g_encode_recv_count + && g_right_encode_recv_count >=g_encode_recv_count)) + { + ros::spinOnce(); // check for incoming messages + r_wait.sleep(); + } + current_time = ros::Time::now(); + g_left_encode_recv_count = 0; + g_right_encode_recv_count = 0; + + right_encode_diff = g_right_encode - last_right_encode; + left_encode_diff = g_left_encode - last_left_encode; + + calculate_robot_move(left_encode_diff, right_encode_diff, dx, dy, dth); + + //compute odometry in a typical way given the velocities of the robot + double dt = (current_time - last_time).toSec(); + if (g_enable_static_period) + dt = g_static_period_num; + //double delta_x = (vx * cos(th) - vy * sin(th)) * dt; + //double delta_y = (vx * sin(th) + vy * cos(th)) * dt; + //double delta_th = vth * dt; + + vx = dx / dt; + vy = dy / dt; + vth = dth / dt; + + //ROS_INFO( "[MY_ODOM] VEL = (%0.3f, %0.3f, %0.3f) ENC = (%d, %d) time = %f", vx, vy, vth, left_encode_diff, right_encode_diff, dt); + //ROS_INFO( "[MY_ODOM] VELOCITY INTERVAL = %f", dt); + + if (INVALID_VELOCITY(vx, vy, vth)) { + ROS_ERROR( "[MY_ODOM] INVALID VELOCITY = (%f, %f, %f) ENCODE = (%d, %d) INTERVAL = %f", + vx, vy, vth, left_encode_diff, right_encode_diff, dt); + dx = dy = dth = vx = vy = vth = 0.0; + } + + total_x += dx; + + double delta_x = (dx * cos(th) - dy * sin(th)); + double delta_y = (dx * sin(th) + dy * cos(th)); + double delta_th = dth; + + x += delta_x; + y += delta_y; + th += delta_th; + + g_pub_mileage.data += sqrt(pow(delta_x,2) + pow(delta_y,2)); + milege_pub.publish(g_pub_mileage); + + //since all odometry is 6DOF we'll need a quaternion created from yaw + geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th); + + //first, we'll publish the transform over tf + geometry_msgs::TransformStamped odom_trans; + odom_trans.header.stamp = current_time; + odom_trans.header.frame_id = g_frame_id_odom; + odom_trans.child_frame_id = g_frame_id_base_link_tf; + + if (vx != 0 || vth != 0) { + ROS_INFO_STREAM("[MY_ODOM] vx = "<< vx << ", vth =" << vth << ", total_x = " << total_x); + } + + odom_trans.transform.translation.x = x; + odom_trans.transform.translation.y = y; + odom_trans.transform.translation.z = 0.0; + odom_trans.transform.rotation = odom_quat; + + //send the transform + odom_broadcaster.sendTransform(odom_trans); + + //next, we'll publish the odometry message over ROS + nav_msgs::Odometry odom; + odom.header.stamp = current_time; + odom.header.frame_id = g_frame_id_odom; + + //set the position + odom.pose.pose.position.x = x; + odom.pose.pose.position.y = y; + odom.pose.pose.position.z = 0.0; + odom.pose.pose.orientation = odom_quat; + + //set the velocity + odom.child_frame_id = g_frame_id_base_link_pub; + odom.twist.twist.linear.x = vx; + odom.twist.twist.linear.y = vy; + odom.twist.twist.angular.z = vth; + + //publish the message + odom_pub.publish(odom); + + last_time = current_time; + + last_left_encode = g_left_encode; + last_right_encode = g_right_encode; + + if(g_enable_write_pose){ + /* Record robot pose in g_transform, which will be written to file before my_odom terminate */ + if ( listener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(0.01)) ) { + listener.lookupTransform("/map", "/base_link", ros::Time(0), g_transform); + } + } + + r.sleep(); + + if(g_enable_write_pose){ + /* Write position to file every 0.5 second */ + tick = (++tick)%10; + if ( (tick % 5) == 0) write_position(); + } + } +} diff --git a/DockerFiles/ROS1.0/catkin_ws/src/my_odom/src/my_odom_sim.cpp b/DockerFiles/ROS1.0/catkin_ws/src/my_odom/src/my_odom_sim.cpp new file mode 100644 index 0000000..e766e07 --- /dev/null +++ b/DockerFiles/ROS1.0/catkin_ws/src/my_odom/src/my_odom_sim.cpp @@ -0,0 +1,496 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define ROBOT_POSE_FILE "/tmp/amcl_pose.yaml" + +#define PUB_HZ 10.0 + +#define MAX_VX 1.0 +#define MAX_VY 1.0 +#define MAX_VTH M_PI + +#define LEFT_ENCODE_NAME "motor/odom_L" +#define RIGHT_ENCODE_NAME "motor/odom_R" + +#define MIN(a,b) (((a)<(b))?(a):(b)) +#define MAX(a,b) (((a)>(b))?(a):(b)) + +#define INVALID_VELOCITY(vx, vy, vth) (abs(vx) > MAX_VX || abs(vy) > MAX_VY || abs(vth) > MAX_VTH) + +boost::mutex mtx_left; +boost::mutex mtx_right; + +/* PARAMETERS */ +double g_cnt_per_cycle = 0; +double g_wheel_radius = 0; +double g_wheel_distance = 0; +bool g_enable_amcl = 0; +bool g_enable_encode = 0; + +double g_distance_per_cnt = 0; +double g_angle_per_cnt_r = 0; +double g_angle_per_cnt_R = 0; + +int32_t g_left_encode = 0; +int32_t g_right_encode = 0; + +std::string g_frame_id_odom = "odom"; +std::string g_frame_id_base_link_tf = "base_link"; +std::string g_frame_id_base_link_pub = "base_link"; + +bool g_left_encode_is_init = false; +bool g_right_encode_is_init = false; + +#define ENCODE_IS_INIT() (g_left_encode_is_init && g_right_encode_is_init) + +double g_cmd_vel_x; +double g_cmd_vel_z; + +tf::StampedTransform g_transform; +std::string g_pose_file; + +void write_position(void) { + + std::ofstream pos_file; + + std::string temp_pose_file = g_pose_file + "~"; + + pos_file.open(temp_pose_file.c_str()); + pos_file << "initial_pose_x: " << g_transform.getOrigin().x() << "\n"; + pos_file << "initial_pose_y: " << g_transform.getOrigin().y() << "\n"; + pos_file << "initial_pose_a: " << tf::getYaw(g_transform.getRotation()) << "\n"; + pos_file.close(); + + if (std::rename(temp_pose_file.c_str(),g_pose_file.c_str()) != 0) + ROS_ERROR( "Error renaming file%s -> %s",temp_pose_file.c_str(),g_pose_file.c_str() ); +} + +void sigint_handler(int sig) +{ + std::ofstream pos_file; + + write_position(); + + ROS_INFO_STREAM("[MY_ODOM] Save pos(" + << g_transform.getOrigin().x() << ", " + << g_transform.getOrigin().y() << ", " + << tf::getYaw(g_transform.getRotation()) << ") to file: " << g_pose_file); + + ros::shutdown(); +} + +void cmd_vel_recv(const geometry_msgs::Twist& msg) +{ + g_cmd_vel_x = msg.linear.x; + g_cmd_vel_z = msg.angular.z; +} + +void encode_left_recv(const std_msgs::Int32& value) +{ + mtx_left.lock(); + g_left_encode = value.data; + mtx_left.unlock(); + + g_left_encode_is_init = true; +} + + +void encode_right_recv(const std_msgs::Int32& value) +{ + mtx_right.lock(); + g_right_encode = value.data; + mtx_right.unlock(); + + g_right_encode_is_init = true; +} + +void mimic_robot_encode(double cmd_vel_x, double cmd_vel_z, double wheel_dist, double dist_per_cnt, int32_t left_encode, int32_t right_encode, ros::Publisher &encode_left_pub, ros::Publisher &encode_right_pub) +{ + double left_move_dist, right_move_dist; + double move_dist = cmd_vel_x / PUB_HZ; + double move_angle = cmd_vel_z / PUB_HZ; + std_msgs::Int32 encode; + + if (cmd_vel_x == 0.0 && cmd_vel_z != 0) { + + /* In-place Rotation */ + left_move_dist = -(wheel_dist * move_angle / 2); + right_move_dist = (wheel_dist * move_angle / 2); + + } else if (cmd_vel_x != 0.0 && cmd_vel_z == 0.0) { + + /* Move forward */ + right_move_dist = left_move_dist = move_dist; + + } else if (cmd_vel_x != 0.0 && cmd_vel_z != 0.0) { + + /* Move in a curve */ + left_move_dist = move_dist - (wheel_dist * move_angle / 2); + right_move_dist = move_dist + (wheel_dist * move_angle / 2); + + } + + //publish the encoder + encode.data = left_encode + left_move_dist / dist_per_cnt; + encode_left_pub.publish(encode); + encode.data = right_encode + right_move_dist / dist_per_cnt; + encode_right_pub.publish(encode); +} + +void mimic_robot_move(double cmd_vel_x, double cmd_vel_z, double &dx, double &dth) { + dx = cmd_vel_x / PUB_HZ; + dth = cmd_vel_z / PUB_HZ; +} + +void calculate_robot_move(int left_encode_diff, int right_encode_diff, double &dx, double &dth) { + + int32_t count = 0; + int diff_count = 0; + double concentric_radius=0.0; + double ratio = 0.0; + + + //ROS_WARN_STREAM("left_encode_diff:"< 0 && right_encode_diff < 0 ) + { + count = (abs(left_encode_diff)+abs(right_encode_diff))/2; + dth = -count * g_angle_per_cnt_r; + dx = 0; + //ROS_INFO_STREAM("Right Rotation!! >0 <0"); + } + else if ( left_encode_diff < 0 && right_encode_diff >0) + { + count = (abs(left_encode_diff)+abs(right_encode_diff))/2; + dth = count * g_angle_per_cnt_r; + dx = 0; + //ROS_INFO_STREAM("Left Rotation!! <0 >0"); + } + else if (right_encode_diff==0 || left_encode_diff==0) + { + //ROS_WARN_STREAM("Left or Right ==0"); + if (right_encode_diff>0) + { + count = right_encode_diff; + //ROS_INFO_STREAM("Right>0 Left==0"); + } + else if (left_encode_diff>0) + { + count = left_encode_diff; + //ROS_WARN_STREAM("Left>0 Right==0"); + } + else if (right_encode_diff<0) + { + count = right_encode_diff; + //ROS_INFO_STREAM("Right<0 Left==0"); + } + else if (left_encode_diff<0) + { + count = left_encode_diff; + //ROS_INFO_STREAM("Left<0 Right==0"); + } + else + count =0; + //ROS_INFO_STREAM("count:"< abs(right_encode_diff)) + { + //ROS_INFO_STREAM("Turn Right!!"); + ratio = double(left_encode_diff)/double(right_encode_diff); + concentric_radius = (g_wheel_distance) / (ratio-1); + dth = (abs(right_encode_diff) * g_distance_per_cnt)/concentric_radius; + dx = (concentric_radius+(g_wheel_distance/2))*sin(-dth); + } + + else if (abs(right_encode_diff) > abs(left_encode_diff)) + { + //ROS_INFO_STREAM("Turn Left!!"); + ratio = double(right_encode_diff)/double(left_encode_diff); + concentric_radius = (g_wheel_distance) / (ratio-1); + dth = -(abs(left_encode_diff) * g_distance_per_cnt)/concentric_radius; + dx = (concentric_radius+(g_wheel_distance/2))*sin(dth); + } + else if (left_encode_diff == right_encode_diff) + { + //ROS_INFO_STREAM("Backward!!"); + count = (abs(left_encode_diff) + abs(right_encode_diff))/2; + dth = 0; + dx = -count * g_distance_per_cnt; + } + } + + else if (right_encode_diff >0 && left_encode_diff >0) + { + if (left_encode_diff > right_encode_diff) + { + //ROS_INFO_STREAM("Turn Right!!"); + ratio = double(left_encode_diff)/double(right_encode_diff); + concentric_radius = (g_wheel_distance) / (ratio-1); + dth = -(right_encode_diff * g_distance_per_cnt)/concentric_radius; + dx = (concentric_radius+(g_wheel_distance/2))*sin(-dth); + } + else if (right_encode_diff > left_encode_diff) + { + //ROS_INFO_STREAM("Turn Left!!"); + ratio = double(right_encode_diff)/double(left_encode_diff); + concentric_radius = (g_wheel_distance) / (ratio-1); + dth = (left_encode_diff * g_distance_per_cnt)/concentric_radius; + dx = (concentric_radius+(g_wheel_distance/2))*sin(dth); + } + else if (left_encode_diff == right_encode_diff) + { + //ROS_INFO_STREAM("Foward!!"); + count = (left_encode_diff + right_encode_diff)/2; + dth = 0; + dx = count * g_distance_per_cnt; + } + } + + if (left_encode_diff != 0 || right_encode_diff != 0) + { + /*ROS_INFO_STREAM( "ROBOT MOVE (" << "left_diff = " << left_encode_diff << ", " + << "right_diff = " << right_encode_diff << ", " + << "dx = " << dx << ", " + << "dth = " << dth << ")");*/ + } +} + + +int main(int argc, char** argv){ + + ros::init(argc, argv, "odometry_publisher", ros::init_options::NoSigintHandler); + signal(SIGINT, sigint_handler); + + ros::NodeHandle n; + ros::Publisher odom_pub = n.advertise("odom", 50); + tf::TransformBroadcaster odom_broadcaster; + tf::TransformListener listener; + + ros::Subscriber cmd_vel_sub = n.subscribe("cmd_vel", 100, &cmd_vel_recv); + ros::Publisher encode_left_pub = n.advertise(LEFT_ENCODE_NAME, 1000); + ros::Publisher encode_right_pub = n.advertise(RIGHT_ENCODE_NAME, 1000); + + ros::Subscriber encode_left_sub = n.subscribe(LEFT_ENCODE_NAME, 1000, &encode_left_recv); + ros::Subscriber encode_right_sub = n.subscribe(RIGHT_ENCODE_NAME, 1000, &encode_right_recv); + + double total_x = 0.0; + + double x = 0.0; + double y = 0.0; + double th = 0.0; + + double dx = 0.0; + double dy = 0.0; + double dth = 0.0; + + double vx = 0.0; + double vy = 0.0; + double vth = 0.0; + + int32_t last_left_encode; + int32_t last_right_encode; + + int left_encode_diff = 0; + int right_encode_diff = 0; + + uint32_t tick = 0; + + ros::Time current_time, last_time; + current_time = ros::Time::now(); + last_time = ros::Time::now(); + + ros::Rate r(PUB_HZ); + + ros::NodeHandle nh_private("~"); + + /* Get parameters */ + nh_private.param("cnt_per_cycle", g_cnt_per_cycle, 714.0); + nh_private.param("wheel_radius", g_wheel_radius, 0.04); + nh_private.param("wheel_distance", g_wheel_distance, 0.29); + nh_private.param("enable_encode", g_enable_encode, false); + nh_private.param("enable_amcl", g_enable_amcl, false); + nh_private.param("pose_file_param", g_pose_file, std::string(ROBOT_POSE_FILE)); + + nh_private.param("frame_id_odom", g_frame_id_odom, std::string(g_enable_amcl ? "odom" : "map")); + nh_private.param("frame_id_base_link_tf", g_frame_id_base_link_tf, std::string("base_link")); + nh_private.param("frame_id_base_link_pub", g_frame_id_base_link_pub, std::string("base_link")); + + g_distance_per_cnt = ( ( 2 * g_wheel_radius * M_PI ) / g_cnt_per_cycle ); + g_angle_per_cnt_R = ( g_distance_per_cnt / g_wheel_distance ); + g_angle_per_cnt_r = ( g_distance_per_cnt / (g_wheel_distance/2) ); + + ROS_INFO_STREAM( "MY_ODOM parameter list:"); + ROS_INFO_STREAM( " - cnt_per_cycle = " << g_cnt_per_cycle); + ROS_INFO_STREAM( " - wheel_radius = " << g_wheel_radius); + ROS_INFO_STREAM( " - wheel_distance = " << g_wheel_distance); + ROS_INFO_STREAM( " - enable_encode = " << g_enable_encode); + ROS_INFO_STREAM( " - enable_amcl = " << g_enable_amcl); + + ROS_INFO_STREAM( " - frame_id_odom = " << g_frame_id_odom); + ROS_INFO_STREAM( " - frame_id_base_link_tf = " << g_frame_id_base_link_tf); + ROS_INFO_STREAM( " - frame_id_base_link_pub = " << g_frame_id_base_link_pub); + + //while (!ENCODE_IS_INIT()) { + // ros::Duration(2.0).sleep(); + // ROS_WARN_STREAM( "[MY_ODOM] Encode hasn't been updated yet...plz check topic " + // << RIGHT_ENCODE_NAME << " and " + // << LEFT_ENCODE_NAME); + // ros::spinOnce(); + //} + + ros::spinOnce(); + + last_left_encode = g_left_encode; + last_right_encode = g_right_encode; + + ROS_INFO_STREAM( "[MY_ODOM] Encode is recv!! MY_ODOM is ready to go."); + + while (n.ok()) + { + dx = 0.0; + dy = 0.0; + dth = 0.0; + + vx = 0.0; + vy = 0.0; + vth = 0.0; + + ros::spinOnce(); // check for incoming messages + + if (g_cmd_vel_x != 0 || g_cmd_vel_z != 0) { + //ROS_INFO_STREAM( "======================================================================"); + //ROS_INFO_STREAM( "[MY_ODOM] cmd_vel_x : " << g_cmd_vel_x << " cmd_vel_z : " << g_cmd_vel_z); + } + + if (g_enable_encode) { + + mimic_robot_encode(g_cmd_vel_x, g_cmd_vel_z, g_wheel_distance, g_distance_per_cnt, g_left_encode, g_right_encode, encode_left_pub, encode_right_pub); + + ros::spinOnce(); // check for incoming messages + + mtx_left.lock(); + left_encode_diff = g_left_encode - last_left_encode; + mtx_left.unlock(); + + mtx_right.lock(); + right_encode_diff = g_right_encode - last_right_encode; + mtx_right.unlock(); + + if (left_encode_diff != 0 || right_encode_diff != 0) { + //ROS_INFO_STREAM( "[MY_ODOM] encode_l : " << left_encode_diff << " encode_r : " << right_encode_diff); + } + + calculate_robot_move(left_encode_diff, right_encode_diff, dx, dth); + + if (dx != 0 || dth != 0) { + //ROS_INFO_STREAM( "[MY_ODOM] dx : " << dx << " dth : " << dth); + } + + } else { + + mimic_robot_move(g_cmd_vel_x, g_cmd_vel_z, dx, dth); + + } + + current_time = ros::Time::now(); + //compute odometry in a typical way given the velocities of the robot + double dt = (current_time - last_time).toSec(); + //double delta_x = (vx * cos(th) - vy * sin(th)) * dt; + //double delta_y = (vx * sin(th) + vy * cos(th)) * dt; + //double delta_th = vth * dt; + + vx = dx / dt; + vy = dy / dt; + vth = dth / dt; + + if (INVALID_VELOCITY(vx, vy, vth)) { + ROS_ERROR( "[MY_ODOM] INVALID VELOCITY = (%f, %f, %f) ENCODE = (%d, %d) INTERVAL = %f", + vx, vy, vth, left_encode_diff, right_encode_diff, dt); + dx = dy = dth = vx = vy = vth = 0.0; + } + + total_x += dx; + + double delta_x = (dx * cos(th) - dy * sin(th)); + double delta_y = (dx * sin(th) + dy * cos(th)); + double delta_th = dth; + + x += delta_x; + y += delta_y; + th += delta_th; + + //since all odometry is 6DOF we'll need a quaternion created from yaw + geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th); + + //first, we'll publish the transform over tf + geometry_msgs::TransformStamped odom_trans; + odom_trans.header.stamp = current_time; + odom_trans.header.frame_id = g_frame_id_odom; + odom_trans.child_frame_id = g_frame_id_base_link_tf; + + //if (x != 0 || y != 0 || th != 0) { + // ROS_INFO_STREAM("[MY_ODOM] tf_x = "<< x << ", tf_y = " << y << ", tf_th = " << th); + //} + + odom_trans.transform.translation.x = x; + odom_trans.transform.translation.y = y; + odom_trans.transform.translation.z = 0.0; + odom_trans.transform.rotation = odom_quat; + + //send the transform + odom_broadcaster.sendTransform(odom_trans); + + //next, we'll publish the odometry message over ROS + nav_msgs::Odometry odom; + odom.header.stamp = current_time; + odom.header.frame_id = g_frame_id_odom; + + //set the position + odom.pose.pose.position.x = x; + odom.pose.pose.position.y = y; + odom.pose.pose.position.z = 0.0; + odom.pose.pose.orientation = odom_quat; + + if (vx != 0 || vth != 0) { + //ROS_INFO_STREAM("[MY_ODOM] vx = "<< vx << ", vth = " << vth << ", total_x = " << total_x); + } + + //set the velocity + odom.child_frame_id = g_frame_id_base_link_pub; + odom.twist.twist.linear.x = vx; + odom.twist.twist.linear.y = vy; + odom.twist.twist.angular.z = vth; + + //publish the message + odom_pub.publish(odom); + + last_time = current_time; + + last_left_encode = g_left_encode; + last_right_encode = g_right_encode; + + /* Record robot pose in g_transform, which will be written to file before my_odom terminate */ + if ( listener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(0.01)) ) { + listener.lookupTransform("/map", "/base_link", ros::Time(0), g_transform); + } + + r.sleep(); + + /* Write position to file every 0.5 second */ + tick = (++tick)%10; + if ( (tick % 5) == 0) write_position(); + } +} diff --git a/DockerFiles/ROS1.0/etc/supervisor/conf.d/supervisord.conf b/DockerFiles/ROS1.0/etc/supervisor/conf.d/supervisord.conf new file mode 100644 index 0000000..8eacfad --- /dev/null +++ b/DockerFiles/ROS1.0/etc/supervisor/conf.d/supervisord.conf @@ -0,0 +1,37 @@ +[supervisord] +redirect_stderr=true +stopsignal=QUIT +autorestart=true +directory=/root + +[group:x] +programs=xvfb,wm,lxpanel,pcmanfm,x11vnc + +[program:wm] +priority=15 +command=/usr/bin/openbox +environment=DISPLAY=":1",HOME="/root",USER="root" + +[program:lxpanel] +priority=15 +directory=/home/ubuntu +command=/usr/bin/lxpanel --profile LXDE +user=ubuntu +environment=DISPLAY=":1",HOME="/home/ubuntu",USER="ubuntu" + +[program:pcmanfm] +priority=15 +directory=/home/ubuntu +command=/usr/bin/pcmanfm --desktop --profile LXDE +user=ubuntu +stopwaitsecs=3 +environment=DISPLAY=":1",HOME="/home/ubuntu",USER="ubuntu" + +[program:xvfb] +priority=10 +command=/usr/local/bin/xvfb.sh +stopsignal=KILL + +[program:x11vnc] +priority=20 +command=x11vnc -display :1 -xkb -forever -shared -repeat -capslock diff --git a/ros_home/.bashrc.default b/ros_home/.bashrc.default new file mode 100644 index 0000000..3fbf260 --- /dev/null +++ b/ros_home/.bashrc.default @@ -0,0 +1,228 @@ +# System-wide .bashrc file for interactive bash(1) shells. + +# To enable the settings / commands in this file for login shells as well, +# this file has to be sourced in /etc/profile. + +# If not running interactively, don't do anything +[ -z "$PS1" ] && return + +# check the window size after each command and, if necessary, +# update the values of LINES and COLUMNS. +shopt -s checkwinsize + +# set variable identifying the chroot you work in (used in the prompt below) +if [ -z "${debian_chroot:-}" ] && [ -r /etc/debian_chroot ]; then + debian_chroot=$(cat /etc/debian_chroot) +fi + +# set a fancy prompt (non-color, overwrite the one in /etc/profile) +PS1='${debian_chroot:+($debian_chroot)}\u@\h:\w\$ ' + +# Commented out, don't overwrite xterm -T "title" -n "icontitle" by default. +# If this is an xterm set the title to user@host:dir +#case "$TERM" in +#xterm*|rxvt*) +# PROMPT_COMMAND='echo -ne "\033]0;${USER}@${HOSTNAME}: ${PWD}\007"' +# ;; +#*) +# ;; +#esac + +# enable bash completion in interactive shells +#if ! shopt -oq posix; then +# if [ -f /usr/share/bash-completion/bash_completion ]; then +# . /usr/share/bash-completion/bash_completion +# elif [ -f /etc/bash_completion ]; then +# . /etc/bash_completion +# fi +#fi + +# sudo hint +if [ ! -e "$HOME/.sudo_as_admin_successful" ] && [ ! -e "$HOME/.hushlogin" ] ; then + case " $(groups) " in *\ admin\ *|*\ sudo\ *) + if [ -x /usr/bin/sudo ]; then + cat <<-EOF + To run a command as administrator (user "root"), use "sudo ". + See "man sudo_root" for details. + + EOF + fi + esac +fi + +# if the command-not-found package is installed, use it +if [ -x /usr/lib/command-not-found -o -x /usr/share/command-not-found/command-not-found ]; then + function command_not_found_handle { + # check because c-n-f could've been removed in the meantime + if [ -x /usr/lib/command-not-found ]; then + /usr/lib/command-not-found -- "$1" + return $? + elif [ -x /usr/share/command-not-found/command-not-found ]; then + /usr/share/command-not-found/command-not-found -- "$1" + return $? + else + printf "%s: command not found\n" "$1" >&2 + return 127 + fi + } +fi + +# ~/.bashrc: executed by bash(1) for non-login shells. +# see /usr/share/doc/bash/examples/startup-files (in the package bash-doc) +# for examples + +# If not running interactively, don't do anything +case $- in + *i*) ;; + *) return;; +esac + +# don't put duplicate lines or lines starting with space in the history. +# See bash(1) for more options +HISTCONTROL=ignoreboth + +# append to the history file, don't overwrite it +shopt -s histappend + +# for setting history length see HISTSIZE and HISTFILESIZE in bash(1) +HISTSIZE=1000 +HISTFILESIZE=2000 + +# check the window size after each command and, if necessary, +# update the values of LINES and COLUMNS. +shopt -s checkwinsize + +# If set, the pattern "**" used in a pathname expansion context will +# match all files and zero or more directories and subdirectories. +#shopt -s globstar + +# make less more friendly for non-text input files, see lesspipe(1) +[ -x /usr/bin/lesspipe ] && eval "$(SHELL=/bin/sh lesspipe)" + +# set variable identifying the chroot you work in (used in the prompt below) +if [ -z "${debian_chroot:-}" ] && [ -r /etc/debian_chroot ]; then + debian_chroot=$(cat /etc/debian_chroot) +fi + +# set a fancy prompt (non-color, unless we know we "want" color) +case "$TERM" in + xterm-color|*-256color) color_prompt=yes;; +esac + +# uncomment for a colored prompt, if the terminal has the capability; turned +# off by default to not distract the user: the focus in a terminal window +# should be on the output of commands, not on the prompt +#force_color_prompt=yes + +if [ -n "$force_color_prompt" ]; then + if [ -x /usr/bin/tput ] && tput setaf 1 >&/dev/null; then + # We have color support; assume it's compliant with Ecma-48 + # (ISO/IEC-6429). (Lack of such support is extremely rare, and such + # a case would tend to support setf rather than setaf.) + color_prompt=yes + else + color_prompt= + fi +fi + +if [ "$color_prompt" = yes ]; then + PS1='${debian_chroot:+($debian_chroot)}\[\033[01;32m\]\u@\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\$ ' +else + PS1='${debian_chroot:+($debian_chroot)}\u@\h:\w\$ ' +fi +unset color_prompt force_color_prompt + +# If this is an xterm set the title to user@host:dir +case "$TERM" in +xterm*|rxvt*) + PS1="\[\e]0;${debian_chroot:+($debian_chroot)}\u@\h: \w\a\]$PS1" + ;; +*) + ;; +esac + +# enable color support of ls and also add handy aliases +if [ -x /usr/bin/dircolors ]; then + test -r ~/.dircolors && eval "$(dircolors -b ~/.dircolors)" || eval "$(dircolors -b)" + alias ls='ls --color=auto' + #alias dir='dir --color=auto' + #alias vdir='vdir --color=auto' + + alias grep='grep --color=auto' + alias fgrep='fgrep --color=auto' + alias egrep='egrep --color=auto' +fi + +# colored GCC warnings and errors +#export GCC_COLORS='error=01;31:warning=01;35:note=01;36:caret=01;32:locus=01:quote=01' + +# some more ls aliases +alias ll='ls -alF' +alias la='ls -A' +alias l='ls -CF' + +# Add an "alert" alias for long running commands. Use like so: +# sleep 10; alert +alias alert='notify-send --urgency=low -i "$([ $? = 0 ] && echo terminal || echo error)" "$(history|tail -n1|sed -e '\''s/^\s*[0-9]\+\s*//;s/[;&|]\s*alert$//'\'')"' + +# enable programmable completion features (you don't need to enable +# this, if it's already enabled in /etc/bash.bashrc and /etc/profile +# sources /etc/bash.bashrc). +if ! shopt -oq posix; then + if [ -f /usr/share/bash-completion/bash_completion ]; then + . /usr/share/bash-completion/bash_completion + elif [ -f /etc/bash_completion ]; then + . /etc/bash_completion + fi +fi + +# My docker ENV default +export PATH=/usr/local/nvidia/bin/:/usr/local/nvidia/bin:/usr/local/cuda/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin + +## Only python2 contains detectron +if [ -d /opt/detectron ] ; then + PYTHONPATH="/opt/detectron:$PYTHONPATH" +else + alias python="python3" + alias pip="pip3" +fi + +alias rm="rm -i" +export CUDA_DEVICE_ORDER=PCI_BUS_ID + +# Alias definitions. +# You may want to put all your additions into a separate file like +# ~/.bash_aliases, instead of adding them here directly. +# See /usr/share/doc/bash-doc/examples in the bash-doc package. + +if [ -f ~/.bash_aliases ]; then + . ~/.bash_aliases +fi + +# if running bash +if [ -n "$BASH_VERSION" ]; then + # include .bashrc if it exists + if [ -f "$HOME/.bashrc" ]; then + . "$HOME/.bashrc" + fi +fi + +# set PATH so it includes user's private bin if it exists +if [ -d "$HOME/bin" ] ; then + PATH="$HOME/bin:$PATH" +fi + +# set PATH so it includes user's private bin if it exists +if [ -d "$HOME/.local/bin" ] ; then + PATH="$HOME/.local/bin:$PATH" +fi + +# Source ROS +source /catkin_ws/devel/setup.bash +source /chickenbot_ws/install/setup.bash +printf "[ROS] setup workspace with /catkin_ws/devel/setup.bash\n\n" + +export PYTHONPATH=$PYTHONPATH:/usr/local/lib + +# Go to the racecar_ws +cd /catkin_ws diff --git a/ros_home/.resolu.sh b/ros_home/.resolu.sh new file mode 100755 index 0000000..2aff69c --- /dev/null +++ b/ros_home/.resolu.sh @@ -0,0 +1 @@ +vnc4server -kill :0 && vnc4server :0 -geometry ${1:-1280x720} diff --git a/ros_home/.startup.sh b/ros_home/.startup.sh new file mode 100644 index 0000000..6755577 --- /dev/null +++ b/ros_home/.startup.sh @@ -0,0 +1,34 @@ +#!/bin/bash + +if [ -n "$VNC_PASSWORD" ]; then + su echo -n "$VNC_PASSWORD" > /.password1 + x11vnc -storepasswd $(cat /.password1) /.password2 + chmod 400 /.password* + sed -i 's/^command=x11vnc.*/& -rfbauth \/.password2/' /etc/supervisor/conf.d/supervisord.conf + export VNC_PASSWORD= +fi + +if [ -n "$X11VNC_ARGS" ]; then + sed -i "s/^command=x11vnc.*/& ${X11VNC_ARGS}/" /etc/supervisor/conf.d/supervisord.conf +fi + +if [ -n "$OPENBOX_ARGS" ]; then + sed -i "s#^command=/usr/bin/openbox.*#& ${OPENBOX_ARGS}#" /etc/supervisor/conf.d/supervisord.conf +fi + +if [ -n "$RESOLUTION" ]; then + sed -i "s/1600x900/$RESOLUTION/" /usr/local/bin/xvfb.sh +fi + +# home folder +if [ ! -x "$HOME/.config/pcmanfm/LXDE/" ]; then + mkdir -p $HOME/.config/pcmanfm/LXDE/ + ln -sf /usr/local/share/doro-lxde-wallpapers/desktop-items-0.conf $HOME/.config/pcmanfm/LXDE/ + chown -R $USER:$USER $HOME +fi + +# clearup +PASSWORD= +HTTP_PASSWORD= + +exec /bin/tini -- supervisord -n -c /etc/supervisor/supervisord.conf diff --git a/ros_home/.vimrc b/ros_home/.vimrc new file mode 100644 index 0000000..203e5b3 --- /dev/null +++ b/ros_home/.vimrc @@ -0,0 +1,10 @@ +set number +syntax on +set background=dark +set mouse=a +set tabstop=2 +set shiftwidth=2 +set spell spelllang=en_us +set expandtab +set belloff=all +filetype indent on diff --git a/ros_home/.vnc/xstartup b/ros_home/.vnc/xstartup new file mode 100755 index 0000000..0a1e41e --- /dev/null +++ b/ros_home/.vnc/xstartup @@ -0,0 +1,18 @@ +#!/bin/sh + +# Uncomment the following two lines for normal desktop: +# unset SESSION_MANAGER +# exec /etc/X11/xinit/xinitrc + +[ -x /etc/vnc/xstartup ] && exec /etc/vnc/xstartup +[ -r $HOME/.Xresources ] && xrdb $HOME/.Xresources +# vncconfig -iconic & +# x-terminal-emulator -geometry 80x24+10+10 -ls -title "$VNCDESKTOP Desktop" & +# x-window-manager & + +vncconfig nowin=1 & +xsetroot -solid black +XDG_CURRENT_DESKTOP=GNOME:ubuntu gnome-panel & +pcmanfm & +metacity & +gnome-terminal & diff --git a/ros_home/Xresources b/ros_home/Xresources new file mode 100644 index 0000000..87c9bfc --- /dev/null +++ b/ros_home/Xresources @@ -0,0 +1,27 @@ +*xterm*background: #231f20 +*xterm*foreground: #d0d0d0 +*xterm*cursorColor: #d0d0d0 +*xterm*color0: #231f20 +*xterm*color1: #960050 +*xterm*color2: #66aa11 +*xterm*color3: #c47f2c +*xterm*color4: #30309b +*xterm*color5: #7e40a5 +*xterm*color6: #3579a8 +*xterm*color7: #9999aa +*xterm*color8: #303030 +*xterm*color9: #a41f35 +*xterm*color10: #80ff00 +*xterm*color11: #ffba68 +*xterm*color12: #5f5fee +*xterm*color13: #bb88dd +*xterm*color14: #4eb4fa +*xterm*color15: #d0d0d0 +*xterm*faceName: Monaco:size=13:antialias=true +*xterm*scrollBar: false + +!Paste-------------- +xterm*VT100.Translations: #override\ + Shift CtrlV: insert-selection(CLIPBOARD) \n\ + Shift CtrlC: copy-selection(CLIPBOARD) +!------------------- diff --git a/ros_home/default.rviz b/ros_home/default.rviz new file mode 100644 index 0000000..171930d --- /dev/null +++ b/ros_home/default.rviz @@ -0,0 +1,212 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 365 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + back_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_hinge: + Alpha: 1 + Show Axes: false + Show Trail: false + front_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_hinge: + Alpha: 1 + Show Axes: false + Show Trail: false + front_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_model: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 19.52252197265625 + Min Color: 0; 0; 0 + Min Intensity: 6.002182483673096 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.15000000596046448 + Style: Flat Squares + Topic: /scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 15.024418830871582 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 700 + Hide Left Dock: true + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000015600000252fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003c00000252000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003c00000252000000a100fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004000000003efc0100000002fb0000000800540069006d00650100000000000004000000025900fffffffb0000000800540069006d00650100000000000004500000000000000000000004000000025200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 700 + X: 0 + Y: 20