diff --git a/.gitmodules b/.gitmodules index 5e04f6aad..dfc8bf112 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/vdb_mapping_ros2"] path = robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2 url = git@github.com:castacks/vdb_mapping_ros2.git +[submodule "ground_control_station/ros_ws/src/ros-gst-bridge"] + path = ground_control_station/ros_ws/src/ros-gst-bridge + url = https://github.com/BrettRD/ros-gst-bridge.git diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/.gitignore b/common/ros_packages/behavior_tree_msgs/.gitignore similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/.gitignore rename to common/ros_packages/behavior_tree_msgs/.gitignore diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/CMakeLists.txt b/common/ros_packages/behavior_tree_msgs/CMakeLists.txt similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/CMakeLists.txt rename to common/ros_packages/behavior_tree_msgs/CMakeLists.txt diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/Active.msg b/common/ros_packages/behavior_tree_msgs/msg/Active.msg similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/Active.msg rename to common/ros_packages/behavior_tree_msgs/msg/Active.msg diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/BehaviorTreeCommand.msg b/common/ros_packages/behavior_tree_msgs/msg/BehaviorTreeCommand.msg similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/BehaviorTreeCommand.msg rename to common/ros_packages/behavior_tree_msgs/msg/BehaviorTreeCommand.msg diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/BehaviorTreeCommands.msg b/common/ros_packages/behavior_tree_msgs/msg/BehaviorTreeCommands.msg similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/BehaviorTreeCommands.msg rename to common/ros_packages/behavior_tree_msgs/msg/BehaviorTreeCommands.msg diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/Status.msg b/common/ros_packages/behavior_tree_msgs/msg/Status.msg similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/Status.msg rename to common/ros_packages/behavior_tree_msgs/msg/Status.msg diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/package.xml b/common/ros_packages/behavior_tree_msgs/package.xml similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/package.xml rename to common/ros_packages/behavior_tree_msgs/package.xml diff --git a/docker-compose.yaml b/docker-compose.yaml index 6638e7db4..5be0350ed 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -2,7 +2,7 @@ include: - simulation/isaac-sim/docker/docker-compose.yaml - robot/docker/docker-compose.yaml - # - ground_control_station/docker/docker-compose.yaml + - ground_control_station/docker/docker-compose.yaml services: docs: # live mkdocs container. open localhost:8000 in browser to see the docs diff --git a/ground_control_station/docker/.bashrc b/ground_control_station/docker/.bashrc index 55ff96494..061bc06bf 100644 --- a/ground_control_station/docker/.bashrc +++ b/ground_control_station/docker/.bashrc @@ -156,4 +156,6 @@ function cws(){ source /opt/ros/humble/setup.bash sws # source the ROS2 workspace by default -export RCUTILS_COLORIZED_OUTPUT=1 \ No newline at end of file +export RCUTILS_COLORIZED_OUTPUT=1 + +export ROS_DOMAIN_ID=0 diff --git a/ground_control_station/docker/Dockerfile.gcs b/ground_control_station/docker/Dockerfile.gcs new file mode 100644 index 000000000..eb812ee7c --- /dev/null +++ b/ground_control_station/docker/Dockerfile.gcs @@ -0,0 +1,91 @@ +FROM osrf/ros:humble-desktop-full + +WORKDIR /root/ros_ws + +RUN apt update +# Install dev tools +RUN apt install -y \ + vim nano emacs wget curl tree \ + iperf3 iftop iputils-ping net-tools htop \ + cmake build-essential \ + less htop jq \ + python3-pip \ + tmux \ + gdb + +# Install any additional ROS2 packages +RUN apt update -y && apt install -y \ + ros-dev-tools \ + ros-humble-mavros \ + ros-humble-tf2* \ + ros-humble-stereo-image-proc \ + ros-humble-image-view \ + ros-humble-topic-tools \ + ros-humble-grid-map \ + ros-humble-domain-bridge \ + ros-humble-ros2cli \ + python3-colcon-common-extensions \ + libglib2.0-dev \ + libcgal-dev \ + mosquitto \ + mosquitto-clients + +# install gstreamer +RUN apt-get update && apt-get install -y \ + libgstreamer1.0-dev \ + libgstreamer-plugins-base1.0-dev \ + libgstreamer-plugins-bad1.0-dev \ + gstreamer1.0-plugins-base \ + gstreamer1.0-plugins-good \ + gstreamer1.0-plugins-bad \ + gstreamer1.0-plugins-ugly \ + gstreamer1.0-libav \ + gstreamer1.0-tools \ + gstreamer1.0-x \ + gstreamer1.0-alsa + +RUN /opt/ros/humble/lib/mavros/install_geographiclib_datasets.sh + + +# Install Python dependencies +RUN pip3 install \ + empy \ + future \ + lxml \ + matplotlib \ + numpy \ + pkgconfig \ + psutil \ + pygments \ + wheel \ + pymavlink \ + pyyaml \ + requests \ + setuptools \ + six \ + toml \ + scipy \ + pytak \ + paho-mqtt + + +# Add ability to SSH +RUN apt-get update && apt-get install -y openssh-server +RUN mkdir /var/run/sshd + +# Password is airstack +RUN echo 'root:airstack' | chpasswd +RUN sed -i 's/#PermitRootLogin prohibit-password/PermitRootLogin yes/' /etc/ssh/sshd_config +RUN sed -i 's/#PasswordAuthentication yes/PasswordAuthentication yes/' /etc/ssh/sshd_config +RUN sed 's@session\s*required\s*pam_loginuid.so@session optional pam_loginuid.so@g' -i /etc/pam.d/sshd + +EXPOSE 22 + + + +# Cleanup. Prevent people accidentally doing git commits as root in Docker +RUN apt purge git -y \ + && apt autoremove -y \ + && apt clean -y \ + && rm -rf /var/lib/apt/lists/* + diff --git a/ground_control_station/docker/docker-compose.yaml b/ground_control_station/docker/docker-compose.yaml index 1416b82cd..c6c0d1331 100644 --- a/ground_control_station/docker/docker-compose.yaml +++ b/ground_control_station/docker/docker-compose.yaml @@ -1,6 +1,11 @@ services: ground-control-station: - image: TODO + image: &gcs airlab-storage.andrew.cmu.edu:5001/shared/ground-control-station_ros-humble:v1.0.0 + build: + context: ../ + dockerfile: docker/Dockerfile.gcs + tags: + - *gcs container_name: ground-control-station entrypoint: "" command: > @@ -12,7 +17,7 @@ services: stdin_open: true # docker run -i tty: true # docker run -t # Needed to display graphical applications - ipc: host + # ipc: host privileged: true networks: - airstack_network @@ -28,171 +33,17 @@ services: count: 1 capabilities: [gpu] ports: - # for ssh - - 2222:22 + - 2222:22 # for ssh + - 1883:1883 # for mosquitto + - 9001:9001 # for mosquitto volumes: # display stuff - $HOME/.Xauthority:/root/.Xauthority - /tmp/.X11-unix:/tmp/.X11-unix # developer stuff - .bashrc:/root/.bashrc:rw # bash config - - .bash_history:/root/.bash_history:rw # save cmdline history - /var/run/docker.sock:/var/run/docker.sock # access docker API for container name # autonomy stack stuff - - ../../common/ros_ws:/root/common/ros_ws:rw # common ROS packages + - ../../common/ros_packages:/root/ros_ws/src/common:rw # common ROS packages - ../ros_ws:/root/ros_ws:rw # gcs-specific ROS packages - - -####################### GSTREAMER TO ROS TOPICS ###################### - gst-ros-bridge-topic1: - container_name: "${PROJECT_NAME}-gst_ros_bridge1" - image: "${PROJECT_NAME}/gcs/gst-ros-bridge" - build: - context: . - dockerfile: Dockerfile.gst-ros-bridge - command: > - /bin/bash -c 'source /ros_ws/install/setup.bash && gst-launch-1.0 --gst-plugin-path=/ros_ws/install/gst_bridge/lib/gst_bridge/ - rtspsrc location="${CAMERA1_STREAM_IP}" latency=0 ! - rtph265depay ! h265parse ! avdec_h265 ! videoconvert ! - rosimagesink ros-topic="${CAMERA1_ROS_TOPIC}"' - environment: - - DISPLAY=${DISPLAY} - - DOCKER_BUILDKIT=0 - - CAMERA1_STREAM_IP=${CAMERA1_STREAM_IP} - - CAMERA1_ROS_TOPIC=${CAMERA1_ROS_TOPIC} - volumes: - - /tmp/.X11-unix:/tmp/.X11-unix - network_mode: host - - -####################### ROS2TAK TOOLS ###################### - ############### MQTT for the GCS - mqtt: - container_name: "mqtt" - image: eclipse-mosquitto:2.0.20 - restart: always - volumes: - - ../ros_ws/src/ros2tak_tools/mosquitto/config:/mosquitto/config - - ../ros_ws/src/ros2tak_tools/mosquitto/data:/mosquitto/data - - ../ros_ws/src/ros2tak_tools/mosquitto/log:/mosquitto/log - env_file: - - .env - ports: - - "1883:1883" - - "9001:9001" - healthcheck: - test: [ "CMD", "mosquitto_pub", "-h", "localhost", "-t", "healthcheck", "-m", "ping", "-u", "${MQTT_USERNAME}", "-P", "${MQTT_PASSWORD}" ] - interval: 5s - timeout: 3s - retries: 2 - start_period: 5s - networks: - - airstack_network - - ################## ROS2COT_AGENT - ros2cot_agent: - build: - context: ../ - dockerfile: docker/Dockerfile.ros2cot_agent - args: - - ROS_WS_DIR=${ROS_WS_DIR} - image: "${PROJECT_NAME}/gcs/ros2cot_agent" - container_name: "${PROJECT_NAME}-ros2cot_agent" - stdin_open: true - tty: true - restart: unless-stopped - depends_on: - mqtt: - condition: service_healthy - networks: - - airstack_network - command: [ - "/bin/bash", - "-c", - "source /opt/ros/humble/setup.bash && \ - source $ROS_WS_DIR/install/setup.bash && \ - ./install/ros2tak_tools/bin/ros2cot_agent --config $ROS_WS_DIR/$ROS2TAK_TOOLS_CONFIG_DIR/$ROS2TAK_TOOLS_CONFIG_FILENAME" - ] - - # ################### TAK_PUBLISHER - tak_publisher: - build: - context: ../ - dockerfile: docker/Dockerfile.tak_publisher - args: - - ROS_WS_DIR=${ROS_WS_DIR} - image: "${PROJECT_NAME}/gcs/tak_publisher" - container_name: "${PROJECT_NAME}-tak_publisher" - stdin_open: true - tty: true - restart: unless-stopped - depends_on: - mqtt: - condition: service_healthy - networks: - - airstack_network - volumes: - - ../ros_ws/src/ros2tak_tools/:${ROS_WS_DIR}/src/ros2tak_tools/ - command: [ - "python3", - "$TAK_PUBLISHER_FILEPATH", - "--config", - "$ROS2TAK_TOOLS_CONFIG_DIR/$ROS2TAK_TOOLS_CONFIG_FILENAME" - ] - - ################### TAK_SUBSCRIBER - tak_subscriber: - build: - context: ../ - dockerfile: docker/Dockerfile.tak_subscriber - args: - - ROS_WS_DIR=${ROS_WS_DIR} - image: "${PROJECT_NAME}/gcs/tak_subscriber" - container_name: "${PROJECT_NAME}-tak_subscriber" - stdin_open: true - tty: true - restart: unless-stopped - depends_on: - mqtt: - condition: service_healthy - networks: - - airstack_network - volumes: - - ../ros_ws/src/ros2tak_tools/:${ROS_WS_DIR}/src/ros2tak_tools/ - command: [ - "python3", - "$TAK_SUBSCRIBER_FILEPATH", - "--config", - "$ROS2TAK_TOOLS_CONFIG_DIR/$ROS2TAK_TOOLS_CONFIG_FILENAME" - ] - - - ################## ROS2COT_AGENT - cot2planner_agent: - build: - context: ../../ - dockerfile: ground_control_station/docker/Dockerfile.cot2planner_agent - args: - - ROS_WS_DIR=${ROS_WS_DIR} - image: "${PROJECT_NAME}/gcs/cot2planner_agent" - container_name: "${PROJECT_NAME}-cot2planner_agent" - stdin_open: true - tty: true - restart: unless-stopped - depends_on: - mqtt: - condition: service_healthy - networks: - - airstack_network - command: [ - "/bin/bash", - "-c", - "source /opt/ros/humble/setup.bash && \ - source $ROS_WS_DIR/install/setup.bash && \ - ./install/ros2tak_tools/bin/cot2planner_agent --config $ROS_WS_DIR/$ROS2TAK_TOOLS_CONFIG_DIR/$ROS2TAK_TOOLS_CONFIG_FILENAME" - ] - -########### NETWORKS ########### -networks: - airstack_network: - driver: bridge + - ../../common/ros_packages/fastdds.xml:/root/ros_ws/fastdds.xml:rw # fastdds.xml diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/resource/rqt_py_template b/ground_control_station/ros_ws/fastdds.xml similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/resource/rqt_py_template rename to ground_control_station/ros_ws/fastdds.xml diff --git a/ground_control_station/ros_ws/src/gcs_bringup/CMakeLists.txt b/ground_control_station/ros_ws/src/gcs_bringup/CMakeLists.txt new file mode 100644 index 000000000..6fe25501a --- /dev/null +++ b/ground_control_station/ros_ws/src/gcs_bringup/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.8) +project(gcs_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +#install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) + +ament_package() + diff --git a/ground_control_station/ros_ws/src/gcs_bringup/config/domain_bridge.yaml b/ground_control_station/ros_ws/src/gcs_bringup/config/domain_bridge.yaml new file mode 100644 index 000000000..df5b71755 --- /dev/null +++ b/ground_control_station/ros_ws/src/gcs_bringup/config/domain_bridge.yaml @@ -0,0 +1,11 @@ +name: my_bridge +from_domain: 0 +to_domain: 1 +topics: + /robot_1/behavior/behavior_tree_commands: + type: behavior_tree_msgs/msg/BehaviorTreeCommands + /robot_1/fixed_trajectory_generator/fixed_trajectory_command: + type: airstack_msgs/msg/FixedTrajectory + /robot_1/behavior/behavior_tree_graphviz: + type: std_msgs/msg/String + reversed: True \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/gcs_bringup/config/gcs.perspective b/ground_control_station/ros_ws/src/gcs_bringup/config/gcs.perspective new file mode 100644 index 000000000..d171a4005 --- /dev/null +++ b/ground_control_station/ros_ws/src/gcs_bringup/config/gcs.perspective @@ -0,0 +1,75 @@ +{ + "keys": {}, + "groups": { + "mainwindow": { + "keys": { + "geometry": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb000300000000006400000064000005ea0000037b0000006400000089000005ea0000037b00000000000000000a000000006400000089000005ea0000037b')", + "type": "repr(QByteArray.hex)", + "pretty-print": " d d { d { d {" + }, + "state": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd000000010000000300000587000002c9fc0100000001fb0000006a007200710074005f00670072006f0075006e0064005f0063006f006e00740072006f006c005f00730074006100740069006f006e005f005f00470072006f0075006e00640043006f006e00740072006f006c00530074006100740069006f006e005f005f0031005f005f010000000000000587000002cd00ffffff000005870000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", + "type": "repr(QByteArray.hex)", + "pretty-print": " jrqt_ground_control_station__GroundControlStation__1__ 6MinimizedDockWidgetsToolbar " + } + }, + "groups": { + "toolbar_areas": { + "keys": { + "MinimizedDockWidgetsToolbar": { + "repr": "8", + "type": "repr" + } + }, + "groups": {} + } + } + }, + "pluginmanager": { + "keys": { + "running-plugins": { + "repr": "{'rqt_ground_control_station/GroundControlStation': [1]}", + "type": "repr" + } + }, + "groups": { + "plugin__rqt_ground_control_station__GroundControlStation__1": { + "keys": {}, + "groups": { + "dock_widget__": { + "keys": { + "dock_widget_title": { + "repr": "''", + "type": "repr" + }, + "dockable": { + "repr": "True", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "command_config_filename": { + "repr": "'/root/ros_ws/install/rqt_ground_control_station/share/rqt_ground_control_station/config/gui_config.yaml'", + "type": "repr" + }, + "trajectory_config_filename": { + "repr": "'/root/ros_ws/install/rqt_ground_control_station/share/rqt_ground_control_station/config/fixed_trajectories.yaml'", + "type": "repr" + } + }, + "groups": {} + } + } + } + } + } + } +} \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/gcs_bringup/launch/#gst2ros.launch# b/ground_control_station/ros_ws/src/gcs_bringup/launch/#gst2ros.launch# new file mode 100644 index 000000000..1f81f8fea --- /dev/null +++ b/ground_control_station/ros_ws/src/gcs_bringup/launch/#gst2ros.launch# @@ -0,0 +1 @@ + \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/gcs_bringup/launch/gcs.launch.xml b/ground_control_station/ros_ws/src/gcs_bringup/launch/gcs.launch.xml new file mode 100644 index 000000000..a10e3ce84 --- /dev/null +++ b/ground_control_station/ros_ws/src/gcs_bringup/launch/gcs.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + diff --git a/ground_control_station/ros_ws/src/gcs_bringup/launch/gst2ros.launch.xml b/ground_control_station/ros_ws/src/gcs_bringup/launch/gst2ros.launch.xml new file mode 100644 index 000000000..02f8c72bd --- /dev/null +++ b/ground_control_station/ros_ws/src/gcs_bringup/launch/gst2ros.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/ground_control_station/ros_ws/src/gcs_bringup/launch/tak.launch.xml b/ground_control_station/ros_ws/src/gcs_bringup/launch/tak.launch.xml new file mode 100644 index 000000000..139fff7db --- /dev/null +++ b/ground_control_station/ros_ws/src/gcs_bringup/launch/tak.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + diff --git a/ground_control_station/ros_ws/src/gcs_bringup/package.xml b/ground_control_station/ros_ws/src/gcs_bringup/package.xml new file mode 100644 index 000000000..345832b21 --- /dev/null +++ b/ground_control_station/ros_ws/src/gcs_bringup/package.xml @@ -0,0 +1,18 @@ + + + + gcs_bringup + 0.0.0 + TODO: Package description + andrew + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ground_control_station/ros_ws/src/gcs_bringup/rviz/gcs.rviz b/ground_control_station/ros_ws/src/gcs_bringup/rviz/gcs.rviz new file mode 100644 index 000000000..db830a45d --- /dev/null +++ b/ground_control_station/ros_ws/src/gcs_bringup/rviz/gcs.rviz @@ -0,0 +1,133 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 555 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/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 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 10 + 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: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 72 + Y: 60 diff --git a/ground_control_station/ros_ws/src/ros-gst-bridge b/ground_control_station/ros_ws/src/ros-gst-bridge new file mode 160000 index 000000000..23980326c --- /dev/null +++ b/ground_control_station/ros_ws/src/ros-gst-bridge @@ -0,0 +1 @@ +Subproject commit 23980326ce8c0fefc0d5d590c2bfc9d308f35a73 diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/config/config.yaml b/ground_control_station/ros_ws/src/ros2tak_tools/config/config.yaml index 805502693..c347cba60 100644 --- a/ground_control_station/ros_ws/src/ros2tak_tools/config/config.yaml +++ b/ground_control_station/ros_ws/src/ros2tak_tools/config/config.yaml @@ -43,10 +43,10 @@ tak_server: mqtt: - host: mqtt # TODO: This should be set as container name for MQTT Docker + host: localhost port: 1883 username: airlab - password: # Enter your password here + password: services: host: '127.0.0.1' # Host settings can be specified here (e.g., localhost or specific IP address). diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/cot2planner_agent.py b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/cot2planner_agent.py index a90b50ccc..7a199acbd 100644 --- a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/cot2planner_agent.py +++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/cot2planner_agent.py @@ -248,7 +248,7 @@ def main(args=None): import argparse parser = argparse.ArgumentParser(description="COT to Planner") parser.add_argument('--config', type=str, required=True, help='Path to the config YAML file.') - args = parser.parse_args() + args, unknown = parser.parse_known_args() cot2planner = Cot2Planner(args.config) rclpy.spin(cot2planner) diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/ros2cot_agent.py b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/ros2cot_agent.py index 8eea64479..15883207d 100755 --- a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/ros2cot_agent.py +++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/ros2cot_agent.py @@ -192,8 +192,8 @@ def main(args=None): ) # Parse the arguments - input_args = parser.parse_args() - + input_args, unknown = parser.parse_known_args() + # Load configuration config = load_config(input_args.config) diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_publisher.py b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/tak_publisher.py similarity index 97% rename from ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_publisher.py rename to ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/tak_publisher.py index bf9a74e5a..734405a1c 100755 --- a/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_publisher.py +++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/tak_publisher.py @@ -80,7 +80,7 @@ async def run(self, number_of_iterations=-1): self.mqtt_client.loop_stop() print("MQTT loop stopped.") -async def main(config): +async def async_main(config): loop = asyncio.get_running_loop() # Capture the main event loop clitool = pytak.CLITool(config["mycottool"]) await clitool.setup() @@ -92,9 +92,12 @@ def run_main_in_process(config): loop.run_until_complete(main(config)) if __name__ == "__main__": + main() + +def main(): parser = argparse.ArgumentParser(description="TAK Publisher Script") parser.add_argument('--config', type=str, required=True, help='Path to the config YAML file.') - args = parser.parse_args() + args, unknown = parser.parse_known_args() # Load the YAML configuration with open(args.config, 'r') as file: @@ -140,4 +143,4 @@ def run_main_in_process(config): process.start() print("Main() is now running in a separate process.") - process.join() \ No newline at end of file + process.join() diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_subscriber.py b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/tak_subscriber.py similarity index 98% rename from ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_subscriber.py rename to ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/tak_subscriber.py index 004e60ad7..7ccab2efe 100644 --- a/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_subscriber.py +++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/tak_subscriber.py @@ -143,10 +143,10 @@ async def run(self): # pylint: disable=arguments-differ await self.handle_data(data) -async def main(): +async def async_main(): parser = argparse.ArgumentParser(description="TAK Subscriber Script") parser.add_argument('--config', type=str, required=True, help='Path to the config YAML file.') - args = parser.parse_args() + args, unknown = parser.parse_known_args() # Load the YAML configuration with open(args.config, 'r') as file: @@ -197,6 +197,8 @@ async def main(): # Start all tasks. await clitool.run() +def main(): + asyncio.run(async_main()) if __name__ == "__main__": - asyncio.run(main()) + main() diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/setup.cfg b/ground_control_station/ros_ws/src/ros2tak_tools/setup.cfg new file mode 100644 index 000000000..23d12fb81 --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/ros2tak_tools +[install] +install_scripts=$base/lib/ros2tak_tools \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/setup.py b/ground_control_station/ros_ws/src/ros2tak_tools/setup.py index ba6900a21..c675e6975 100644 --- a/ground_control_station/ros_ws/src/ros2tak_tools/setup.py +++ b/ground_control_station/ros_ws/src/ros2tak_tools/setup.py @@ -1,4 +1,5 @@ from setuptools import find_packages, setup +import glob package_name = 'ros2tak_tools' @@ -10,6 +11,7 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/config/', glob.glob('config/*')), ], install_requires=['setuptools'], zip_safe=True, @@ -23,6 +25,8 @@ 'ros2cot_agent = ros2tak_tools.ros2cot_agent:main', 'cot2ros_agent = ros2tak_tools.cot2ros_agent:main', 'cot2planner_agent = ros2tak_tools.cot2planner_agent:main', + 'tak_subscriber = ros2tak_tools.tak_subscriber:main', + 'tak_publisher = ros2tak_tools.tak_publisher:main', ], }, ) diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/CHANGELOG.rst b/ground_control_station/ros_ws/src/rqt_ground_control_station/CHANGELOG.rst similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/CHANGELOG.rst rename to ground_control_station/ros_ws/src/rqt_ground_control_station/CHANGELOG.rst diff --git a/ground_control_station/ros_ws/src/rqt_ground_control_station/README.md b/ground_control_station/ros_ws/src/rqt_ground_control_station/README.md new file mode 100644 index 000000000..464d515b9 --- /dev/null +++ b/ground_control_station/ros_ws/src/rqt_ground_control_station/README.md @@ -0,0 +1,19 @@ +# RQT Python GroundControlStation + +If you `colcon build` this package in a workspace and then run "rqt --force-discover" after sourcing the workspace, the plugin should show up as "Ground Control Station" in "Miscellaneous Tools" in the "Plugins" menu. + +You can use the `generate_rqt_py_package.sh` script to generate a new package by doing the following from the rqt_ground_control_station directory + +``` +./generate_rqt_py_package.sh [package name] [class name] [plugin title] +``` + +[package name] will be the name of the package and a directory with this name will be created above `rqt_ground_control_station/`. [class name] is the name of the class in `src/[package name]/template.py`. [plugin title] is what the plugin will be called in the "Miscellaneous Tools" menu. + +For example, + +``` +cd rqt_ground_control_station/ +./generate_rqt_py_package.sh new_rqt_package ClassName "Plugin Title" +``` + diff --git a/ground_control_station/ros_ws/src/rqt_ground_control_station/config/fixed_trajectories.yaml b/ground_control_station/ros_ws/src/rqt_ground_control_station/config/fixed_trajectories.yaml new file mode 100644 index 000000000..64c1f54de --- /dev/null +++ b/ground_control_station/ros_ws/src/rqt_ground_control_station/config/fixed_trajectories.yaml @@ -0,0 +1,38 @@ +trajectories: + - Figure8: + attributes: + - frame_id + - velocity + - max_acceleration + - length + - width + - height + - Racetrack: + attributes: + - frame_id + - velocity + - max_acceleration + - length + - width + - height + - Circle: + attributes: + - frame_id + - velocity + - radius + - Line: + attributes: + - frame_id + - velocity + - max_acceleration + - length + - width + - height + - Point: + attributes: + - frame_id + - velocity + - max_acceleration + - x + - y + - height diff --git a/ground_control_station/ros_ws/src/rqt_ground_control_station/config/gui_config.yaml b/ground_control_station/ros_ws/src/rqt_ground_control_station/config/gui_config.yaml new file mode 100644 index 000000000..0ee314d5e --- /dev/null +++ b/ground_control_station/ros_ws/src/rqt_ground_control_station/config/gui_config.yaml @@ -0,0 +1,20 @@ +groups: + - commands: + - Arm and Takeoff: + condition_name: Auto Takeoff Commanded + - Fixed Trajectory: + condition_name: Fixed Trajectory Commanded + - Global Plan: + condition_name: Global Plan Commanded + - Pause: + condition_name: Pause Commanded + - Rewind: + condition_name: Rewind Commanded + - Disarm: + condition_name: Disarm Commanded + - Land: + condition_name: Land Commanded +robots: # these should match a robot's namespace + - robot_1 + - robot_2 + - robot_3 \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/rqt_ground_control_station/package.xml b/ground_control_station/ros_ws/src/rqt_ground_control_station/package.xml new file mode 100644 index 000000000..a34e35dac --- /dev/null +++ b/ground_control_station/ros_ws/src/rqt_ground_control_station/package.xml @@ -0,0 +1,28 @@ + + rqt_ground_control_station + 1.0.2 + rqt_ground_control_station is a Python GUI template. + John Keller + + BSD + + + + + + John Keller + + ament_index_python + python_qt_binding + qt_gui + qt_gui_py_common + rclpy + rqt_gui + rqt_gui_py + + + + + ament_python + + diff --git a/ground_control_station/ros_ws/src/rqt_ground_control_station/plugin.xml b/ground_control_station/ros_ws/src/rqt_ground_control_station/plugin.xml new file mode 100644 index 000000000..ed23bc0c3 --- /dev/null +++ b/ground_control_station/ros_ws/src/rqt_ground_control_station/plugin.xml @@ -0,0 +1,17 @@ + + + + A Python GUI plugin providing an interactive Python console. + + + + + folder + Plugins related to miscellaneous tools. + + + applications-python + A Python RQT GUI template. + + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/resource/py_console_widget.ui b/ground_control_station/ros_ws/src/rqt_ground_control_station/resource/py_console_widget.ui similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/resource/py_console_widget.ui rename to ground_control_station/ros_ws/src/rqt_ground_control_station/resource/py_console_widget.ui diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/__init__.py b/ground_control_station/ros_ws/src/rqt_ground_control_station/resource/rqt_ground_control_station similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/__init__.py rename to ground_control_station/ros_ws/src/rqt_ground_control_station/resource/rqt_ground_control_station diff --git a/ground_control_station/ros_ws/src/rqt_ground_control_station/setup.cfg b/ground_control_station/ros_ws/src/rqt_ground_control_station/setup.cfg new file mode 100644 index 000000000..c3f0a9b42 --- /dev/null +++ b/ground_control_station/ros_ws/src/rqt_ground_control_station/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rqt_ground_control_station +[install] +install_scripts=$base/lib/rqt_ground_control_station diff --git a/ground_control_station/ros_ws/src/rqt_ground_control_station/setup.py b/ground_control_station/ros_ws/src/rqt_ground_control_station/setup.py new file mode 100644 index 000000000..abc326378 --- /dev/null +++ b/ground_control_station/ros_ws/src/rqt_ground_control_station/setup.py @@ -0,0 +1,41 @@ +from setuptools import setup + +package_name = 'rqt_ground_control_station' +import glob + +setup( + name=package_name, + version='1.0.2', + packages=[package_name], + package_dir={'': 'src'}, + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name + '/resource', + ['resource/py_console_widget.ui']), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name, ['plugin.xml']), + ('share/' + package_name + '/config/', glob.glob('config/*')), + ], + install_requires=['setuptools'], + zip_safe=True, + author='', + maintainer='', + maintainer_email='', + keywords=['ROS'], + classifiers=[ + '', + '', + '', + '', + ], + description=( + 'rqt_ground_control_station' + ), + license='BSD', + entry_points={ + 'console_scripts': [ + 'rqt_ground_control_station = ' + package_name + '.main:main', + ], + }, +) diff --git a/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/__init__.py b/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/drag_and_drop.py b/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/drag_and_drop.py new file mode 100644 index 000000000..0041927fb --- /dev/null +++ b/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/drag_and_drop.py @@ -0,0 +1,157 @@ +# adapted from https://www.pythonguis.com/faq/pyqt-drag-drop-widgets/from PyQt5 import QtCore +from PyQt5 import QtCore +from PyQt5.QtCore import QMimeData, Qt, pyqtSignal +from PyQt5.QtGui import QDrag, QPixmap +from PyQt5.QtWidgets import ( + QApplication, + QHBoxLayout, + QLabel, + QMainWindow, + QVBoxLayout, + QGridLayout, + QWidget, + QComboBox, + QLineEdit +) + +class DragTargetIndicator(QLabel): + def __init__(self, parent=None): + super().__init__(parent) + #self.setContentsMargins(25, 5, 25, 5) + self.setStyleSheet("QLabel { background-color: #ccc; border: 1px solid black; }") + + def set_size(self, size): + self.setFixedSize(size) + + +class DragItem(QWidget): + def __init__(self, w): + super().__init__() + self.widget = w + self.widget.destroyed.connect(self.child_destroyed) + self.setObjectName('main') + self.layout = QVBoxLayout() + self.setLayout(self.layout) + self.setAttribute(QtCore.Qt.WA_StyledBackground, True) + self.setStyleSheet('QWidget#main {background-color: lightcyan; border: 1px solid black;}') + + self.layout.addWidget(w) + + def set_data(self, data): + self.data = data + + def child_destroyed(self): + self.deleteLater() + + def mouseMoveEvent(self, e): + if e.buttons() == Qt.LeftButton: + drag = QDrag(self) + mime = QMimeData() + drag.setMimeData(mime) + + pixmap = QPixmap(self.size()) + self.render(pixmap) + drag.setPixmap(pixmap) + + drag.exec_(Qt.MoveAction) + self.show() # Show this widget again, if it's dropped outside. + + +class DragWidget(QWidget): + """ + Generic list sorting handler. + """ + + orderChanged = pyqtSignal(list) + + def __init__(self, *args, orientation=Qt.Orientation.Horizontal, **kwargs): + super().__init__() + self.setAcceptDrops(True) + + # Store the orientation for drag checks later. + self.orientation = orientation + + if self.orientation == Qt.Orientation.Vertical: + self.blayout = QVBoxLayout() + else: + self.blayout = QHBoxLayout() + + # Add the drag target indicator. This is invisible by default, + # we show it and move it around while the drag is active. + self._drag_target_indicator = DragTargetIndicator() + self.blayout.addWidget(self._drag_target_indicator) + self._drag_target_indicator.hide() + + self.setLayout(self.blayout) + + def dragEnterEvent(self, e): + e.accept() + + def dragLeaveEvent(self, e): + self._drag_target_indicator.hide() + e.accept() + + def dragMoveEvent(self, e): + # Find the correct location of the drop target, so we can move it there. + index = self._find_drop_location(e) + if index is not None: + # Inserting moves the item if its alreaady in the layout. + self.blayout.insertWidget(index, self._drag_target_indicator) + # Hide the item being dragged. + e.source().hide() + # Show the target. + self._drag_target_indicator.set_size(e.source().size()) + self._drag_target_indicator.show() + e.accept() + + def dropEvent(self, e): + widget = e.source() + # Use drop target location for destination, then remove it. + self._drag_target_indicator.hide() + index = self.blayout.indexOf(self._drag_target_indicator) + if index is not None: + self.blayout.insertWidget(index, widget) + self.orderChanged.emit(self.get_item_data()) + widget.show() + self.blayout.activate() + e.accept() + + def _find_drop_location(self, e): + pos = e.pos() + spacing = self.blayout.spacing() / 2 + + for n in range(self.blayout.count()): + # Get the widget at each index in turn. + w = self.blayout.itemAt(n).widget() + + if self.orientation == Qt.Orientation.Vertical: + # Drag drop vertically. + drop_here = ( + pos.y() >= w.y() - spacing + and pos.y() <= w.y() + w.size().height() + spacing + ) + else: + # Drag drop horizontally. + drop_here = ( + pos.x() >= w.x() - spacing + and pos.x() <= w.x() + w.size().width() + spacing + ) + + if drop_here: + # Drop over this target. + break + + return n + + def add_item(self, item): + self.blayout.addWidget(item) + + def get_item_data(self): + data = [] + for n in range(self.blayout.count()): + # Get the widget at each index in turn. + w = self.blayout.itemAt(n).widget() + if hasattr(w, "data"): + # The target indicator has no data. + data.append(w.data) + return data diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/main.py b/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/main.py similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/main.py rename to ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/main.py diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/py_console_text_edit.py b/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/py_console_text_edit.py similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/py_console_text_edit.py rename to ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/py_console_text_edit.py diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/py_console_widget.py b/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/py_console_widget.py similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/py_console_widget.py rename to ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/py_console_widget.py diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/spyder_console_widget.py b/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/spyder_console_widget.py similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/spyder_console_widget.py rename to ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/spyder_console_widget.py diff --git a/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/template.py b/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/template.py new file mode 100644 index 000000000..19f43b262 --- /dev/null +++ b/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/template.py @@ -0,0 +1,738 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from python_qt_binding.QtWidgets import QVBoxLayout, QWidget +from rqt_gui_py.plugin import Plugin +from qt_gui_py_common.simple_settings_dialog import SimpleSettingsDialog +from rqt_py_console.py_console_widget import PyConsoleWidget + +import python_qt_binding.QtWidgets as qt +import python_qt_binding.QtWidgets as QtWidgets +import python_qt_binding.QtGui as gui +import python_qt_binding.QtCore as core + +from ament_index_python.packages import get_package_share_directory +import yaml +import os +import collections +import copy +import inspect +import time +import pickle + +from behavior_tree_msgs.msg import Status, BehaviorTreeCommand, BehaviorTreeCommands +from airstack_msgs.msg import FixedTrajectory +from diagnostic_msgs.msg import KeyValue + +from .drag_and_drop import DragWidget, DragItem +from .trajectory_dialog import TrajectoryDialog + +logger = None + +class GroundControlStation(Plugin): + + def __init__(self, context): + super(GroundControlStation, self).__init__(context) + self.setObjectName('GroundControlStation') + + self.settings = {'command_config_filename': None, 'trajectory_config_filename': None, + 'robots': [], 'groups': {'commands': []}, 'publishers': {}} + + #self.config_filename = '' + self.button_groups = {} + + self.context = context + self.node = self.context.node + global logger + logger = self.node.get_logger() + + # main layout + self.widget = QWidget() + self.vbox = qt.QVBoxLayout() + self.widget.setLayout(self.vbox) + context.add_widget(self.widget) + + # config widget + self.config_widget = qt.QWidget() + #self.config_widget.setStyleSheet('QWidget{margin-left:-1px;}') + self.config_layout = qt.QHBoxLayout() + self.config_widget.setLayout(self.config_layout) + self.config_widget.setFixedHeight(50) + + self.robot_selection_label = qt.QLabel('Robot to command:') + self.config_layout.addWidget(self.robot_selection_label) + + self.robot_combo_box = qt.QComboBox() + self.config_layout.addWidget(self.robot_combo_box) + + self.vbox.addWidget(self.config_widget) + + spacer = qt.QSpacerItem(20, 40, qt.QSizePolicy.Minimum, qt.QSizePolicy.Expanding) + self.vbox.addItem(spacer) + + # button widget + self.button_widget = qt.QWidget() + self.button_layout = qt.QVBoxLayout() + self.button_widget.setLayout(self.button_layout) + self.vbox.addWidget(self.button_widget) + + spacer = qt.QSpacerItem(20, 40, qt.QSizePolicy.Minimum, qt.QSizePolicy.Expanding) + self.vbox.addItem(spacer) + + # timeline widget + self.timeline_widget = qt.QWidget() + self.timeline_layout = qt.QHBoxLayout() + self.timeline_widget.setLayout(self.timeline_layout) + + self.timeline_button_widget = qt.QWidget() + self.timeline_button_layout = qt.QVBoxLayout(self.timeline_button_widget) + self.timeline_layout.addWidget(self.timeline_button_widget) + + self.default_button_style = 'QPushButton{font-size: 40px; font-weight: bold}' + self.green_button_style = 'QPushButton{font-size: 40px; font-weight: bold; background-color: green}' + self.yellow_button_style = 'QPushButton{font-size: 40px; font-weight: bold; background-color: yellow}' + self.red_button_style = 'QPushButton{font-size: 40px; font-weight: bold; background-color: red}' + self.timeline_play_button = AspectRatioButton('▶') + self.timeline_play_button.setStyleSheet(self.default_button_style) + self.timeline_play_button.clicked.connect(self.play) + self.timeline_button_layout.addWidget(self.timeline_play_button) + + self.timeline_pause_button = AspectRatioButton('❚❚') + self.timeline_pause_button.setStyleSheet(self.default_button_style) + self.timeline_pause_button.clicked.connect(self.pause) + self.timeline_button_layout.addWidget(self.timeline_pause_button) + + self.timeline_stop_button = AspectRatioButton('◼') + self.timeline_stop_button.setStyleSheet(self.red_button_style) + self.timeline_stop_button.clicked.connect(self.stop) + self.timeline_button_layout.addWidget(self.timeline_stop_button) + + self.timeline_drag_container_widget = qt.QWidget() + self.timeline_drag_container_layout = qt.QHBoxLayout(self.timeline_drag_container_widget) + + self.timeline_drag_widget = DragWidget() + self.timeline_drag_container_layout.addWidget(self.timeline_drag_widget) + self.timeline_drag_container_layout.addStretch(1) + #self.timeline_layout.addWidget(self.timeline_drag_widget) + self.timeline_scroll_area = qt.QScrollArea() + self.timeline_scroll_area.setWidgetResizable(True) + self.timeline_scroll_area.setFixedHeight(300) + self.timeline_scroll_area.setWidget(self.timeline_drag_container_widget) + #self.timeline_scroll_area.setWidget(self.timeline_drag_widget) + self.timeline_layout.addWidget(self.timeline_scroll_area) + + self.right_widget = qt.QWidget() + self.right_layout = qt.QVBoxLayout(self.right_widget) + + self.timeline_add_button = AspectRatioButton('+') + self.timeline_add_button.setStyleSheet(self.default_button_style) + self.timeline_add_button.clicked.connect(self.add_timeline_item) + self.right_layout.addWidget(self.timeline_add_button) + self.right_layout.addStretch(1) + + self.timeline_save_button = qt.QPushButton('Save Mission') + self.timeline_save_button.clicked.connect(self.save_timeline) + self.right_layout.addWidget(self.timeline_save_button) + + self.timeline_load_button = qt.QPushButton('Load Mission') + self.timeline_load_button.clicked.connect(self.load_timeline) + self.right_layout.addWidget(self.timeline_load_button) + + self.timeline_clear_button = qt.QPushButton('Clear Mission') + self.timeline_clear_button.clicked.connect(self.clear_timeline) + self.right_layout.addWidget(self.timeline_clear_button) + + self.timeline_layout.addWidget(self.right_widget) + + self.vbox.addWidget(self.timeline_widget) + + self.timer = core.QTimer(self) + self.timer.timeout.connect(self.play) + + def save_timeline(self): + timeline_widgets = self.get_timeline_widgets() + save_data = [] + for t in timeline_widgets: + save_data.append(t.get_save_data()) + filename = qt.QFileDialog.getSaveFileName(self.widget, 'Save Mission', '', 'Pickle Files (*.pickle)')[0] + if not filename.endswith('.pickle'): + filename += '.pickle' + with open(filename, 'wb') as handle: + pickle.dump(save_data, handle, protocol=pickle.HIGHEST_PROTOCOL) + + def load_timeline(self): + self.clear_timeline() + filename = qt.QFileDialog.getOpenFileName(self.widget, 'Load Mission', '', 'Pickle Files (*.pickle)')[0] + if not os.path.isfile(filename): + return + with open(filename, 'rb') as handle: + save_data = pickle.load(handle) + for s in save_data: + item = DragItem(TimelineEventWidget(self.settings, s)) + item.setMinimumSize(260, 260) + item.setMaximumSize(260, 260) + self.timeline_drag_widget.add_item(item) + + def clear_timeline(self): + timeline_widgets = self.get_timeline_widgets() + for t in timeline_widgets: + t.deleteLater() + + def get_timeline_widgets(self): + timeline_widgets = [] + for i in range(self.timeline_drag_widget.blayout.count()): + try: + timeline_widgets.append(self.timeline_drag_widget.blayout.itemAt(i).widget().widget) + except: + pass + + return timeline_widgets + + def update_timeline_widgets(self): + timeline_widgets = self.get_timeline_widgets() + for tw in timeline_widgets: + tw.set_done(tw.event.is_done()) + + def play(self): + timeline_widgets = self.get_timeline_widgets() + if len(timeline_widgets) == 0: + return + self.timeline_play_button.setStyleSheet(self.green_button_style) + self.timeline_pause_button.setStyleSheet(self.default_button_style) + self.timeline_stop_button.setStyleSheet(self.default_button_style) + for tw in timeline_widgets: + if not tw.event.is_done(): + tw.event.play() + break + self.timer.start(100) + self.update_timeline_widgets() + self.timeline_drag_widget.setEnabled(False) + self.timeline_add_button.setEnabled(False) + + def pause(self): + self.timeline_play_button.setStyleSheet(self.default_button_style) + self.timeline_pause_button.setStyleSheet(self.yellow_button_style) + self.timeline_stop_button.setStyleSheet(self.default_button_style) + self.timer.stop() + timeline_widgets = self.get_timeline_widgets() + for tw in timeline_widgets: + tw.event.pause() + self.update_timeline_widgets() + + def stop(self): + self.timeline_play_button.setStyleSheet(self.default_button_style) + self.timeline_pause_button.setStyleSheet(self.default_button_style) + self.timeline_stop_button.setStyleSheet(self.red_button_style) + self.timer.stop() + timeline_widgets = self.get_timeline_widgets() + for tw in timeline_widgets: + tw.event.stop() + self.update_timeline_widgets() + self.timeline_drag_widget.setEnabled(True) + self.timeline_add_button.setEnabled(True) + + def add_timeline_item(self): + item = DragItem(TimelineEventWidget(self.settings)) + item.setMinimumSize(260, 260) + item.setMaximumSize(260, 260) + self.timeline_drag_widget.add_item(item) + + def select_config_file(self): + starting_path = get_package_share_directory('rqt_ground_control_station') + '/config/' + filename = qt.QFileDialog.getOpenFileName(self.widget, 'Open Config File', starting_path, "Config Files (*.yaml)")[0] + print(filename) + self.set_command_config(filename) + + def set_command_config(self, filename): + if filename != '': + self.settings['command_config_filename'] = filename + if self.settings['command_config_filename'] != None: + self.init_buttons(filename) + + def set_trajectory_config(self, filename): + if filename != '': + self.settings['trajectory_config_filename'] = filename + + def init_buttons(self, filename): + y = yaml.load(open(filename, 'r').read(), Loader=yaml.Loader) + #self.node.get_logger().info(str(y)) + + for i in reversed(range(self.button_layout.count())): + self.button_layout.itemAt(i).widget().setParent(None) + self.button_groups = {} + + def get_click_function(group, button): + def click_function(): + commands = BehaviorTreeCommands() + #self.node.get_logger().info(str(self.button_groups)) + for i in range(len(self.button_groups[group]['buttons'])): + b = self.button_groups[group]['buttons'][i] + command = BehaviorTreeCommand() + command.condition_name = self.button_groups[group]['condition_names'][i] + + if b != button and b.isChecked(): + b.toggle() + command.status = Status.FAILURE + elif b == button and not b.isChecked(): + command.status = Status.FAILURE + elif b == button and b.isChecked(): + command.status = Status.SUCCESS + commands.commands.append(command) + #self.command_pub.publish(commands) + self.settings['publishers'][self.robot_combo_box.currentText()]['command_pub'].publish(commands) + #self.node.get_logger().info(str(self.settings['publishers'][self.robot_combo_box.currentText()]['command_pub'].topic_name)) + return click_function + + self.robot_combo_box.clear() + for robot in y['robots']: + self.robot_combo_box.addItem(robot) + #self.robot_combo_box.model().item(0).setBackground(gui.QColor('red')) + self.settings['robots'].append(robot) + + # init publishers + for robot in self.settings['robots']: + self.settings['publishers'][robot] = {'command_pub': + self.node.create_publisher(BehaviorTreeCommands, + '/' + robot + '/behavior/behavior_tree_commands', 1), + 'trajectory_pub': + self.node.create_publisher(FixedTrajectory, + '/' + robot + '/fixed_trajectory_generator/fixed_trajectory_command', 1)} + + for group in y['groups']: + group_name = list(group.keys())[0] + if group_name not in self.button_groups.keys(): + self.button_groups[group_name] = {'buttons' : [], 'condition_names': []} + self.settings['groups'][group_name] = {'condition_titles' : [], 'condition_names': []} + + group_widget = qt.QWidget() + group_layout = qt.QVBoxLayout() + group_widget.setLayout(group_layout) + self.button_layout.addWidget(group_widget) + + group_layout.addWidget(qt.QLabel(group_name)) + + button_widget = qt.QWidget() + button_layout = qt.QHBoxLayout() + button_widget.setLayout(button_layout) + group_layout.addWidget(button_widget) + + for buttons in group[group_name]: + button_name = list(buttons.keys())[0] + condition_name = buttons[button_name]['condition_name'] + + button = qt.QPushButton(button_name) + button.clicked.connect(get_click_function(group_name, button)) + button.setCheckable(True) + button_layout.addWidget(button) + + #print(condition_name, bt.get_condition_topic_name(condition_name)) + self.button_groups[group_name]['buttons'].append(button) + self.button_groups[group_name]['condition_names'].append(condition_name) + + self.settings['groups'][group_name]['condition_titles'].append(button_name) + self.settings['groups'][group_name]['condition_names'].append(condition_name) + + def save_settings(self, plugin_settings, instance_settings): + instance_settings.set_value('command_config_filename', self.settings['command_config_filename']) + instance_settings.set_value('trajectory_config_filename', self.settings['trajectory_config_filename']) + + def restore_settings(self, plugin_settings, instance_settings): + self.set_command_config(instance_settings.value('command_config_filename')) + self.set_trajectory_config(instance_settings.value('trajectory_config_filename')) + + def trigger_configuration(self): + sd = SettingsDialog(self.settings) + if sd.exec(): + self.set_command_config(sd.command_config_filename) + self.set_trajectory_config(sd.trajectory_config_filename) + else: + pass + + def shutdown_console_widget(self): + pass + + def shutdown_plugin(self): + self.shutdown_console_widget() + +class SettingsDialog(qt.QDialog): + def __init__(self, settings): + super().__init__(None) + self.setWindowTitle('Settings') + layout = qt.QVBoxLayout() + + self.command_config_filename = settings['command_config_filename'] + self.trajectory_config_filename = settings['trajectory_config_filename'] + + command_widget = qt.QWidget() + command_layout = qt.QHBoxLayout(command_widget) + + command_config_button = qt.QPushButton('Open Config...') + command_config_button.clicked.connect(self.select_command_config_file) + command_layout.addWidget(command_config_button) + + self.command_config_label = qt.QLabel('Command Config File:' + os.path.basename(str(self.command_config_filename))) + command_layout.addWidget(self.command_config_label) + + trajectory_widget = qt.QWidget() + trajectory_layout = qt.QHBoxLayout(trajectory_widget) + + trajectory_config_button = qt.QPushButton('Open Config...') + trajectory_layout.addWidget(trajectory_config_button) + trajectory_config_button.clicked.connect(self.select_trajectory_config_file) + + self.trajectory_config_label = qt.QLabel('Trajectory Config File:' + os.path.basename(str(self.trajectory_config_filename))) + trajectory_layout.addWidget(self.trajectory_config_label) + + self.buttons = qt.QDialogButtonBox(qt.QDialogButtonBox.Ok | qt.QDialogButtonBox.Cancel) + self.buttons.accepted.connect(self.accept) + self.buttons.rejected.connect(self.reject) + + layout.addWidget(command_widget) + layout.addWidget(trajectory_widget) + layout.addWidget(self.buttons) + self.setLayout(layout) + + def select_command_config_file(self): + starting_path = get_package_share_directory('rqt_ground_control_station') + '/config/' + self.command_config_filename = \ + qt.QFileDialog.getOpenFileName(self, 'Open Config File', starting_path, 'Config Files (*.yaml)')[0] + self.command_config_label.setText('Command Config File: ' + os.path.basename(self.command_config_filename)) + + def select_trajectory_config_file(self): + starting_path = get_package_share_directory('rqt_ground_control_station') + '/config/' + self.trajectory_config_filename = \ + qt.QFileDialog.getOpenFileName(self, 'Open Config File', starting_path, 'Config Files (*.yaml)')[0] + self.trajectory_config_label.setText('Trajectory Config File: ' + os.path.basename(self.trajectory_config_filename)) + +class TimelineEventWidget(QWidget): + + def __init__(self, global_settings, save_data=None): + super().__init__() + self.global_settings = global_settings + self.event = None + + self.layout = qt.QVBoxLayout() + self.setLayout(self.layout) + + self.top_widget = qt.QWidget() + #self.top_widget.setObjectName('top') + #self.top_widget.setStyleSheet('QWidget#top {background-color: red; border: 1px solid black;}') + self.top_widget.setMaximumHeight(50) + self.top_layout = qt.QHBoxLayout(self.top_widget) + #self.top_layout.setSpacing(0) + #self.top_layout.setContentsMargins(0, 0, 0, 0) + + self.task_combo_box = qt.QComboBox() + self.task_combo_box.addItem('Wait', WaitEvent) + self.task_combo_box.addItem('Command', CommandEvent) + self.task_combo_box.addItem('Trajectory', TrajectoryEvent) + self.top_layout.addWidget(self.task_combo_box) + #self.layout.addWidget(self.task_combo_box) + + self.delete_button = qt.QPushButton('X') + self.delete_button.clicked.connect(self.deleteLater) + self.delete_button.setMaximumWidth(20) + self.top_layout.addWidget(self.delete_button) + self.layout.addWidget(self.top_widget) + #self.layout.addStretch(1) + + self.content_widget = qt.QWidget() + self.content_widget.setObjectName('content') + self.content_layout = qt.QVBoxLayout() + self.content_widget.setLayout(self.content_layout) + self.layout.addWidget(self.content_widget) + + if save_data == None: + self.task_combo_box.currentIndexChanged.connect(self.task_combo_box_change) + self.task_combo_box_change(0) + else: + self.task_combo_box.setCurrentText(save_data['type']) + self.clear_content() + self.event = self.task_combo_box.itemData(self.task_combo_box.currentIndex())(self.global_settings, + save_data['local_settings']) + self.event.init_widgets(self.content_layout) + + def get_save_data(self): + dct = {'type': self.task_combo_box.currentText(), 'local_settings': {}} + if self.event != None: + dct['local_settings'] = self.event.local_settings + return dct + + def set_done(self, b): + if b: + self.content_widget.setStyleSheet('QWidget#content {background-color: green; border: 1px solid black;}') + else: + self.content_widget.setStyleSheet('QWidget#content {background-color: lightcyan;}') + + def task_combo_box_change(self, s): + self.clear_content() + self.event = self.task_combo_box.itemData(s)(self.global_settings) + self.event.init_widgets(self.content_layout) + + def clear_content(self): + for i in reversed(range(self.content_layout.count())): + self.content_layout.itemAt(i).widget().setParent(None) + + +class TimelineEvent: + def __init__(self, global_settings, local_settings={}): + self.global_settings = global_settings + self.local_settings = local_settings + + self.done = False + + def play(self): + self.done = True + + def pause(self): + pass + + def stop(self): + self.done = False + + def init_widgets(self, parent_layout): + pass + + def is_done(self): + return self.done + +class RobotEvent(TimelineEvent): + def __init__(self, global_settings, local_settings={}): + super(RobotEvent, self).__init__(global_settings, local_settings) + if 'robot' not in self.local_settings.keys(): + self.local_settings['robot'] = self.global_settings['robots'][0] + + def init_widgets(self, parent_layout): + super().init_widgets(parent_layout) + + widget = qt.QWidget() + layout = qt.QHBoxLayout(widget) + + self.robot_label = qt.QLabel('Robot:') + layout.addWidget(self.robot_label) + + self.robots_combo_box = qt.QComboBox() + self.robots_combo_box.addItems(self.global_settings['robots']) + self.robots_combo_box.currentIndexChanged.connect(self.robots_combo_box_change) + self.robots_combo_box.setCurrentText(self.local_settings['robot']) + layout.addWidget(self.robots_combo_box) + + parent_layout.addWidget(widget) + + def robots_combo_box_change(self, index): + self.local_settings['robot'] = self.robots_combo_box.itemText(index) + +class PublisherEvent(TimelineEvent): + def __init__(self, global_settings, local_settings={}): + super(PublisherEvent, self).__init__(global_settings, local_settings) + + def init_widgets(self, parent_layout): + super().init_widgets(parent_layout) + + def get_publisher(self): + return None + + def get_message(self): + return None + + def play(self): + self.done = True + pub = self.get_publisher() + msg = self.get_message() + if pub == None or msg == None: + return + pub.publish(msg) + +class CommandEvent(RobotEvent, PublisherEvent): + def __init__(self, global_settings, local_settings={}): + super(CommandEvent, self).__init__(global_settings, local_settings) + if 'command_title' not in self.local_settings.keys(): + self.local_settings['command_title'] = self.global_settings['groups']['commands']['condition_titles'][0] + if 'command_name' not in self.local_settings.keys(): + self.local_settings['command_name'] = self.global_settings['groups']['commands']['condition_names'][0] + + def get_publisher(self): + if self.local_settings['robot'] in self.global_settings['publishers']: + return self.global_settings['publishers'][self.local_settings['robot']]['command_pub'] + return None + + def get_message(self): + commands = BehaviorTreeCommands() + selected = self.local_settings['command_title']#self.commands_combo_box.currentText() + for i in range(self.commands_combo_box.count()): + command = BehaviorTreeCommand() + command.condition_name = self.local_settings['command_name']#self.commands_combo_box.itemData(i) + if selected == self.commands_combo_box.itemText(i): + command.status = Status.SUCCESS + else: + command.status = Status.FAILURE + commands.commands.append(command) + return commands + + def init_widgets(self, parent_layout): + super().init_widgets(parent_layout) + + self.commands_combo_box = qt.QComboBox() + for i in range(len(self.global_settings['groups']['commands']['condition_titles'])): + title = self.global_settings['groups']['commands']['condition_titles'][i] + name = self.global_settings['groups']['commands']['condition_names'][i] + self.commands_combo_box.addItem(title, name) + self.commands_combo_box.currentIndexChanged.connect(self.commands_combo_box_change) + self.commands_combo_box.setCurrentText(self.local_settings['command_title']) + #self.commands_combo_box_change(self.global_settings['groups']['commands']['condition_titles'].index(self.local_settings['command_title'])) + parent_layout.addWidget(self.commands_combo_box) + + def commands_combo_box_change(self, index): + self.local_settings['command_title'] = self.commands_combo_box.itemText(index) + self.local_settings['command_name'] = self.commands_combo_box.itemData(index) + +class TrajectoryEvent(RobotEvent, PublisherEvent): + def __init__(self, global_settings, local_settings={}): + super(TrajectoryEvent, self).__init__(global_settings, local_settings) + if 'trajectory_attributes' not in self.local_settings.keys(): + self.local_settings['trajectory_attributes'] = collections.OrderedDict() + + def get_publisher(self): + if self.local_settings['robot'] in self.global_settings['publishers']: + return self.global_settings['publishers'][self.local_settings['robot']]['trajectory_pub'] + return None + + def get_message(self): + trajectory_name = self.label.text().split(':')[-1].strip() + if trajectory_name == 'None': + return None + msg = FixedTrajectory() + msg.type = trajectory_name + for attribute, value in iter(self.local_settings['trajectory_attributes'][trajectory_name].items()): + key_value = KeyValue() + key_value.key = attribute + key_value.value = value + msg.attributes.append(key_value) + return msg + + def init_widgets(self, parent_layout): + super().init_widgets(parent_layout) + + widget = qt.QWidget() + layout = qt.QGridLayout(widget) + + traj_name = 'None' + if 'trajectory_name' in self.local_settings['trajectory_attributes'].keys(): + traj_name = self.local_settings['trajectory_attributes']['trajectory_name'] + self.label = qt.QLabel('Trajectory: ' + traj_name) + layout.addWidget(self.label, 1, 0) + + button = qt.QPushButton('Configure') + def click(s): + td = TrajectoryDialog(self.global_settings['trajectory_config_filename'], + copy.deepcopy(self.local_settings['trajectory_attributes'])) + ret = td.exec() + if ret: + self.local_settings['trajectory_attributes'] = copy.deepcopy(td.attribute_settings) + self.label.setText('Trajectory: ' + td.attribute_settings['trajectory_name']) + button.clicked.connect(click) + layout.addWidget(button, 2, 0) + + parent_layout.addWidget(widget) + +class WaitEvent(TimelineEvent): + def __init__(self, global_settings, local_settings={}): + super(WaitEvent, self).__init__(global_settings, local_settings) + if 'wait_time' not in self.local_settings.keys(): + self.local_settings['wait_time'] = 5. + self.start_time = None + self.paused_elapsed = 0. + self.elapsed = 0. + + def init_widgets(self, parent_layout): + super().init_widgets(parent_layout) + + widget = qt.QWidget() + layout = qt.QHBoxLayout(widget) + + time_label = qt.QLabel('Time:') + layout.addWidget(time_label) + + self.line_edit = qt.QLineEdit() + self.line_edit.textChanged.connect(self.text_changed) + self.line_edit.setText(str(self.local_settings['wait_time'])) + layout.addWidget(self.line_edit) + + s_label = qt.QLabel('s') + layout.addWidget(s_label) + + self.elapsed_label = qt.QLabel('Elapsed: %0.1f / %0.1f s' % (0., self.local_settings['wait_time'])) + + parent_layout.addWidget(widget) + parent_layout.addWidget(self.elapsed_label) + + def text_changed(self, text): + try: + self.local_settings['wait_time'] = float(text) + self.elapsed_label.setText('Elapsed: %0.1f / %0.1f s' % (0., self.local_settings['wait_time'])) + except: + pass + + def play(self): + if self.start_time == None or self.paused_elapsed != 0.: + self.start_time = time.time() - self.paused_elapsed + self.paused_elapsed = 0. + self.elapsed = time.time() - self.start_time + self.elapsed_label.setText('Elapsed: %0.1f / %0.1f s' % (self.elapsed, self.local_settings['wait_time'])) + + def pause(self): + if self.start_time != None: + self.paused_elapsed = time.time() - self.start_time + + def stop(self): + self.start_time = None + self.paused_elapsed = 0. + self.elapsed = 0. + self.elapsed_label.setText('Elapsed: %0.1f / %0.1f s' % (self.elapsed, self.local_settings['wait_time'])) + + def is_done(self): + return (self.start_time != None) and (self.elapsed >= self.local_settings['wait_time']) + + + +class AspectRatioButton(qt.QPushButton): + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.aspect_ratio = 1.0 + + def resizeEvent(self, event): + size = event.size() + if size.height() == 0 or size.width() / size.height() > self.aspect_ratio: + size.setWidth(int(size.height() * self.aspect_ratio)) + else: + size.setHeight(int(size.width() / self.aspect_ratio)) + self.resize(size) diff --git a/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/trajectory_dialog.py b/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/trajectory_dialog.py new file mode 100644 index 000000000..6820be84d --- /dev/null +++ b/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/trajectory_dialog.py @@ -0,0 +1,181 @@ +from python_qt_binding.QtWidgets import QVBoxLayout, QWidget +from rqt_gui_py.plugin import Plugin +from qt_gui_py_common.simple_settings_dialog import SimpleSettingsDialog +from rqt_py_console.py_console_widget import PyConsoleWidget + +import python_qt_binding.QtWidgets as qt +import python_qt_binding.QtWidgets as QtWidgets +import python_qt_binding.QtGui as gui +import python_qt_binding.QtCore as QtCore + +from ament_index_python.packages import get_package_share_directory +import yaml +import os +import collections + + +class TrajectoryDialog(qt.QDialog): + ''' + def __init__(self, parent=None): + super().__init__(parent) + + self.setWindowTitle("HELLO!") + + QBtn = qt.QDialogButtonBox.Ok | qt.QDialogButtonBox.Cancel + + self.buttonBox = qt.QDialogButtonBox(QBtn) + self.buttonBox.accepted.connect(self.accept) + self.buttonBox.rejected.connect(self.reject) + + layout = qt.QVBoxLayout() + message = qt.QLabel("Something happened, is that OK?") + layout.addWidget(message) + layout.addWidget(self.buttonBox) + self.setLayout(layout) + ''' + def __init__(self, trajectory_config_filename, default_attribute_settings): + super().__init__(None) + + self.config_filename = '' + + self.button_dct = {} + self.attribute_settings = default_attribute_settings + + # main layout + self.vbox = qt.QVBoxLayout() + self.setLayout(self.vbox) + + # trajectory widget + self.trajectory_widget = qt.QWidget() + self.trajectory_layout = qt.QVBoxLayout() + self.trajectory_widget.setLayout(self.trajectory_layout) + self.vbox.addWidget(self.trajectory_widget) + + self.tab_widget = qt.QTabWidget() + self.trajectory_layout.addWidget(self.tab_widget) + + # button widget + self.button_widget = qt.QWidget() + self.button_layout = qt.QHBoxLayout() + self.button_widget.setLayout(self.button_layout) + self.vbox.addWidget(self.button_widget) + + self.publish_button = qt.QPushButton('Publish') + self.publish_button.clicked.connect(self.publish_trajectory) + self.button_layout.addWidget(self.publish_button) + + self.trajectory_type_label = qt.QLabel('Type: ') + self.button_layout.addWidget(self.trajectory_type_label) + + self.trajectory_type_combo_box = qt.QComboBox() + self.trajectory_type_combo_box.addItem('Fixed Trajectory') + self.trajectory_type_combo_box.addItem('Global Plan') + self.button_layout.addWidget(self.trajectory_type_combo_box) + + # ok/cancel buttons + self.buttons = qt.QDialogButtonBox(qt.QDialogButtonBox.Ok | qt.QDialogButtonBox.Cancel) + self.buttons.accepted.connect(self.accept) + self.buttons.rejected.connect(self.reject) + self.vbox.addWidget(self.buttons) + + self.set_config(trajectory_config_filename) + self.tab_widget.currentChanged.connect(self.on_tab_changed) + + def on_tab_changed(self, index): + self.attribute_settings['tab_index'] = index + self.attribute_settings['trajectory_name'] = self.tab_widget.tabText(index) + + def publish_trajectory(self): + trajectory_type = self.trajectory_type_combo_box.currentText() + trajectory_name = self.tab_widget.tabText(self.tab_widget.currentIndex()) + msg = FixedTrajectory() + msg.type = trajectory_name + for attribute, value in iter(self.attribute_settings[trajectory_name].items()): + key_value = KeyValue() + key_value.key = attribute + key_value.value = value + msg.attributes.append(key_value) + if trajectory_type == 'Fixed Trajectory': + self.fixed_trajectory_pub.publish(msg) + elif trajectory_type == 'Global Plan': + self.global_plan_fixed_trajectory_pub.publish(msg) + + def select_config_file(self): + starting_path = get_package_share_directory('rqt_fixed_trajectory_generator') + '/config/' + print(starting_path) + filename = qt.QFileDialog.getOpenFileName(self.widget, 'Open Config File', starting_path, "Config Files (*.yaml)")[0] + self.set_config(filename) + + def set_config(self, filename): + if filename != '': + self.config_filename = filename + if self.config_filename != None: + self.init_buttons(filename) + if 'trajectory_name' not in self.attribute_settings: + self.on_tab_changed(0) + + def init_buttons(self, filename): + y = yaml.load(open(filename, 'r').read(), Loader=yaml.Loader) + print(y) + + def get_attribute_changed_function(trajectory_name, attribute_name): + def attribute_changed(text): + if trajectory_name not in self.attribute_settings: + self.attribute_settings[trajectory_name] = {} + self.attribute_settings[trajectory_name][attribute_name] = text + return attribute_changed + + def get_publish_function(trajectory_name): + def publish_function(): + msg = FixedTrajectory() + msg.type = trajectory_name + for attribute, value in iter(self.attribute_settings[trajectory_name].items()): + key_value = KeyValue() + key_value.key = attribute + key_value.value = value + msg.attributes.append(key_value) + self.fixed_trajectory_pub.publish(msg) + return publish_function + + + for trajectory in y['trajectories']: + trajectory_name = list(trajectory.keys())[0] + attributes = trajectory[trajectory_name]['attributes'] + + trajectory_tab = qt.QWidget() + trajectory_layout = qt.QVBoxLayout() + trajectory_tab.setLayout(trajectory_layout) + + for attribute in attributes: + attribute_widget = qt.QWidget() + attribute_layout = qt.QHBoxLayout() + attribute_widget.setLayout(attribute_layout) + + attribute_label = qt.QLabel() + attribute_label.setText(attribute) + attribute_layout.addWidget(attribute_label) + + attribute_default = '0' + if attribute == 'frame_id': + attribute_default = 'base_link' + if trajectory_name in self.attribute_settings.keys(): + if attribute in self.attribute_settings[trajectory_name].keys(): + attribute_default = self.attribute_settings[trajectory_name][attribute] + + attribute_edit = qt.QLineEdit() + attribute_edit.textChanged.connect(get_attribute_changed_function(trajectory_name, + attribute)) + attribute_edit.setText(attribute_default) + + attribute_layout.addWidget(attribute_edit) + + trajectory_layout.addWidget(attribute_widget) + + #publish_button = qt.QPushButton('Publish') + #publish_button.clicked.connect(get_publish_function(trajectory_name)) + #trajectory_layout.addWidget(publish_button) + + self.tab_widget.addTab(trajectory_tab, trajectory_name) + if 'tab_index' in self.attribute_settings: + print(self.attribute_settings['tab_index']) + self.tab_widget.setCurrentIndex(self.attribute_settings['tab_index']) diff --git a/ground_control_station/ros_ws/src/rqt_py_template/CHANGELOG.rst b/ground_control_station/ros_ws/src/rqt_py_template/CHANGELOG.rst new file mode 100644 index 000000000..0a789b786 --- /dev/null +++ b/ground_control_station/ros_ws/src/rqt_py_template/CHANGELOG.rst @@ -0,0 +1,149 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rqt_py_console +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.2 (2021-08-31) +------------------ +* Fix modern setuptools warning about dashes instead of underscores (`#11 `_) +* Contributors: Chris Lalancette + +1.0.1 (2021-04-27) +------------------ +* Changed the build type to ament_python and fixed package to run with ros2 run (`#8 `_) +* Contributors: Alejandro Hernández Cordero + +1.0.0 (2018-12-11) +------------------ +* spyderlib -> spyder (`#5 `_) +* ros2 port (`#3 `_) +* autopep8 (`#2 `_) +* Contributors: Mike Lautman + +0.4.8 (2017-04-28) +------------------ + +0.4.7 (2017-03-02) +------------------ + +0.4.6 (2017-02-27) +------------------ + +0.4.5 (2017-02-03) +------------------ + +0.4.4 (2017-01-24) +------------------ +* use Python 3 compatible syntax (`#421 `_) + +0.4.3 (2016-11-02) +------------------ + +0.4.2 (2016-09-19) +------------------ + +0.4.1 (2016-05-16) +------------------ + +0.4.0 (2016-04-27) +------------------ +* Support Qt 5 (in Kinetic and higher) as well as Qt 4 (in Jade and earlier) (`#359 `_) + +0.3.13 (2016-03-08) +------------------- + +0.3.12 (2015-07-24) +------------------- + +0.3.11 (2015-04-30) +------------------- + +0.3.10 (2014-10-01) +------------------- +* update plugin scripts to use full name to avoid future naming collisions + +0.3.9 (2014-08-18) +------------------ + +0.3.8 (2014-07-15) +------------------ + +0.3.7 (2014-07-11) +------------------ +* export architecture_independent flag in package.xml (`#254 `_) + +0.3.6 (2014-06-02) +------------------ + +0.3.5 (2014-05-07) +------------------ + +0.3.4 (2014-01-28) +------------------ + +0.3.3 (2014-01-08) +------------------ +* add groups for rqt plugins, renamed some plugins (`#167 `_) + +0.3.2 (2013-10-14) +------------------ + +0.3.1 (2013-10-09) +------------------ + +0.3.0 (2013-08-28) +------------------ + +0.2.17 (2013-07-04) +------------------- + +0.2.16 (2013-04-09 13:33) +------------------------- + +0.2.15 (2013-04-09 00:02) +------------------------- + +0.2.14 (2013-03-14) +------------------- + +0.2.13 (2013-03-11 22:14) +------------------------- + +0.2.12 (2013-03-11 13:56) +------------------------- + +0.2.11 (2013-03-08) +------------------- + +0.2.10 (2013-01-22) +------------------- + +0.2.9 (2013-01-17) +------------------ + +0.2.8 (2013-01-11) +------------------ + +0.2.7 (2012-12-24) +------------------ + +0.2.6 (2012-12-23) +------------------ + +0.2.5 (2012-12-21 19:11) +------------------------ + +0.2.4 (2012-12-21 01:13) +------------------------ + +0.2.3 (2012-12-21 00:24) +------------------------ + +0.2.2 (2012-12-20 18:29) +------------------------ + +0.2.1 (2012-12-20 17:47) +------------------------ + +0.2.0 (2012-12-20 17:39) +------------------------ +* first release of this package into groovy diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/README.md b/ground_control_station/ros_ws/src/rqt_py_template/README.md similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/README.md rename to ground_control_station/ros_ws/src/rqt_py_template/README.md diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/generate_rqt_py_package.sh b/ground_control_station/ros_ws/src/rqt_py_template/generate_rqt_py_package.sh similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/generate_rqt_py_package.sh rename to ground_control_station/ros_ws/src/rqt_py_template/generate_rqt_py_package.sh diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/package.xml b/ground_control_station/ros_ws/src/rqt_py_template/package.xml similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/package.xml rename to ground_control_station/ros_ws/src/rqt_py_template/package.xml diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/plugin.xml b/ground_control_station/ros_ws/src/rqt_py_template/plugin.xml similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/plugin.xml rename to ground_control_station/ros_ws/src/rqt_py_template/plugin.xml diff --git a/ground_control_station/ros_ws/src/rqt_py_template/resource/py_console_widget.ui b/ground_control_station/ros_ws/src/rqt_py_template/resource/py_console_widget.ui new file mode 100644 index 000000000..12810f1c5 --- /dev/null +++ b/ground_control_station/ros_ws/src/rqt_py_template/resource/py_console_widget.ui @@ -0,0 +1,53 @@ + + + PyConsole + + + + 0 + 0 + 276 + 212 + + + + PyConsole + + + + 0 + + + 0 + + + 0 + + + 3 + + + 0 + + + + + 0 + + + + + + + + + + + PyConsoleTextEdit + QTextEdit +
rqt_py_console.py_console_text_edit
+ + + + + diff --git a/ground_control_station/ros_ws/src/rqt_py_template/resource/rqt_py_template b/ground_control_station/ros_ws/src/rqt_py_template/resource/rqt_py_template new file mode 100644 index 000000000..e69de29bb diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/setup.cfg b/ground_control_station/ros_ws/src/rqt_py_template/setup.cfg similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/setup.cfg rename to ground_control_station/ros_ws/src/rqt_py_template/setup.cfg diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/setup.py b/ground_control_station/ros_ws/src/rqt_py_template/setup.py similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/setup.py rename to ground_control_station/ros_ws/src/rqt_py_template/setup.py diff --git a/ground_control_station/ros_ws/src/rqt_py_template/src/rqt_py_template/__init__.py b/ground_control_station/ros_ws/src/rqt_py_template/src/rqt_py_template/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ground_control_station/ros_ws/src/rqt_py_template/src/rqt_py_template/main.py b/ground_control_station/ros_ws/src/rqt_py_template/src/rqt_py_template/main.py new file mode 100755 index 000000000..9a5c9376e --- /dev/null +++ b/ground_control_station/ros_ws/src/rqt_py_template/src/rqt_py_template/main.py @@ -0,0 +1,12 @@ +import sys + +from rqt_gui.main import Main + + +def main(): + main = Main() + sys.exit(main.main(sys.argv, standalone='rqt_py_console.py_console.PyConsole')) + + +if __name__ == '__main__': + main() diff --git a/ground_control_station/ros_ws/src/rqt_py_template/src/rqt_py_template/py_console_text_edit.py b/ground_control_station/ros_ws/src/rqt_py_template/src/rqt_py_template/py_console_text_edit.py new file mode 100644 index 000000000..dc9ce1a0a --- /dev/null +++ b/ground_control_station/ros_ws/src/rqt_py_template/src/rqt_py_template/py_console_text_edit.py @@ -0,0 +1,69 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import sys +from code import InteractiveInterpreter + +from python_qt_binding import QT_BINDING, QT_BINDING_VERSION +from python_qt_binding.QtCore import Qt, Signal + +from qt_gui_py_common.console_text_edit import ConsoleTextEdit + + +class PyConsoleTextEdit(ConsoleTextEdit): + _color_stdin = Qt.darkGreen + _multi_line_char = ':' + _multi_line_indent = ' ' + _prompt = ('>>> ', '... ') # prompt for single and multi line + exit = Signal() + + def __init__(self, parent=None): + super(PyConsoleTextEdit, self).__init__(parent) + + self._interpreter_locals = {} + self._interpreter = InteractiveInterpreter(self._interpreter_locals) + + self._comment_writer.write('Python %s on %s\n' % + (sys.version.replace('\n', ''), sys.platform)) + self._comment_writer.write( + 'Qt bindings: %s version %s\n' % (QT_BINDING, QT_BINDING_VERSION)) + + self._add_prompt() + + def update_interpreter_locals(self, newLocals): + self._interpreter_locals.update(newLocals) + + def _exec_code(self, code): + try: + self._interpreter.runsource(code) + except SystemExit: # catch sys.exit() calls, so they don't close the whole gui + self.exit.emit() diff --git a/ground_control_station/ros_ws/src/rqt_py_template/src/rqt_py_template/py_console_widget.py b/ground_control_station/ros_ws/src/rqt_py_template/src/rqt_py_template/py_console_widget.py new file mode 100644 index 000000000..e69bde34c --- /dev/null +++ b/ground_control_station/ros_ws/src/rqt_py_template/src/rqt_py_template/py_console_widget.py @@ -0,0 +1,59 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import os +from ament_index_python.resources import get_resource + +from python_qt_binding import loadUi +from python_qt_binding.QtWidgets import QWidget +from rqt_py_console.py_console_text_edit import PyConsoleTextEdit + + +class PyConsoleWidget(QWidget): + + def __init__(self, context=None): + super(PyConsoleWidget, self).__init__() + + _, package_path = get_resource('packages', 'rqt_py_console') + ui_file = os.path.join( + package_path, 'share', 'rqt_py_console', 'resource', 'py_console_widget.ui') + + loadUi(ui_file, self, {'PyConsoleTextEdit': PyConsoleTextEdit}) + self.setObjectName('PyConsoleWidget') + + my_locals = { + 'context': context + } + self.py_console.update_interpreter_locals(my_locals) + self.py_console.print_message( + 'The variable "context" is set to the PluginContext of this plugin.') + self.py_console.exit.connect(context.close_plugin) diff --git a/ground_control_station/ros_ws/src/rqt_py_template/src/rqt_py_template/spyder_console_widget.py b/ground_control_station/ros_ws/src/rqt_py_template/src/rqt_py_template/spyder_console_widget.py new file mode 100644 index 000000000..374ef7a5d --- /dev/null +++ b/ground_control_station/ros_ws/src/rqt_py_template/src/rqt_py_template/spyder_console_widget.py @@ -0,0 +1,60 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from python_qt_binding.QtGui import QFont + +from spyder.widgets.internalshell import InternalShell +from spyder.utils.module_completion import moduleCompletion + +class SpyderConsoleWidget(InternalShell): + + def __init__(self, context=None): + my_locals = { + 'context': context + } + super(SpyderConsoleWidget, self).__init__(namespace=my_locals) + self.setObjectName('SpyderConsoleWidget') + self.set_pythonshell_font(QFont('Mono')) + self.interpreter.restore_stds() + + def get_module_completion(self, objtxt): + """Return module completion list associated to object name""" + return moduleCompletion(objtxt) + + def run_command(self, *args): + self.interpreter.redirect_stds() + super(SpyderConsoleWidget, self).run_command(*args) + self.flush() + self.interpreter.restore_stds() + + def shutdown(self): + self.exit_interpreter() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/template.py b/ground_control_station/ros_ws/src/rqt_py_template/src/rqt_py_template/template.py similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/template.py rename to ground_control_station/ros_ws/src/rqt_py_template/src/rqt_py_template/template.py