aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-04-20 11:51:16 -05:00
Code Issues Packages Projects Releases Wiki Activity

refactor: (headless) finish integrating Core cmd_vel

Browse Source
This commit is contained in:
David
2026-03-19 20:02:37 -05:00
parent 120891c8e5
commit ff4a58e6ed
1 changed files with 19 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
src/headless_pkg/src/headless_node.py
View File

@@ -139,7 +139,9 @@ class Headless(Node):
) )
self.declare_parameter("use_cmd_vel", False) self.declare_parameter("use_cmd_vel", False)
self.using_cmd_vel = self.get_parameter("use_cmd_vel").value self.use_cmd_vel = (
self.get_parameter("use_cmd_vel").get_parameter_value().bool_value
)
self.declare_parameter("use_bio", False) self.declare_parameter("use_bio", False)
self.use_bio = self.get_parameter("use_bio").get_parameter_value().bool_value self.use_bio = self.get_parameter("use_bio").get_parameter_value().bool_value
@@ -157,7 +159,7 @@ class Headless(Node):
) )
# Check parameter validity # Check parameter validity
if self.using_cmd_vel: if self.use_cmd_vel:
self.get_logger().info("Using cmd_vel for core control") self.get_logger().info("Using cmd_vel for core control")
global CORE_MODE global CORE_MODE
CORE_MODE = "twist" CORE_MODE = "twist"
@@ -251,6 +253,9 @@ class Headless(Node):
self.core_publisher.publish(CORE_STOP_MSG) self.core_publisher.publish(CORE_STOP_MSG)
self.arm_publisher.publish(ARM_STOP_MSG) self.arm_publisher.publish(ARM_STOP_MSG)
self.bio_publisher.publish(BIO_STOP_MSG) self.bio_publisher.publish(BIO_STOP_MSG)
else:
if self.use_cmd_vel:
self.core_cmd_vel_pub_.publish(self.core_cmd_vel_stop_msg())
else: else:
self.core_twist_pub_.publish(CORE_STOP_TWIST_MSG) self.core_twist_pub_.publish(CORE_STOP_TWIST_MSG)
if self.use_arm_ik: if self.use_arm_ik:
@@ -355,16 +360,18 @@ class Headless(Node):
right_stick_x**2, right_stick_x right_stick_x**2, right_stick_x
) # Exponent for finer control (curve) ) # Exponent for finer control (curve)
if self.using_cmd_vel: # This kinda looks dumb being seperate from the following block, but this
# maintains the separation between modifying the control message and sending it
if self.use_cmd_vel:
twist.linear.x *= 1.5 twist.linear.x *= 1.5
twist.angular.z *= 0.5 twist.angular.z *= 0.5
# Publish # Publish
if not self.using_cmd_vel: if self.use_cmd_vel:
self.core_twist_pub_.publish(twist)
else:
header = Header(stamp=self.get_clock().now().to_msg()) header = Header(stamp=self.get_clock().now().to_msg())
self.core_cmd_vel_pub_.publish(TwistStamped(header=header, twist=twist)) self.core_cmd_vel_pub_.publish(TwistStamped(header=header, twist=twist))
else:
self.core_twist_pub_.publish(twist)
self.get_logger().debug( self.get_logger().debug(
f"[Core Ctrl] Linear: {round(twist.linear.x, 2)}, Angular: {round(twist.angular.z, 2)}" f"[Core Ctrl] Linear: {round(twist.linear.x, 2)}, Angular: {round(twist.angular.z, 2)}"
) )
@@ -666,6 +673,11 @@ class Headless(Node):
else: else:
pass # TODO: implement new bio control topics pass # TODO: implement new bio control topics
def core_cmd_vel_stop_msg(self):
return TwistStamped(
header=Header(frame_id="base_link", stamp=self.get_clock().now().to_msg())
)
def arm_manual_stop_msg(self): def arm_manual_stop_msg(self):
return JointJog( return JointJog(
header=Header(frame_id="base_link", stamp=self.get_clock().now().to_msg()), header=Header(frame_id="base_link", stamp=self.get_clock().now().to_msg()),