latest pushes
This commit is contained in:
parent
9f051b74d6
commit
7e61eae008
@ -24,9 +24,10 @@ from geometry_msgs.msg import Point
|
||||
from humanoid_robot_intelligence_control_system_walking_module_msgs.msg import WalkingParam
|
||||
from humanoid_robot_intelligence_control_system_walking_module_msgs.srv import GetWalkingParam
|
||||
|
||||
from humanoid_robot_intelligence_control_system_detection.follower import calculate_distance_to_face, calculate_footstep, calculate_delta_time
|
||||
from humanoid_robot_intelligence_control_system_detection.follower import calculate_distance_to, calculate_footstep, calculate_delta_time
|
||||
from humanoid_robot_intelligence_control_system_detection.follower_config import FollowerConfig, FollowerInitializeConfig
|
||||
|
||||
|
||||
class FaceFollower:
|
||||
def __init__(self, ros_version):
|
||||
self.ros_version = ros_version
|
||||
@ -116,7 +117,7 @@ class FaceFollower:
|
||||
|
||||
self.initialize_config.approach_face_position = "OutOfRange"
|
||||
|
||||
distance_to_face = calculate_distance_to_face(
|
||||
distance_to_face = calculate_distance_to(
|
||||
self.config.CAMERA_HEIGHT, self.initialize_config.current_tilt, self.config.hip_pitch_offset, face_size)
|
||||
|
||||
distance_to_approach = 0.22
|
||||
|
||||
@ -15,31 +15,17 @@
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
|
||||
import os
|
||||
import sys
|
||||
import rospy
|
||||
import math
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import String
|
||||
from std_msgs.msg import String, Float64MultiArray
|
||||
from humanoid_robot_intelligence_control_system_ball_detector.msg import CircleSetStamped
|
||||
from geometry_msgs.msg import Point
|
||||
|
||||
class FaceTracker:
|
||||
def __init__(self, ros_version):
|
||||
self.ros_version = ros_version
|
||||
if self.ros_version == '1':
|
||||
rospy.init_node('face_tracker')
|
||||
self.get_param = rospy.get_param
|
||||
self.logger = rospy
|
||||
else:
|
||||
rclpy.init()
|
||||
self.node = Node('face_tracker')
|
||||
self.get_param = self.node.get_parameter
|
||||
self.logger = self.node.get_logger()
|
||||
|
||||
self.initialize_parameters()
|
||||
self.setup_ros_communication()
|
||||
|
||||
|
||||
def initialize_parameters(self):
|
||||
def __init__(self):
|
||||
rospy.init_node('face_tracker')
|
||||
|
||||
self.FOV_WIDTH = 35.2 * math.pi / 180
|
||||
self.FOV_HEIGHT = 21.6 * math.pi / 180
|
||||
self.NOT_FOUND_THRESHOLD = 50
|
||||
@ -51,38 +37,31 @@ class FaceTracker:
|
||||
self.current_face_tilt = 0
|
||||
self.x_error_sum = 0
|
||||
self.y_error_sum = 0
|
||||
self.current_face_bottom = 0
|
||||
self.tracking_status = "NotFound"
|
||||
self.DEBUG_PRINT = False
|
||||
|
||||
self.p_gain = self.get_param('p_gain', 0.4)
|
||||
self.i_gain = self.get_param('i_gain', 0.0)
|
||||
self.d_gain = self.get_param('d_gain', 0.0)
|
||||
|
||||
self.logger.info(f"Face tracking Gain : {self.p_gain}, {self.i_gain}, {self.d_gain}")
|
||||
param_nh = rospy.get_param("~")
|
||||
self.p_gain = param_nh.get("p_gain", 0.4)
|
||||
self.i_gain = param_nh.get("i_gain", 0.0)
|
||||
self.d_gain = param_nh.get("d_gain", 0.0)
|
||||
|
||||
def setup_ros_communication(self):
|
||||
if self.ros_version == '1':
|
||||
self.head_joint_pub = rospy.Publisher(
|
||||
"/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=1)
|
||||
self.head_scan_pub = rospy.Publisher(
|
||||
"/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=1)
|
||||
self.face_position_sub = rospy.Subscriber(
|
||||
"/face_detector/face_position", Point, self.face_position_callback)
|
||||
self.face_tracking_command_sub = rospy.Subscriber(
|
||||
"/face_tracker/command", String, self.face_tracker_command_callback)
|
||||
self.prev_time = rospy.Time.now()
|
||||
else:
|
||||
self.head_joint_pub = self.node.create_publisher(
|
||||
JointState, "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 1)
|
||||
self.head_scan_pub = self.node.create_publisher(
|
||||
String, "/humanoid_robot_intelligence_control_system/head_control/scan_command", 1)
|
||||
self.face_position_sub = self.node.create_subscription(
|
||||
Point, "/face_detector/face_position", self.face_position_callback, 10)
|
||||
self.face_tracking_command_sub = self.node.create_subscription(
|
||||
String, "/face_tracker/command", self.face_tracker_command_callback, 10)
|
||||
self.prev_time = self.node.get_clock().now()
|
||||
rospy.loginfo(f"face tracking Gain : {self.p_gain}, {self.i_gain}, {self.d_gain}")
|
||||
|
||||
self.head_joint_offset_pub = rospy.Publisher(
|
||||
"/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", JointState, queue_size=0)
|
||||
self.head_joint_pub = rospy.Publisher(
|
||||
"/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=0)
|
||||
self.head_scan_pub = rospy.Publisher(
|
||||
"/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=0)
|
||||
|
||||
self.face_position_sub = rospy.Subscriber(
|
||||
"/face_detector_node/face_position", Point, self.face_position_callback)
|
||||
self.face_tracking_command_sub = rospy.Subscriber(
|
||||
"/face_tracker/command", String, self.face_tracker_command_callback)
|
||||
|
||||
self.face_position = Point()
|
||||
self.prev_time = rospy.Time.now()
|
||||
|
||||
def face_position_callback(self, msg):
|
||||
self.face_position = msg
|
||||
@ -100,17 +79,20 @@ class FaceTracker:
|
||||
|
||||
def start_tracking(self):
|
||||
self.on_tracking = True
|
||||
self.logger.info("Start Face tracking")
|
||||
rospy.loginfo("Start face tracking") if self.DEBUG_PRINT else None
|
||||
|
||||
def stop_tracking(self):
|
||||
self.go_init()
|
||||
self.on_tracking = False
|
||||
self.logger.info("Stop Face tracking")
|
||||
rospy.loginfo("Stop face tracking") if self.DEBUG_PRINT else None
|
||||
self.current_face_pan = 0
|
||||
self.current_face_tilt = 0
|
||||
self.x_error_sum = 0
|
||||
self.y_error_sum = 0
|
||||
|
||||
def set_using_head_scan(self, use_scan):
|
||||
self.use_head_scan = use_scan
|
||||
|
||||
def process_tracking(self):
|
||||
if not self.on_tracking:
|
||||
self.face_position.z = 0
|
||||
@ -120,7 +102,10 @@ class FaceTracker:
|
||||
if self.face_position.z <= 0:
|
||||
self.count_not_found += 1
|
||||
if self.count_not_found < self.WAITING_THRESHOLD:
|
||||
tracking_status = "Waiting"
|
||||
if self.tracking_status in ["Found", "Waiting"]:
|
||||
tracking_status = "Waiting"
|
||||
else:
|
||||
tracking_status = "NotFound"
|
||||
elif self.count_not_found > self.NOT_FOUND_THRESHOLD:
|
||||
self.scan_face()
|
||||
self.count_not_found = 0
|
||||
@ -133,17 +118,18 @@ class FaceTracker:
|
||||
|
||||
if tracking_status != "Found":
|
||||
self.tracking_status = tracking_status
|
||||
self.current_face_pan = 0
|
||||
self.current_face_tilt = 0
|
||||
self.x_error_sum = 0
|
||||
self.y_error_sum = 0
|
||||
return tracking_status
|
||||
|
||||
x_error = -math.atan(self.face_position.x * math.tan(self.FOV_WIDTH))
|
||||
y_error = -math.atan(self.face_position.y * math.tan(self.FOV_HEIGHT))
|
||||
face_size = self.face_position.z
|
||||
|
||||
if self.ros_version == '1':
|
||||
curr_time = rospy.Time.now()
|
||||
delta_time = (curr_time - self.prev_time).to_sec()
|
||||
else:
|
||||
curr_time = self.node.get_clock().now()
|
||||
delta_time = (curr_time - self.prev_time).nanoseconds / 1e9
|
||||
curr_time = rospy.Time.now()
|
||||
delta_time = (curr_time - self.prev_time).to_sec()
|
||||
self.prev_time = curr_time
|
||||
|
||||
x_error_diff = (x_error - self.current_face_pan) / delta_time
|
||||
@ -154,15 +140,18 @@ class FaceTracker:
|
||||
y_error_target = y_error * self.p_gain + y_error_diff * self.d_gain + self.y_error_sum * self.i_gain
|
||||
|
||||
if self.DEBUG_PRINT:
|
||||
self.logger.info(f"Error: {x_error * 180 / math.pi} | {y_error * 180 / math.pi}")
|
||||
self.logger.info(f"Error diff: {x_error_diff * 180 / math.pi} | {y_error_diff * 180 / math.pi} | {delta_time}")
|
||||
self.logger.info(f"Error sum: {self.x_error_sum * 180 / math.pi} | {self.y_error_sum * 180 / math.pi}")
|
||||
self.logger.info(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}")
|
||||
rospy.loginfo(f"Error: {x_error * 180 / math.pi} | {y_error * 180 / math.pi}")
|
||||
rospy.loginfo(f"Error diff: {x_error_diff * 180 / math.pi} | {y_error_diff * 180 / math.pi} | {delta_time}")
|
||||
rospy.loginfo(f"Error sum: {self.x_error_sum * 180 / math.pi} | {self.y_error_sum * 180 / math.pi}")
|
||||
rospy.loginfo(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}")
|
||||
|
||||
self.publish_head_joint(x_error_target, y_error_target)
|
||||
|
||||
self.current_face_pan = x_error
|
||||
self.current_face_tilt = y_error
|
||||
self.current_face_bottom = face_size
|
||||
|
||||
self.face_position.z = 0
|
||||
|
||||
self.tracking_status = tracking_status
|
||||
return tracking_status
|
||||
@ -176,7 +165,7 @@ class FaceTracker:
|
||||
head_angle_msg.name = ["head_pan", "head_tilt"]
|
||||
head_angle_msg.position = [pan, tilt]
|
||||
|
||||
self.head_joint_pub.publish(head_angle_msg)
|
||||
self.head_joint_offset_pub.publish(head_angle_msg)
|
||||
|
||||
def go_init(self):
|
||||
head_angle_msg = JointState()
|
||||
@ -194,44 +183,12 @@ class FaceTracker:
|
||||
|
||||
self.head_scan_pub.publish(scan_msg)
|
||||
|
||||
def run(self):
|
||||
if self.ros_version == '1':
|
||||
rate = rospy.Rate(30) # 30Hz
|
||||
while not rospy.is_shutdown():
|
||||
self.process_tracking()
|
||||
rate.sleep()
|
||||
else:
|
||||
while rclpy.ok():
|
||||
self.process_tracking()
|
||||
rclpy.spin_once(self.node, timeout_sec=0.03) # Approximately 30Hz
|
||||
|
||||
def main(ros_version):
|
||||
try:
|
||||
tracker = FaceTracker(ros_version)
|
||||
tracker.run()
|
||||
except Exception as e:
|
||||
if ros_version == '1':
|
||||
rospy.logerr(f"An error occurred: {e}")
|
||||
else:
|
||||
rclpy.shutdown()
|
||||
print(f"An error occurred: {e}")
|
||||
|
||||
if __name__ == '__main__':
|
||||
ros_version = os.getenv("ROS_VERSION")
|
||||
if ros_version == "1":
|
||||
try:
|
||||
import rospy
|
||||
main(ros_version)
|
||||
except rospy.ROSInterruptException:
|
||||
print("Error in ROS1 main")
|
||||
elif ros_version == "2":
|
||||
try:
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
main(ros_version)
|
||||
except KeyboardInterrupt:
|
||||
rclpy.shutdown()
|
||||
print("Error in ROS2 main")
|
||||
else:
|
||||
print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.")
|
||||
sys.exit(1)
|
||||
try:
|
||||
tracker = FaceTracker()
|
||||
rate = rospy.Rate(30)
|
||||
while not rospy.is_shutdown():
|
||||
tracker.process_tracking()
|
||||
rate.sleep()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
||||
@ -24,9 +24,10 @@ from geometry_msgs.msg import Point
|
||||
from humanoid_robot_intelligence_control_system_walking_module_msgs.msg import WalkingParam
|
||||
from humanoid_robot_intelligence_control_system_walking_module_msgs.srv import GetWalkingParam
|
||||
|
||||
from humanoid_robot_intelligence_control_system_detection.follower import calculate_distance_to_object, calculate_footstep, calculate_delta_time
|
||||
from humanoid_robot_intelligence_control_system_detection.follower import calculate_distance_to, calculate_footstep, calculate_delta_time
|
||||
from humanoid_robot_intelligence_control_system_detection.follower_config import FollowerConfig, FollowerInitializeConfig
|
||||
|
||||
|
||||
class ObjectFollower:
|
||||
def __init__(self, ros_version):
|
||||
self.ros_version = ros_version
|
||||
@ -116,7 +117,7 @@ class ObjectFollower:
|
||||
|
||||
self.initialize_config.approach_object_position = "OutOfRange"
|
||||
|
||||
distance_to_object = calculate_distance_to_object(
|
||||
distance_to_object = calculate_distance_to(
|
||||
self.config.CAMERA_HEIGHT, self.initialize_config.current_tilt, self.config.hip_pitch_offset, object_size)
|
||||
|
||||
distance_to_approach = 0.22
|
||||
|
||||
@ -15,56 +15,68 @@
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
|
||||
import rospy
|
||||
import os
|
||||
import sys
|
||||
import math
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import String, Float64MultiArray
|
||||
from humanoid_robot_intelligence_control_system_ball_detector.msg import CircleSetStamped
|
||||
from std_msgs.msg import String
|
||||
from geometry_msgs.msg import Point
|
||||
|
||||
from humanoid_robot_intelligence_control_system_detection.tracker import calculate_error, calculate_error_target, calculate_delta_time
|
||||
from humanoid_robot_intelligence_control_system_detection.tracker_config import TrackerConfig, TrackerInitializeConfig
|
||||
|
||||
class ObjectTracker:
|
||||
def __init__(self):
|
||||
rospy.init_node('object_tracker')
|
||||
def __init__(self, ros_version):
|
||||
self.ros_version = ros_version
|
||||
self.initialize_config = TrackerInitializeConfig()
|
||||
self.config = TrackerConfig()
|
||||
self.initialize_ros()
|
||||
self.setup_ros_communication()
|
||||
|
||||
def initialize_ros(self):
|
||||
if self.ros_version == '1':
|
||||
rospy.init_node('object_tracker')
|
||||
self.get_param = rospy.get_param
|
||||
self.logger = rospy
|
||||
else:
|
||||
rclpy.init()
|
||||
self.node = rclpy.create_node('object_tracker')
|
||||
self.get_param = self.node.get_parameter
|
||||
self.logger = self.node.get_logger()
|
||||
|
||||
self.FOV_WIDTH = 35.2 * math.pi / 180
|
||||
self.FOV_HEIGHT = 21.6 * math.pi / 180
|
||||
self.NOT_FOUND_THRESHOLD = 50
|
||||
self.WAITING_THRESHOLD = 5
|
||||
self.use_head_scan = True
|
||||
self.count_not_found = 0
|
||||
self.on_tracking = False
|
||||
self.current_object_pan = 0
|
||||
self.current_object_tilt = 0
|
||||
self.x_error_sum = 0
|
||||
self.y_error_sum = 0
|
||||
self.current_object_bottom = 0
|
||||
self.tracking_status = "NotFound"
|
||||
self.DEBUG_PRINT = False
|
||||
self.config.update_from_params(self.get_param)
|
||||
self.initialize_config.update_from_params(self.get_param)
|
||||
|
||||
param_nh = rospy.get_param("~")
|
||||
self.p_gain = param_nh.get("p_gain", 0.4)
|
||||
self.i_gain = param_nh.get("i_gain", 0.0)
|
||||
self.d_gain = param_nh.get("d_gain", 0.0)
|
||||
|
||||
rospy.loginfo(f"Object tracking Gain : {self.p_gain}, {self.i_gain}, {self.d_gain}")
|
||||
|
||||
self.head_joint_offset_pub = rospy.Publisher(
|
||||
"/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", JointState, queue_size=0)
|
||||
self.head_joint_pub = rospy.Publisher(
|
||||
"/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=0)
|
||||
self.head_scan_pub = rospy.Publisher(
|
||||
"/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=0)
|
||||
|
||||
self.object_position_sub = rospy.Subscriber(
|
||||
"/object_detector_node/object_position", Point, self.object_position_callback)
|
||||
self.object_tracking_command_sub = rospy.Subscriber(
|
||||
"/object_tracker/command", String, self.object_tracker_command_callback)
|
||||
|
||||
self.object_position = Point()
|
||||
self.prev_time = rospy.Time.now()
|
||||
def setup_ros_communication(self):
|
||||
if self.ros_version == '1':
|
||||
self.head_joint_offset_pub = rospy.Publisher(
|
||||
"/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", JointState, queue_size=0)
|
||||
self.head_joint_pub = rospy.Publisher(
|
||||
"/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=0)
|
||||
self.head_scan_pub = rospy.Publisher(
|
||||
"/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=0)
|
||||
self.object_position_sub = rospy.Subscriber(
|
||||
"/object_detector_node/object_position", Point, self.object_position_callback)
|
||||
self.object_tracking_command_sub = rospy.Subscriber(
|
||||
"/object_tracker/command", String, self.object_tracker_command_callback)
|
||||
self.prev_time = rospy.Time.now()
|
||||
else:
|
||||
self.head_joint_offset_pub = self.node.create_publisher(
|
||||
JointState, "/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", 10)
|
||||
self.head_joint_pub = self.node.create_publisher(
|
||||
JointState, "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 10)
|
||||
self.head_scan_pub = self.node.create_publisher(
|
||||
String, "/humanoid_robot_intelligence_control_system/head_control/scan_command", 10)
|
||||
self.object_position_sub = self.node.create_subscription(
|
||||
Point, "/object_detector_node/object_position", self.object_position_callback, 10)
|
||||
self.object_tracking_command_sub = self.node.create_subscription(
|
||||
String, "/object_tracker/command", self.object_tracker_command_callback, 10)
|
||||
self.prev_time = self.node.get_clock().now()
|
||||
|
||||
def object_position_callback(self, msg):
|
||||
self.object_position = msg
|
||||
self.initialize_config.object_position = msg
|
||||
if self.initialize_config.on_tracking:
|
||||
self.process_tracking()
|
||||
|
||||
def object_tracker_command_callback(self, msg):
|
||||
if msg.data == "start":
|
||||
@ -72,88 +84,90 @@ class ObjectTracker:
|
||||
elif msg.data == "stop":
|
||||
self.stop_tracking()
|
||||
elif msg.data == "toggle_start":
|
||||
if not self.on_tracking:
|
||||
if not self.initialize_config.on_tracking:
|
||||
self.start_tracking()
|
||||
else:
|
||||
self.stop_tracking()
|
||||
|
||||
def start_tracking(self):
|
||||
self.on_tracking = True
|
||||
rospy.loginfo("Start Object tracking") if self.DEBUG_PRINT else None
|
||||
self.initialize_config.on_tracking = True
|
||||
self.logger.info("Start Object tracking")
|
||||
|
||||
def stop_tracking(self):
|
||||
self.go_init()
|
||||
self.on_tracking = False
|
||||
rospy.loginfo("Stop Object tracking") if self.DEBUG_PRINT else None
|
||||
self.current_object_pan = 0
|
||||
self.current_object_tilt = 0
|
||||
self.x_error_sum = 0
|
||||
self.y_error_sum = 0
|
||||
self.initialize_config.on_tracking = False
|
||||
self.logger.info("Stop Object tracking")
|
||||
self.initialize_config.current_object_pan = 0
|
||||
self.initialize_config.current_object_tilt = 0
|
||||
self.initialize_config.x_error_sum = 0
|
||||
self.initialize_config.y_error_sum = 0
|
||||
|
||||
def set_using_head_scan(self, use_scan):
|
||||
self.use_head_scan = use_scan
|
||||
self.config.use_head_scan = use_scan
|
||||
|
||||
def process_tracking(self):
|
||||
if not self.on_tracking:
|
||||
self.object_position.z = 0
|
||||
self.count_not_found = 0
|
||||
if not self.initialize_config.on_tracking:
|
||||
self.initialize_config.object_position.z = 0
|
||||
self.initialize_config.count_not_found = 0
|
||||
return "NotFound"
|
||||
|
||||
if self.object_position.z <= 0:
|
||||
self.count_not_found += 1
|
||||
if self.count_not_found < self.WAITING_THRESHOLD:
|
||||
if self.tracking_status in ["Found", "Waiting"]:
|
||||
if self.initialize_config.object_position.z <= 0:
|
||||
self.initialize_config.count_not_found += 1
|
||||
if self.initialize_config.count_not_found < self.config.WAITING_THRESHOLD:
|
||||
if self.initialize_config.tracking_status in ["Found", "Waiting"]:
|
||||
tracking_status = "Waiting"
|
||||
else:
|
||||
tracking_status = "NotFound"
|
||||
elif self.count_not_found > self.NOT_FOUND_THRESHOLD:
|
||||
elif self.initialize_config.count_not_found > self.config.NOT_FOUND_THRESHOLD:
|
||||
self.scan_object()
|
||||
self.count_not_found = 0
|
||||
self.initialize_config.count_not_found = 0
|
||||
tracking_status = "NotFound"
|
||||
else:
|
||||
tracking_status = "NotFound"
|
||||
else:
|
||||
self.count_not_found = 0
|
||||
self.initialize_config.count_not_found = 0
|
||||
tracking_status = "Found"
|
||||
|
||||
if tracking_status != "Found":
|
||||
self.tracking_status = tracking_status
|
||||
self.current_object_pan = 0
|
||||
self.current_object_tilt = 0
|
||||
self.x_error_sum = 0
|
||||
self.y_error_sum = 0
|
||||
self.initialize_config.tracking_status = tracking_status
|
||||
self.initialize_config.current_object_pan = 0
|
||||
self.initialize_config.current_object_tilt = 0
|
||||
self.initialize_config.x_error_sum = 0
|
||||
self.initialize_config.y_error_sum = 0
|
||||
return tracking_status
|
||||
|
||||
x_error = -math.atan(self.object_position.x * math.tan(self.FOV_WIDTH))
|
||||
y_error = -math.atan(self.object_position.y * math.tan(self.FOV_HEIGHT))
|
||||
object_size = self.object_position.z
|
||||
x_error, y_error = calculate_error(self.initialize_config.object_position, self.config.FOV_WIDTH, self.config.FOV_HEIGHT)
|
||||
object_size = self.initialize_config.object_position.z
|
||||
|
||||
curr_time = rospy.Time.now()
|
||||
delta_time = (curr_time - self.prev_time).to_sec()
|
||||
curr_time = rospy.Time.now() if self.ros_version == '1' else self.node.get_clock().now()
|
||||
delta_time = calculate_delta_time(curr_time, self.prev_time)
|
||||
self.prev_time = curr_time
|
||||
|
||||
x_error_diff = (x_error - self.current_object_pan) / delta_time
|
||||
y_error_diff = (y_error - self.current_object_tilt) / delta_time
|
||||
self.x_error_sum += x_error
|
||||
self.y_error_sum += y_error
|
||||
x_error_target = x_error * self.p_gain + x_error_diff * self.d_gain + self.x_error_sum * self.i_gain
|
||||
y_error_target = y_error * self.p_gain + y_error_diff * self.d_gain + self.y_error_sum * self.i_gain
|
||||
x_error_diff = (x_error - self.initialize_config.current_object_pan) / delta_time
|
||||
y_error_diff = (y_error - self.initialize_config.current_object_tilt) / delta_time
|
||||
self.initialize_config.x_error_sum += x_error
|
||||
self.initialize_config.y_error_sum += y_error
|
||||
|
||||
x_error_target = calculate_error_target(x_error, x_error_diff, self.initialize_config.x_error_sum,
|
||||
self.config.p_gain, self.config.i_gain, self.config.d_gain)
|
||||
y_error_target = calculate_error_target(y_error, y_error_diff, self.initialize_config.y_error_sum,
|
||||
self.config.p_gain, self.config.i_gain, self.config.d_gain)
|
||||
|
||||
if self.DEBUG_PRINT:
|
||||
rospy.loginfo(f"Error: {x_error * 180 / math.pi} | {y_error * 180 / math.pi}")
|
||||
rospy.loginfo(f"Error diff: {x_error_diff * 180 / math.pi} | {y_error_diff * 180 / math.pi} | {delta_time}")
|
||||
rospy.loginfo(f"Error sum: {self.x_error_sum * 180 / math.pi} | {self.y_error_sum * 180 / math.pi}")
|
||||
rospy.loginfo(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}")
|
||||
if self.config.DEBUG_PRINT:
|
||||
self.logger.info(f"Error: {x_error * 180 / math.pi} | {y_error * 180 / math.pi}")
|
||||
self.logger.info(f"Error diff: {x_error_diff * 180 / math.pi} | {y_error_diff * 180 / math.pi} | {delta_time}")
|
||||
self.logger.info(f"Error sum: {self.initialize_config.x_error_sum * 180 / math.pi} | {self.initialize_config.y_error_sum * 180 / math.pi}")
|
||||
self.logger.info(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}")
|
||||
|
||||
self.publish_head_joint(x_error_target, y_error_target)
|
||||
|
||||
self.current_object_pan = x_error
|
||||
self.current_object_tilt = y_error
|
||||
self.current_object_bottom = object_size
|
||||
self.initialize_config.current_object_pan = x_error
|
||||
self.initialize_config.current_object_tilt = y_error
|
||||
self.initialize_config.current_object_bottom = object_size
|
||||
|
||||
self.object_position.z = 0
|
||||
self.initialize_config.object_position.z = 0
|
||||
|
||||
self.tracking_status = tracking_status
|
||||
self.initialize_config.tracking_status = tracking_status
|
||||
return tracking_status
|
||||
|
||||
def publish_head_joint(self, pan, tilt):
|
||||
@ -175,7 +189,7 @@ class ObjectTracker:
|
||||
self.head_joint_pub.publish(head_angle_msg)
|
||||
|
||||
def scan_object(self):
|
||||
if not self.use_head_scan:
|
||||
if not self.config.use_head_scan:
|
||||
return
|
||||
|
||||
scan_msg = String()
|
||||
@ -183,12 +197,39 @@ class ObjectTracker:
|
||||
|
||||
self.head_scan_pub.publish(scan_msg)
|
||||
|
||||
if __name__ == '__main__':
|
||||
def run(self):
|
||||
if self.ros_version == '1':
|
||||
rospy.spin()
|
||||
else:
|
||||
rclpy.spin(self.node)
|
||||
|
||||
def main(ros_version):
|
||||
try:
|
||||
tracker = ObjectTracker()
|
||||
rate = rospy.Rate(30)
|
||||
while not rospy.is_shutdown():
|
||||
tracker.process_tracking()
|
||||
rate.sleep()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
tracker = ObjectTracker(ros_version)
|
||||
tracker.start_tracking()
|
||||
tracker.run()
|
||||
except Exception as e:
|
||||
if ros_version == '1':
|
||||
rospy.logerr(f"An error occurred: {e}")
|
||||
else:
|
||||
rclpy.shutdown()
|
||||
print(f"An error occurred: {e}")
|
||||
|
||||
if __name__ == '__main__':
|
||||
ros_version = os.getenv("ROS_VERSION")
|
||||
if ros_version == "1":
|
||||
import rospy
|
||||
try:
|
||||
main(ros_version)
|
||||
except rospy.ROSInterruptException:
|
||||
print("Error in ROS1 main")
|
||||
elif ros_version == "2":
|
||||
import rclpy
|
||||
try:
|
||||
main(ros_version)
|
||||
except KeyboardInterrupt:
|
||||
rclpy.shutdown()
|
||||
print("Error in ROS2 main")
|
||||
else:
|
||||
print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.")
|
||||
sys.exit(1)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user