Merge pull request #190 from utiasASRL/waypoint_improvement

Waypoint improvement

Dockerfile

@@ -28,7 +28,11 @@ ENV VTRROOT=${HOMEDIR}/ASRL/vtr3
 ENV VTRSRC=${VTRROOT}/src \
   VTRDATA=${VTRROOT}/data \
   VTRTEMP=${VTRROOT}/temp \
-  GRIZZLY=${VTRROOT}/grizzly
+  GRIZZLY=${VTRROOT}/grizzly \
+  VTRUI=${VTRSRC}/main/src/vtr_gui/vtr_gui/vtr-gui
+
+RUN echo "alias build_ui='npm --prefix ${VTRUI} install ${VTRUI}; npm --prefix ${VTRUI} run build'" >> ~/.bashrc
+RUN echo "alias build_vtr='source /opt/ros/humble/setup.bash; cd ${VTRSRC}/main; colcon build --symlink-install'" >> ~/.bashrc
 
 ## Switch to root to install dependencies
 USER 0:0
@@ -42,6 +46,7 @@ RUN apt update && apt install -q -y freeglut3-dev
 RUN apt update && apt install -q -y python3 python3-distutils python3-pip
 RUN apt update && apt install -q -y libeigen3-dev
 RUN apt update && apt install -q -y libsqlite3-dev sqlite3
+RUN apt install -q -y libc6-dbg gdb valgrind
 
 ## Install PROJ (8.2.0) (this is for graph_map_server in vtr_navigation)
 RUN apt update && apt install -q -y cmake libsqlite3-dev sqlite3 libtiff-dev libcurl4-openssl-dev
@@ -108,7 +113,6 @@ RUN apt install ros-humble-velodyne -q -y
 
 # Install vim
 RUN apt update && apt install -q -y vim
-RUN apt install -q -y libc6-dbg gdb valgrind
 
 ## Switch to specified user
 USER ${USERID}:${GROUPID}

launch/offline_honeycomb_grizzly.launch.yaml

@@ -40,5 +40,5 @@ windows:
     shell_command_before:
       - source /opt/ros/humble/setup.bash
     panes:
-      - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz
+      - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/honeycomb.rviz
       # - ros2 run rqt_reconfigure rqt_reconfigure

main/src/vtr_common/vtr_include.cmake

@@ -18,6 +18,7 @@ add_compile_options(-march=native -O3 -pthread -Wall -Wextra)
 # address sanitizer
 # add_compile_options(-fsanitize=address)
 # set(CMAKE_CXX_STANDARD_LIBRARIES -lasan)
+# add_compile_options(-g -Og)
 
 
 ## Common packages setup

main/src/vtr_gui/vtr_gui/socket_client.py

@@ -21,7 +21,7 @@
 from vtr_navigation.vtr_ui_builder import build_master
 
 from vtr_tactic_msgs.msg import EnvInfo
-from vtr_navigation_msgs.msg import MoveGraph, AnnotateRoute
+from vtr_navigation_msgs.msg import MoveGraph, AnnotateRoute, UpdateWaypoint
 from vtr_navigation_msgs.msg import MissionCommand, ServerState, GoalHandle
 
 # socket io server address and port
@@ -41,6 +41,7 @@ def graph_state_from_ros(ros_graph_state):
           'lat': v.lat,
           'theta': v.theta,
           'type': v.type,
+          'name': v.name
       } for v in ros_graph_state.vertices],
       'fixed_routes': [{
           'ids': [id for id in r.ids],
@@ -64,6 +65,7 @@ def graph_update_from_ros(ros_graph_update):
           'lat': vf.lat,
           'theta': vf.theta,
           'type': vf.type,
+          'name': vf.name
       },
       'vertex_to': {
           'id': vt.id,
@@ -72,6 +74,7 @@ def graph_update_from_ros(ros_graph_update):
           'lat': vt.lat,
           'theta': vt.theta,
           'type': vt.type,
+          'name': vt.name
       },
   }
 
@@ -244,6 +247,11 @@ def cancel_goal(self, data):
     ros_command.type = MissionCommand.CANCEL_GOAL
     ros_command.goal_handle.id = [int(id) for id in data['id']]
     return super().cancel_goal(ros_command)
+
+  def begin_goals(self):
+    ros_command = MissionCommand()
+    ros_command.type = MissionCommand.BEGIN_GOALS
+    return super().begin_goals(ros_command)
 
   def move_robot(self, data):
     ros_command = MissionCommand()
@@ -266,6 +274,16 @@ def continue_teach(self, _):
     ros_command = MissionCommand()
     ros_command.type = MissionCommand.CONTINUE_TEACH
     return super().continue_teach(ros_command)
+
+  def update_waypoint(self, data):
+    ros_waypoint_update = UpdateWaypoint()
+    ros_waypoint_update.vertex_id = int(data['vertex_id'])
+    ros_waypoint_update.type = int(data['type'])
+    if int(data['type']) == UpdateWaypoint.ADD:
+      ros_waypoint_update.name = data['name']
+    else:
+      ros_waypoint_update.name = ""
+    return super().update_waypoint(ros_waypoint_update)
 
   def annotate_route(self, data):
     ros_annotate_route = AnnotateRoute()

main/src/vtr_gui/vtr_gui/socket_server.py

@@ -66,7 +66,7 @@ def on_disconnect():
 
 @socketio.on('command/set_pause')
 def handle_set_pause(data):
-  logger.info('Received add goal command', data)
+  logger.info('Received set pause command', data)
   build_remote().set_pause(data)
 
 
@@ -82,6 +82,12 @@ def handle_cancel_goal(data):
   build_remote().cancel_goal(data)
 
 
+@socketio.on('command/begin_goals')
+def handle_begin_goals():
+  logger.info('Received begin goals command')
+  build_remote().begin_goals()
+
+
 @socketio.on('command/merge')
 def handle_merge(data):
   logger.info('Received merge command', data)
@@ -100,6 +106,12 @@ def handle_continue_teach(data):
   build_remote().continue_teach(data)
 
 
+@socketio.on('command/update_waypoint')
+def handle_update_waypoint(data):
+  logger.info('Received update waypoint command', data)
+  build_remote().update_waypoint(data)
+
+
 @socketio.on('command/annotate_route')
 def handle_annotate_route(data):
   logger.info('Received annotate route command', data)

main/src/vtr_gui/vtr_gui/vtr-gui/src/components/goal/GoalCard.js

@@ -67,15 +67,15 @@ class GoalCard extends React.Component {
             </Typography>
             <Button
               sx={{ width: 100, m: 1 }}
-              color={"primary"}
+              color={"secondary"}
               disableElevation={true}
               fullWidth={true}
               size="small"
               startIcon={<ClearIcon />}
               variant={"contained"}
               onClick={() => cancelGoal(goal)}
             >
-              Cancel
+              CANCEL
             </Button>
           </Box>
           {expand && goal.type === "repeat" && (

main/src/vtr_gui/vtr_gui/vtr-gui/src/components/goal/GoalForm.js

@@ -38,11 +38,12 @@ class GoalForm extends React.Component {
 
   componentDidUpdate(prevProps) {
     // Update goal_waypoints_str if goalWaypoints is changed by either this or GraphMap.
-    if (prevProps.goalWaypoints !== this.props.goalWaypoints) this.convertGoalWaypoints(this.props.goalWaypoints);
+    if (prevProps.goalWaypoints !== this.props.goalWaypoints || prevProps.waypointsMap !== this.props.waypointsMap) 
+    this.updateGoalWaypointsStr(this.props.goalWaypoints);
   }
 
   render() {
-    const { goalType, goalWaypoints, setGoalWaypoints } = this.props;
+    const { waypointsMap, goalType, goalWaypoints, setNewGoalWaypoints } = this.props;
     const { goal_form_open, goal_waypoints_str, goal_waypoints_invalid, pause_after, pause_before } = this.state;
     return (
       <>
@@ -115,8 +116,10 @@ class GoalForm extends React.Component {
                     variant="standard"
                     size="small"
                     error={goal_waypoints_invalid}
-                    onChange={(e) => this.setState({ goal_waypoints_str: e.target.value })}
-                    onKeyPress={this.parseGoalWaypoints.bind(this)}
+                    onChange={(e) => {
+                      this.setState({ goal_waypoints_str: e.target.value });
+                      this.parseGoalWaypoints(e);
+                    }}
                     value={goal_waypoints_str}
                   />
                   <IconButton
@@ -125,13 +128,13 @@ class GoalForm extends React.Component {
                     onClick={() => {
                       if (goalWaypoints.length > 0) {
                         let new_goal_waypoints = goalWaypoints.slice(0, goalWaypoints.length - 1);
-                        setGoalWaypoints(new_goal_waypoints.map((p) => p.id));
+                        setNewGoalWaypoints(new_goal_waypoints);
                       }
                     }}
                   >
                     <ArrowBackIcon />
                   </IconButton>
-                  <IconButton color="secondary" size="small" onClick={() => setGoalWaypoints([])}>
+                  <IconButton color="secondary" size="small" onClick={() => setNewGoalWaypoints([])}>
                     <ClearIcon />
                   </IconButton>
                 </Box>
@@ -147,7 +150,7 @@ class GoalForm extends React.Component {
                 size="small"
                 startIcon={<CheckIcon />}
                 variant={"contained"}
-                onClick={this.submitGoal.bind(this)}
+                onClick={this.addGoal.bind(this)}
               >
                 Confirm
               </Button>
@@ -186,7 +189,7 @@ class GoalForm extends React.Component {
   toggleGoalForm(enable) {
     // clear existing goal
     this.props.setGoalType("");
-    this.props.setGoalWaypoints([]);
+    this.props.setNewGoalWaypoints([]);
     this.setState({
       goal_form_open: enable ? true : false,
       goal_waypoints_invalid: false,
@@ -202,7 +205,7 @@ class GoalForm extends React.Component {
    */
   selectGoalType(type) {
     this.props.setGoalType(type);
-    this.props.setGoalWaypoints([]);
+    this.props.setNewGoalWaypoints([]);
     this.setState({ goal_waypoints_invalid: false, goal_waypoints_str: "", pause_after: "", pause_before: "" });
   }
 
@@ -220,32 +223,39 @@ class GoalForm extends React.Component {
 
   /** @brief Selects repeat waypoints. */
   parseGoalWaypoints(e) {
-    if (e.key === "Enter") {
-      let input = e.target.value;
-      let ids_str = input.replace(/ /g, "").split(",");
-      let ids = [];
-      for (let id of ids_str) {
-        let idpair = id.split("-");
-        for (let i of idpair) if (isNaN(i)) continue;
+    let input = e.target.value;
+    let names_lst = input.split(", ");
+    let ids = [];
 
-        if (idpair.length === 1) ids.push(parseInt(idpair[0]));
-        else if (idpair.length === 2) ids.push(parseInt(idpair[0]) * Math.pow(2, 32) + parseInt(idpair[1]));
-      }
-      this.props.setGoalWaypoints(ids);
+    if (input == "") {
+      this.props.setNewGoalWaypoints([]);
       this.setState({ goal_waypoints_invalid: false });
-      e.preventDefault();
-    } else {
-      this.setState({ goal_waypoints_invalid: true });
+      return;
     }
+
+    for (let name of names_lst) {
+      if (Array.from(this.props.waypointsMap.values()).includes(name)){
+        ids.push(
+          Array.from(this.props.waypointsMap.keys()).find(
+            (key) => this.props.waypointsMap.get(key) === name
+          )
+        );
+      }
+      else {
+        this.setState({ goal_waypoints_invalid: true });
+        return;
+      }
+    }
+
+    this.props.setNewGoalWaypoints(ids);
+    this.setState({ goal_waypoints_invalid: false });
   }
 
   /** @brief Parses repeat waypoints and generate a user readable string. */
-  convertGoalWaypoints(goal_waypoints) {
+  updateGoalWaypointsStr(goal_waypoints) {
     let s = "";
     goal_waypoints.forEach((id) => {
-      let vl = parseInt(id % Math.pow(2, 32));
-      let vh = parseInt((id - vl) / Math.pow(2, 32));
-      s += vh.toString() + "-" + vl.toString() + ", ";
+      if (this.props.waypointsMap.has(id)) s += this.props.waypointsMap.get(id) + ", ";
     });
     s = s.slice(0, s.length - 2);
     this.setState({
@@ -254,22 +264,22 @@ class GoalForm extends React.Component {
   }
 
   /** @brief Calls GoalManager to submit the goal */
-  submitGoal() {
+  addGoal() {
     this.setState(
       (state, props) => {
         let goal = {
           type: props.goalType,
           pause_before: Number(state.pause_before),
           pause_after: Number(state.pause_after),
-          waypoints: props.goalWaypoints,
+          waypoints: props.goalWaypoints
         };
-        console.debug("Submit new goal:", goal);
+        console.debug("Create new goal:", goal);
         props.socket.emit("command/add_goal", goal);
         return { goal_waypoints_invalid: false, goal_waypoints_str: "", pause_after: "", pause_before: "" };
       },
       () => {
         this.props.setGoalType("");
-        this.props.setGoalWaypoints([]);
+        this.props.setNewGoalWaypoints([]);
       }
     );
   }