Construct interface

README.md

@@ -13,7 +13,7 @@ mkdir -p roboy_ws/src
 cd roboy_ws/src
 git clone https://github.com/Roboy/roboy3_models.git -b bullet
 git clone https://github.com/Roboy/roboy_communication.git -b dashing
-git clone https://github.com/Roboy/bulletroboy.git
+git clone https://github.com/Roboy/bulletroboy.git -b exoforce
 
 # Building packages in workspace
 cd roboy_ws
@@ -34,10 +34,7 @@ Run the system with the operator simulation.
 ros2 launch bulletroboy cage_sim.launch.py  # use -s to list arguments
 ```
 
-Run the system with the real exoforce cage. 
-(Each command must be executed in a new terminal. Operator and exoforce nodes require interaction through the terminal and cannot be added to the launch file yet.)
+Run the system with the real exoforce cage.
 ```bash
 ros2 launch bulletroboy exoforce.launch.py # use -s to list arguments
-ros2 run bulletroboy operator
-ros2 run bulletroboy exoforce
 ```

bulletroboy/exoforce/exoforce.py

@@ -7,10 +7,9 @@
 from xml.dom import minidom
 
 from rclpy.node import Node
-from roboy_control_msgs.msg import CageState, EndEffector as EndEffectorMsg, ViaPoint as ViaPointMsg, MuscleUnit as MuscleUnitMsg
+from roboy_control_msgs.msg import EFPose, CageState, EndEffector as EndEffectorMsg, ViaPoint as ViaPointMsg, MuscleUnit as MuscleUnitMsg
 from roboy_control_msgs.srv import GetCageEndEffectors
 from roboy_simulation_msgs.msg import Collision
-from geometry_msgs.msg import PoseStamped
 from geometry_msgs.msg import Point
 
 from ..utils.force_decomposition import decompose_force_ef_to_tendons
@@ -369,7 +368,7 @@ def __init__(self, cage_conf, node_name):
 		self.cage_state_publisher = self.create_publisher(CageState, Topics.CAGE_STATE, 1)
 		self.initial_conf_service = self.create_service(GetCageEndEffectors, Topics.CAGE_END_EFFECTORS, self.get_end_effectors_callback)
 
-		self.create_subscription(PoseStamped, Topics.OP_EF_POSES, self.operator_ef_pos_listener, 1)
+		self.create_subscription(EFPose, Topics.OP_EF_POSES, self.operator_ef_pos_listener, 1)
 
 	def init_force_decomp_params(self):
 		"""Reads force decomposition params.
@@ -528,21 +527,21 @@ def operator_ef_pos_listener(self, ef_pose):
 		"""Callback of the pose subscriber. Sets the pose of the end effector given in the msg.
 
 		Args:
-			ef_pose: received PoseStamped msg.
+			ef_pose: received EFPose msg.
 
 		Returns:
 			-
 
 		"""
-		ef_name = ef_pose.header.frame_id
+		ef_name = ef_pose.ef_name
 		#self.get_logger().info("Received pose for " + ef_name)
 		end_effector = self.get_ef_name(ef_name)
 		if end_effector is None:
 			self.get_logger().warn(ef_name + " is not an end effector!")
 			return
 
-		link_pos = np.array([ef_pose.pose.position.x, ef_pose.pose.position.y, ef_pose.pose.position.z])
-		link_orn = np.array([ef_pose.pose.orientation.x, ef_pose.pose.orientation.y, ef_pose.pose.orientation.z, ef_pose.pose.orientation.w])
+		link_pos = np.array([ef_pose.ef_pose.position.x, ef_pose.ef_pose.position.y, ef_pose.ef_pose.position.z])
+		link_orn = np.array([ef_pose.ef_pose.orientation.x, ef_pose.ef_pose.orientation.y, ef_pose.ef_pose.orientation.z, ef_pose.ef_pose.orientation.w])
 
 		end_effector.position = link_pos
 		end_effector.orientation = link_orn

bulletroboy/exoforce/exoforce_hardware.py

@@ -1,9 +1,12 @@
 import time
+from functools import partial
 
 import numpy as np
-from geometry_msgs.msg import PoseStamped
 from pyquaternion import Quaternion
+from roboy_middleware_msgs.msg import ExoforceResponse, InitExoforceRequest
+from roboy_middleware_msgs.srv import InitExoforce
 from roboy_simulation_msgs.msg import Collision, TendonUpdate
+from std_msgs.msg import Empty
 from std_srvs.srv import Trigger
 
 from ..utils.utils import Services, Topics, call_service_async
@@ -27,42 +30,91 @@ def __init__(self, cage_conf):
 				('max_force_apply_time', 1.0)]
 			)
 
+		# state variables
+		self.active = False
+		self.processing_flag = False
+
+		# Initial parameters
 		self.no_slack_force = self.get_parameter("no_slack_force").get_parameter_value().double_value
 		self.tendon_init_time = self.get_parameter("tendon_init_time").get_parameter_value().double_value
 		self.max_force_apply_time = self.get_parameter("max_force_apply_time").get_parameter_value().double_value
 
-		self.active = False
+		# Force timer
 		self.apply_min_force_timer = self.create_timer(self.max_force_apply_time, lambda: self.set_target_force(self.no_slack_force))
 		self.apply_min_force_timer.cancel()
 
+		# Collision subscription
 		self.create_subscription(Collision, Topics.MAPPED_COLLISIONS, self.collision_listener, 1)
 		self.create_subscription(Collision, 'roboy/simulation/roboy/collision_hw', self.collision_listener, 1)
-
+
+		# Target force publisher
 		self.target_force_publisher = self.create_publisher(TendonUpdate, Topics.TARGET_FORCE, 1)
 
+		# Force Control node start/stop clients
 		self.start_force_control_client = self.create_client(Trigger, Services.START_FORCE_CONTROL)
 		self.stop_force_control_client = self.create_client(Trigger, Services.STOP_FORCE_CONTROL)
 
-		self.start_exoforce()
+		# Init operator client
+		self.init_operator_client = self.create_client(InitExoforce, Services.INIT_OPERATOR)
 
-	def start_exoforce(self):
-		"""Starts exoforce.
+		# Start exoforce topics
+		self.create_subscription(InitExoforceRequest, Topics.INIT_EXOFORCE_REQ, self.start_exoforce, 1)
+		self.init_response_publisher = self.create_publisher(ExoforceResponse, Topics.INIT_EXOFORCE_RES, 1)
+
+		# Stop exoforce topics
+		self.create_subscription(Empty, Topics.STOP_EXOFORCE_REQ, self.stop_exoforce, 1)
+		self.stop_response_publisher = self.create_publisher(ExoforceResponse, Topics.STOP_EXOFORCE_RES, 1)
+
+	def send_exoforce_response(self, publisher, success, message=""):
+		"""Sends ExoforceResponse msg through response topic.
 
 		Args:
+			publisher (Publisher): Response publisher.
+			success (bool): Success msg data.
+			message (string): Message msg data.
+
+		Returns:
 			-
 
+		"""
+		msg = ExoforceResponse()
+		msg.success = success
+		msg.message = message
+		publisher.publish(msg)
+		self.processing_flag = False
+
+	def start_exoforce(self, init_msg):
+		"""Starts exoforce.
+
+		Args:
+			init_msg (InitExoforceRequest): Initialization message.
+
 		Returns:
 			-
 
 		"""
+		if self.processing_flag:
+			return
+
+		self.processing_flag = True
 		self.get_logger().info("Starting Exoforce...")
-		call_service_async(self.start_force_control_client, Trigger.Request(), self.start_exoforce_callback, self.get_logger())
 
-	def start_exoforce_callback(self, future):
+		for ef_name in init_msg.ef_name:
+			if ef_name not in [ef.name for ef in self.end_effectors]:
+				err_msg = f"{ef_name} is not a cage end effector!"
+				self.get_logger().error(f"Cannot start the Exoforce: {err_msg}")
+				self.send_exoforce_response(self.init_response_publisher, False, err_msg)
+				return
+
+		self.get_logger().info("Requesting Force Control node to start...")
+		call_service_async(self.start_force_control_client, Trigger.Request(), partial(self.start_exoforce_callback, init_msg), self.get_logger())
+
+	def start_exoforce_callback(self, init_msg, future):
 		"""Start exoforce callback.
 
 		Args:
-			-
+			init_msg (InitExoforceRequest): Initialization message.
+			future: Service future var.
 
 		Returns:
 			-
@@ -74,16 +126,68 @@ def start_exoforce_callback(self, future):
 			self.get_logger().error(f"{self.start_force_control_client.srv_name} service call failed {e}")
 		else:
 			if not result.success:
-				self.get_logger().error(f"Failed to start force control node: {result.message}")
-				self.get_logger().error(f"Failed to start Exoforce!")
+				err_msg = f"Failed to start force control node: {result.message}"
+				self.get_logger().error(f"Failed to start Exoforce: {err_msg}")
+				self.send_exoforce_response(self.init_response_publisher, False, err_msg)
 			else:
 				self.get_logger().info("Force Control node started.")
+
+				# Applying no slack force to all tendons
 				self.set_target_force(self.no_slack_force)
+
+				# Requesting operator node to start
+				self.init_operator(init_msg)
+
+				# Sending initialization response
+				self.get_logger().info("Exoforce succesfully started.")
+				self.send_exoforce_response(self.init_response_publisher, True)
+
+				# Waiting for pulling slack time
 				time.sleep(self.tendon_init_time)
+
+				# Activating exoforce
 				self.active = True
-				self.get_logger().info("Exoforce succesfully started.")
 
-	def stop_exoforce(self):
+	def init_operator(self, init_msg):
+		"""Inits operator node.
+
+		Args:
+			init_msg (InitExoforceRequest): Initialization message.
+
+		Returns:
+			-
+
+		"""
+		self.get_logger().info("Requesting operator node to initialize...")
+
+		request = InitExoforce.Request(ef_name=init_msg.ef_name,
+										ef_enabled=init_msg.ef_enabled,
+										ef_init_pose=init_msg.ef_init_pose,
+										operator_height=init_msg.operator_height)
+
+		call_service_async(self.init_operator_client, request, self.init_operator_callback, self.get_logger())
+
+	def init_operator_callback(self, future):
+		"""Init operator callback.
+
+		Args:
+			future: Service future var.
+
+		Returns:
+			-
+
+		"""
+		try:
+			result = future.result()
+		except Exception as e:
+			self.get_logger().error(f"{self.init_operator_client.srv_name} service call failed {e}")
+		else:
+			if result.success:
+				self.get_logger().info("Operator node succesfully initialized.")
+			else:
+				self.get_logger().error(f"Operator node could not be initialized: {result.message}")
+
+	def stop_exoforce(self, _):
 		"""Stops exoforce.
 
 		Args:
@@ -93,8 +197,15 @@ def stop_exoforce(self):
 			-
 
 		"""
+		if self.processing_flag:
+			return
+
+		self.get_logger().info("Stopping Exoforce...")
+		self.processing_flag = True
 		self.active = False
 		self.apply_min_force_timer.cancel()
+		self.set_target_force(0.0)
+		self.get_logger().info("Stopping Force Control node...")
 		call_service_async(self.stop_force_control_client, Trigger.Request(), self.stop_force_control_callback, self.get_logger())
 
 	def stop_force_control_callback(self, future):
@@ -113,9 +224,13 @@ def stop_force_control_callback(self, future):
 			self.get_logger().error(f"{self.stop_force_control_client.srv_name} service call failed {e}")
 		else:
 			if not result.success:
-				self.get_logger().error(f"Failed to stop force control node: {result.message}")
+				err_msg = f"Failed to stop force control node: {result.message}"
+				self.get_logger().error(f"Cannot stop Exoforce: {err_msg}")
+				self.send_exoforce_response(self.stop_response_publisher, False, err_msg)
 			else:
 				self.get_logger().info("Force Control node stopped.")
+				self.get_logger().info("Exoforce stopped.")
+				self.send_exoforce_response(self.stop_response_publisher, True)
 
 	def set_target_force(self, force):
 		"""Sets a target force to all the tendons.
@@ -162,7 +277,10 @@ def collision_listener(self, collision_msg):
 		self.get_logger().info(f"Received collision: link: {collision_msg.linkid} force: {collision_msg.normalforce}")
 
 		ef = self.map_link_to_ef(collision_msg.linkid)
-		if ef.orientation is None:
+		if ef is None:
+			self.get_logger().warn(f"Link '{collision_msg.linkid}' has no mapping to ef!")
+			return
+		elif ef.orientation is None:
 			self.get_logger().warn(f"Orientation for ef '{ef.name}' is not initialized!")
 			return
 

bulletroboy/nodes/exoforce_node.py

@@ -1,20 +1,24 @@
-import os
+import os, sys
 import rclpy
 from ..exoforce.exoforce import CageConfiguration
 from ..exoforce.exoforce_hardware import ExoforceHW
+from ..utils.utils import parse_launch_arg
 
-CONFIG_DEFAULT_PATH = os.path.dirname(os.path.realpath(__file__)) + "/" + "../../config/realCageConfiguration.xml"
+CAGE_CONF_DEFAULT_PATH = os.path.dirname(os.path.realpath(__file__)) + "/" + "../../config/realCageConfiguration.xml"
 
 def main(args=None):
-	rclpy.init(args=args)
+	try:
+		rclpy.init(args=args)
 
-	# EXOFORCE SETUP
-	initial_cage_conf = CageConfiguration(CONFIG_DEFAULT_PATH)
-	exoforce = ExoforceHW(initial_cage_conf)
-	rclpy.spin(exoforce)
+		cage_conf_path = parse_launch_arg(sys.argv[1], CAGE_CONF_DEFAULT_PATH, rclpy.logging._root_logger.info)
 
-	exoforce.destroy_node()
-	rclpy.shutdown()
+		# EXOFORCE SETUP
+		initial_cage_conf = CageConfiguration(cage_conf_path)
+		exoforce = ExoforceHW(initial_cage_conf)
+		rclpy.spin(exoforce)
+
+	except KeyboardInterrupt:
+		rclpy.shutdown()
 
 if __name__ == "__main__":
 	main()

bulletroboy/nodes/operator_node.py

@@ -2,13 +2,14 @@
 from ..operator.operator_hardware import OperatorHW
 
 def main(args=None):
-    rclpy.init(args=args)
+    try:
+        rclpy.init(args=args)
 
-    operator = OperatorHW()
-    rclpy.spin(operator)
+        operator = OperatorHW()
+        rclpy.spin(operator)
 
-    operator.destroy_node()
-    rclpy.shutdown()
+    except KeyboardInterrupt:
+        rclpy.shutdown()
 
 if __name__ == "__main__":
 	main()

bulletroboy/nodes/state_mapper_node.py

@@ -3,14 +3,15 @@
 from ..state_mapper.state_mapper import StateMapper
 
 def main(args=None):
-    rclpy.init(args=args)
+    try:
+        rclpy.init(args=args)
 
-    forcesMapper = StateMapper()
-    executor = MultiThreadedExecutor()
-    rclpy.spin(forcesMapper, executor)
+        forcesMapper = StateMapper()
+        executor = MultiThreadedExecutor()
+        rclpy.spin(forcesMapper, executor)
 
-    forcesMapper.destroy_node()
-    rclpy.shutdown()
+    except KeyboardInterrupt:
+        rclpy.shutdown()
 
 
 if __name__ == '__main__':