Super basic gantry model and subscriber

mercury-app/src/hw_ifc/CMakeLists.txt

@@ -151,7 +151,7 @@ include_directories(
 
 add_custom_target(hw_ifc
 	ALL
-	COMMAND cargo build -p mercury --bin drive_sub #--release
+	COMMAND cargo build -p mercury --release
   WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/../src/hw_ifc
 	#COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_BINARY_DIR}/cargo/release/my-project-binary ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/my-project-binary
 	COMMENT "Build Mercury..."

mercury-app/src/hw_ifc/src/gantry_model.rs

@@ -1,22 +1,98 @@
-pub struct DriveMotorModel {
-	wheel_base: f64,
-	wheel_radius: f64,
+/* Model Describing the Behavior of the Gantry */
+
+use std::ops::{Index, IndexMut};
+
+pub enum GantryAxes {
+    GantryX,
+    GantryY,
+    GantryZ,
+}
+
+pub struct StepperCtrlCmd {
+    pub steps: i32,
+    speed: f64,
+}
+
+pub struct StepperCtrlCmdGroup {
+    x: StepperCtrlCmd,
+    y: StepperCtrlCmd,
+    z: StepperCtrlCmd,
+}
+
+impl Index<GantryAxes> for StepperCtrlCmdGroup {
+    type Output = StepperCtrlCmd;
+
+    fn index(&self, side: GantryAxes) -> &Self::Output {
+        match side {
+            GantryAxes::GantryX => &self.x,
+            GantryAxes::GantryY => &self.y,
+            GantryAxes::GantryZ => &self.z,
+        }
+    }
 }
 
-impl DriveMotorModel {
-    pub fn new(wheel_base : f64, wheel_radius : f64) -> DriveMotorModel{
-        DriveMotorModel {
-            wheel_base,
-            wheel_radius
+impl IndexMut<GantryAxes> for StepperCtrlCmdGroup {
+    fn index_mut(&mut self, side: GantryAxes) -> &mut Self::Output {
+        match side {
+            GantryAxes::GantryX => &mut self.x,
+            GantryAxes::GantryY => &mut self.y,
+            GantryAxes::GantryZ => &mut self.z,
         }
     }
+}
+
+pub struct GantryPosition {
+    x: f64,
+    y: f64,
+    z: f64,
+}
 
-    /* Immutable access */
-    pub fn wheel_base(&self) -> &f64 {
-        &self.wheel_base
+pub struct GantryModel {
+    current_position: GantryPosition,
+}
+
+impl GantryModel {
+    pub fn new() -> GantryModel {
+        GantryModel {
+            current_position: GantryPosition {
+                x: 0.0,
+                y: 0.0,
+                z: 0.0,
+            }
+        }
     }
 
-    pub fn wheel_radius(&self) -> &f64 {
-        &self.wheel_radius
+    pub fn set_position(&mut self, pos: GantryPosition) {
+        self.current_position = pos;
+    }
+
+    pub fn calc_control_signals(
+        &mut self,
+        target_position: GantryPosition,
+        max_speed: f64,
+    ) -> StepperCtrlCmdGroup {
+        // assuming 1 unit = 1 step for simplicity
+        let x_steps = (target_position.x - self.current_position.x) as i32;
+        let y_steps = (target_position.y - self.current_position.y) as i32;
+        let z_steps = (target_position.z - self.current_position.z) as i32;
+
+        // Update current position
+        self.current_position = target_position;
+
+        // For now, we'll set the same speed for all axes, but this could be adjusted based on the axis and specific requirements
+        StepperCtrlCmdGroup {
+            x: StepperCtrlCmd {
+                steps: x_steps,
+                speed: max_speed,
+            },
+            y: StepperCtrlCmd {
+                steps: y_steps,
+                speed: max_speed,
+            },
+            z: StepperCtrlCmd {
+                steps: z_steps,
+                speed: max_speed,
+            },
+        }
     }
 }

mercury-app/src/hw_ifc/src/gantry_sub.rs

@@ -1,110 +1,59 @@
 /* Subscriber for Carrying out Commands for Chassis Drive Motors */
-
 use rosrust;
-use rosrust_msg::geometry_msgs::Twist;
-use std::sync::RwLock;
+pub mod gantry_model;
+use self::gantry_model::{
+    GantryAxes, GantryModel, GantryPosition, StepperCtrlCmd, StepperCtrlCmdGroup,
+};
 
 pub mod msg {
-    rosrust::rosmsg_include!(gantry/calibrate);
+    rosrust::rosmsg_include!(hw_srv/calibrate);
 }
 
-
 pub struct GantryController {
-	model : GantryModel,
-	max_linear_vel: f64,	/* meters per second  */
-	max_angular_vel: f64,	/* radians per second  */
-	velocities: RwLock<(f64, f64)>, /* Left and right velocities */
+    model: GantryModel,
 }
 
 impl GantryController {
-	pub fn new() -> Self
-	{
-		// Parameters
-		let wheel_base = rosrust::param("~wheel_base").unwrap().get().unwrap_or(0.2);
-		let wheel_radius = rosrust::param("~wheel_radius").unwrap().get().unwrap_or(0.095);
-		let max_velocity_meters_per_second = rosrust::param("~max_velocity_meters_per_second").unwrap().get().unwrap_or(10.0);
-		let max_angular_velocity_rad_per_second = rosrust::param("~max_angular_velocity_rad_per_second").unwrap().get().unwrap_or(6.0);
-
-		GantryController {
-			model : GantryModel::new(wheel_base, wheel_radius),
-			max_linear_vel : max_velocity_meters_per_second,
-			max_angular_vel : max_angular_velocity_rad_per_second,
-			velocities: RwLock::new((0.0, 0.0)),
+    pub fn new() -> Self {
+        GantryController {
+			model: GantryModel::new(),
 		}
-	}
-
-	fn clamp_linear_speed(&self, speed: f64) -> f64
-	{
-		speed.max(-self.max_linear_vel)
-			 .min(0.0)
-	}
-
-	fn clamp_angular_speed(&self, speed: f64) -> f64
-	{
-		speed.max(-self.max_angular_vel)
-			 .min(0.0)
-	}
-
-	fn send_motor_commands(&self)
-	{
-		let velocities = self.velocities.read().unwrap();
-
-		// TODO Make hardware call
-
-		rosrust::ros_info!("Left Vel/Command: {}, Right Vel/Command: {}", velocities.0, velocities.1);
-	}
-
-	pub fn command_callback(&self, data: Twist)
-	{
-		let linear_velocity = self.clamp_linear_speed(data.linear.x);
-		let angular_velocity = self.clamp_angular_speed(data.angular.z);
+    }
 
-		let left_velocity = linear_velocity - (angular_velocity * self.model.wheel_base() / 2.0);
-		let right_velocity = linear_velocity + (angular_velocity * self.model.wheel_base() / 2.0);
-
-		// Store the velocities safely
-		let mut velocities = self.velocities.write().unwrap();
-		*velocities = (left_velocity, right_velocity);
-		self.send_motor_commands();
-	}
+    pub fn command_callback(&self, data: rosrust_msg::hw_srv::calibrate) {}
 }
 
-fn main()
-{
-	/* Initialize ROS node */
-	rosrust::init("gantry_sub");
-
-	let gantry_ctrl = GantryController::new();
-
-	/*
-	 * Create subscriber
-	 */
-	let _subscriber_info = rosrust::subscribe_with_ids(
-		"/gantry/pose/goal",
-		2,
-		move |v: Twist, _caller_id: &str| {
-			gantry_ctrl.command_callback(v);
-		}
-	)
-	.unwrap();
-
-	/**
-	 * Create service
-	 */
-	let _service_raii = rosrust::service::<msg::gantry::calibrate, _>("/services/gantry/calibrate", move |req| {
-		gantry_ctrl.command_callback(req);
-
-		Ok(msg::gantry::calibrate { sum })
-	})
-	.unwrap();
-
-	let log_names = rosrust::param("~log_names").unwrap()
-										 .get()
-										 .unwrap_or(false);
-	if log_names {
-		while rosrust::is_ok() {
-			/* Spin forever, we only execute things on callbacks from here */
-			rosrust::spin();
-		}
-	}
-}
+fn main() {
+    /* Initialize ROS node */
+    rosrust::init("gantry_sub");
+
+    let gantry_ctrl = GantryController::new();
+
+    /*
+     * Create subscriber
+     */
+    //let _subscriber_info = rosrust::subscribe_with_ids(
+    //    "/gantry/pose/goal",
+    //    2,
+    //    move |v: rosrust_msg::hw_srv::calibrate, _caller_id: &str| {
+    //        gantry_ctrl.command_callback(v);
+    //    },
+    //)
+    //.unwrap();
+
+    /**
+     * Create service
+     */
+    //let _service_raii =
+    //    rosrust::service::<rosrust_msg::hw_srv::calibrate, _>("/services/gantry/calibrate", move |req| {
+    //        gantry_ctrl.command_callback(req);
+//
+    //        Ok(rosrust_msg::hw_srv::calibrate { })
+    //    })
+    //    .unwrap();
+
+    while rosrust::is_ok() {
+        /* Spin forever, we only execute things on callbacks from here */
+        rosrust::spin();
+    }
+}

mercury-app/src/hw_srv/srv/calibrate.srv

@@ -0,0 +1,2 @@
+bool status
+---