Feature implement move (#285)

* Revert build

* Add move msgs and action

* Delete unused files

* Add move

* change nav params

* delete strategy

* implmenet translate action

* try fix

* fix

* fix

* Reimplement motion behavior

* Bug fix in ROTATE TOWARD GOAL behavior

* Add angular velocity for move behavior

* fix

* fix simulation th]eme and add navigation node (#287)

* fix merge confilic

* fix move param

* test

* fix ax

---------

Co-authored-by: Darko Lukic <[email protected]>

docker/Makefile

@@ -14,6 +14,13 @@ vnc:
 	$(eval IMAGE=ghcr.io/memristor/mep3-vnc)
 	@true
 
+build:
+	echo ${NO_CACHE_ARG}
+	@DOCKER_BUILDKIT=1 docker build ${DOCKER_DIR} -f ${DOCKER_DIR}/Dockerfile.base -t mep3 ${DOCKER_ARGS} --build-arg UID=${UID}
+	@[ ${FLAVOR} != 'devel' ] && \
+		DOCKER_BUILDKIT=1 docker build ${DOCKER_DIR} -f ${DOCKER_DIR}/Dockerfile.${FLAVOR} -t ${IMAGE} ${DOCKER_ARGS} || \
+		true
+
 run:
 	@${MAKE} -s test-nvidia
 	@docker run \

mep3_behavior/include/mep3_behavior/joint_position_command_action.hpp

@@ -60,10 +60,10 @@ namespace mep3_behavior
 
     bool setGoal(Goal &goal) override
     {
-      std::cout << "Dynamixel desired position to θ=" << position_ \
-        <<" max_velocity="<<max_velocity_\
-        <<" max effort="<<max_effort_\
-        <<" max_acceleration="<<max_acceleration_<< std::endl;
+      std::cout << "Dynamixel desired position to θ=" << position_
+                << " max_velocity=" << max_velocity_
+                << " max effort=" << max_effort_
+                << " max_acceleration=" << max_acceleration_ << std::endl;
 
       goal.position = position_ * M_PI / 180;
       goal.max_velocity = max_velocity_ * M_PI / 180;
@@ -98,12 +98,12 @@ namespace mep3_behavior
       return port_list;
     }
 
-    BT::NodeStatus onResultReceived(const WrappedResult & wr) override
+    BT::NodeStatus onResultReceived(const WrappedResult &wr) override
     {
       setOutput("result", (double)wr.result->result);
       setOutput("feedback_effort", wr.result->last_effort);
       setOutput("feedback_position", wr.result->last_position);
-      std::cout << "Last result: " << (double)wr.result->result << "; last effort: " << (double)wr.result->last_effort << "; last position: " <<  (double)wr.result->last_position << std::endl;
+      std::cout << "Last result: " << (double)wr.result->result << "; last effort: " << (double)wr.result->last_effort << "; last position: " << (double)wr.result->last_position << std::endl;
 
       return BT::NodeStatus::SUCCESS;
     }
@@ -112,7 +112,7 @@ namespace mep3_behavior
     {
 
       setOutput("feedback_effort", feedback->effort);
-      std::cout << "====Max effort: " << feedback->effort <<std::endl;
+      std::cout << "====Max effort: " << feedback->effort << std::endl;
       setOutput("feedback_position", feedback->position);
 
       return BT::NodeStatus::RUNNING;