scripts/idi.sh

@@ -0,0 +1,42 @@
+#!/bin/bash
+
+TARGET="$1"
+
+if [[ "$TARGET" == "-h" || "$TARGET" == "--help" ]]; then
+    exit 0
+fi
+
+if [[ $# -lt 1 ]]; then
+    echo "Error: Expected at least 1 arg"
+    echo "Available commands: napred, rotiraj, na"
+    exit 1
+fi
+
+if [[ $# -ge 2 ]]; then
+  SMER="$2"
+fi
+
+source install/setup.bash
+
+case $TARGET in
+    "na") 
+        ros2 action send_goal /moveCoords toid_msgs/action/SimpleMoveCoords "
+          x: 0.0
+          y: 0.0
+          theta: 0.0
+          backwards: ${SMER:-0}"
+        ;;
+    "pravo") 
+        ros2 action send_goal /translateX toid_msgs/action/SimpleTranslateX  "distance: ${SMER:-0.3}"
+        ;;
+    "stani") 
+        ros2 service call /moveCoords/_action/cancel_goal action_msgs/srv/CancelGoal &
+        ros2 service call /translateX/_action/cancel_goal action_msgs/srv/CancelGoal &
+        wait
+        ;;
+    *)
+        echo "Target not defined"
+        exit 1
+        ;;
+esac
+exit 0

toid_behaviors/src/move_coords.cpp

@@ -123,8 +123,8 @@ ResultStatus MoveCoords::updateVel(
   scl.calculate_vel(target_pose_, pose, out_vel, backwards_);
   last_speed_ = out_vel.linear.x;
 
-  RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left); 
-  RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl.v_linear_max); 
+  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left); 
+  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl.v_linear_max); 
 
   return ResultStatus{Status::RUNNING};
 }