new driving tool services

wvu-navLab · Sep 17, 2020 · 779c5f1 · 779c5f1
1 parent ed12277
commit 779c5f1
Show file tree

Hide file tree

Showing 5 changed files with 80 additions and 0 deletions.
diff --git a/driving_tools/CMakeLists.txt b/driving_tools/CMakeLists.txt
@@ -63,6 +63,8 @@ add_service_files(
   Stop.srv
   CirculateBaseStation.srv
   RotateInPlace.srv
+  TurnWheelsSideways.srv
+  MoveSideways.srv
 )
 
 ## Generate actions in the 'action' folder

diff --git a/driving_tools/include/driving_tools/driving_tools.h b/driving_tools/include/driving_tools/driving_tools.h
@@ -25,6 +25,8 @@
 #include <driving_tools/RotateInPlace.h>
 #include <driving_tools/CirculateBaseStation.h>
 #include <driving_tools/MoveForward.h>
+#include <driving_tools/TurnWheelsSideways.h>
+#include <driving_tools/MoveSideways.h>
 #include <std_msgs/Int64.h>
 
 // ConstantsS
@@ -80,6 +82,8 @@ class DrivingTools
     ros::ServiceServer rotateInPlaceServer;
     ros::ServiceServer circBaseStationServer;
     ros::ServiceServer moveForwardServer;
+    ros::ServiceServer turnWheelsSidewaysServer;
+    ros::ServiceServer moveSidewaysServer;
 
     // Publishers
     ros::Publisher pubDrivingMode;
@@ -97,6 +101,8 @@ class DrivingTools
     bool RotateInPlace(driving_tools::RotateInPlace::Request  &req, driving_tools::RotateInPlace::Response &res);
     bool CirculateBaseStation(driving_tools::CirculateBaseStation::Request  &req, driving_tools::CirculateBaseStation::Response &res);
     bool MoveForward(driving_tools::MoveForward::Request  &req, driving_tools::MoveForward::Response &res);
+    bool TurnWheelsSideways(driving_tools::TurnWheelsSideways::Request  &req, driving_tools::TurnWheelsSideways::Response &res);
+    bool MoveSideways(driving_tools::MoveSideways::Request  &req, driving_tools::MoveSideways::Response &res);
 };
 
 

diff --git a/driving_tools/src/driving_tools.cpp b/driving_tools/src/driving_tools.cpp
@@ -22,6 +22,8 @@ DrivingTools::DrivingTools()
   rotateInPlaceServer = nh_.advertiseService("driving/rotate_in_place", &DrivingTools::RotateInPlace, this);
   circBaseStationServer = nh_.advertiseService("driving/circ_base_station", &DrivingTools::CirculateBaseStation, this);
   moveForwardServer = nh_.advertiseService("driving/move_forward", &DrivingTools::MoveForward, this);
+  turnWheelsSidewaysServer = nh_.advertiseService("driving/turn_wheels_sideways", &DrivingTools::TurnWheelsSideways, this);
+  moveSidewaysServer = nh_.advertiseService("driving/move_sideways", &DrivingTools::MoveSideways, this);
 }
 
 bool DrivingTools::RotateInPlace(driving_tools::RotateInPlace::Request &req, driving_tools::RotateInPlace::Response &res)
@@ -138,6 +140,70 @@ bool DrivingTools::MoveForward(driving_tools::MoveForward::Request  &req, drivin
   return true;
 }
 
+
+bool DrivingTools::TurnWheelsSideways(driving_tools::TurnWheelsSideways::Request  &req, driving_tools::TurnWheelsSideways::Response &res)
+{
+  driving_mode_ = CRAB_MODE;
+
+  // ROS_INFO("Move Forward Service requested.");
+  // Rotate wheels at 45 deg
+  s1 = MAX_STEERING_ANGLE;
+  s2 = MAX_STEERING_ANGLE;
+  s3 = MAX_STEERING_ANGLE;
+  s4 = MAX_STEERING_ANGLE;
+
+  s.s1 = s1;
+  s.s2 = s2;
+  s.s3 = s3;
+  s.s4 = s4;
+
+  std_msgs::Int64 mode;
+  mode.data = driving_mode_;
+  pubDrivingMode.publish(mode);
+  pubSteeringEfforts.publish(s);
+
+  res.success = true;
+  return true;
+}
+
+bool DrivingTools::MoveSideways(driving_tools::MoveSideways::Request  &req, driving_tools::MoveSideways::Response &res)
+{
+  driving_mode_ = DACK_MODE;
+
+  // ROS_INFO("Move Forward Service requested.");
+  // Rotate wheels at 45 deg
+  s1 = MAX_STEERING_ANGLE;
+  s2 = MAX_STEERING_ANGLE;
+  s3 = MAX_STEERING_ANGLE;
+  s4 = MAX_STEERING_ANGLE;
+
+  s.s1 = s1;
+  s.s2 = s2;
+  s.s3 = s3;
+  s.s4 = s4;
+
+  double throttle = req.throttle;
+  // Grab the directional data
+  m1 = throttle * MAX_MOTOR_EFFORT;
+  m2 = throttle * MAX_MOTOR_EFFORT;
+  m3 = throttle * MAX_MOTOR_EFFORT;
+  m4 = throttle * MAX_MOTOR_EFFORT;
+
+  m.m1 = m1;
+  m.m2 = m2;
+  m.m3 = m3;
+  m.m4 = m4;
+
+  std_msgs::Int64 mode;
+  mode.data = driving_mode_;
+  pubDrivingMode.publish(mode);
+  pubMotorEfforts.publish(m);
+  pubSteeringEfforts.publish(s);
+
+  res.success = true;
+  return true;
+}
+
 bool DrivingTools::Stop(driving_tools::Stop::Request  &req, driving_tools::Stop::Response &res)
 {
   driving_mode_ = STOP_MODE;

diff --git a/driving_tools/srv/MoveSideways.srv b/driving_tools/srv/MoveSideways.srv
@@ -0,0 +1,3 @@
+float64 throttle
+---
+bool success
diff --git a/driving_tools/srv/TurnWheelsSideways.srv b/driving_tools/srv/TurnWheelsSideways.srv
@@ -0,0 +1,3 @@
+bool start
+---
+bool success