diff --git a/scripts/idi.sh b/scripts/idi.sh new file mode 100755 index 0000000..5544f36 --- /dev/null +++ b/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 diff --git a/toid_behaviors/src/move_coords.cpp b/toid_behaviors/src/move_coords.cpp index 4a2ebef..7a6a259 100644 --- a/toid_behaviors/src/move_coords.cpp +++ b/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}; }