def __init__(self):
rospy.loginfo("INIT CONTROL VISION")
rospy.init_node("robot_vision", anonymous=True)
self.control_pid_x = ControlPid(5, -5, 0.01, 0, 0)
self.control_pid_yaw = ControlPid(3, -3, 0.001, 0, 0)
self.flag_find = False
self.flag_explore = False
self.stop = False
self.time_start = time.time()
rospy.Subscriber("/diff/camera_top/camera_info", CameraInfo,
self.callback_camera_info)
rospy.Subscriber("/move_base/status", GoalStatusArray,
self.callback_move_base_info)
rospy.Subscriber("/Explore/status", GoalStatusArray,
self.callback_explore_status)
self.pub_cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
self.pub_move_to_goal = rospy.Publisher("/move_base_simple/goal",
PoseStamped,
queue_size=1)
self.pub_first_map_goal = rospy.Publisher("/GetFirstMap/goal",
GetFirstMapActionGoal,
queue_size=1)
self.pub_explore_goal = rospy.Publisher("/Explore/goal",
ExploreActionGoal,
queue_size=1)
cancel_first_map = rospy.Publisher("/GetFirstMap/cancel",
GoalID,
queue_size=1)
time.sleep(1)
self.pub_first_map_goal.publish()
time.sleep(1)
cancel_first_map.publish(GoalID())
class ControlVision:
control_pid_x = None
control_pid_yaw = None
pub_cmd_vel = None
msg_twist = None
camera_info = None
def __init__(self):
rospy.init_node("robot_vision", anonymous=True)
self.pub_cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
self.control_pid_x = ControlPid(5, -5, 0.01, 0, 0)
self.control_pid_yaw = ControlPid(3, -3, 0.001, 0, 0)
self.msg_twist = Twist()
rospy.Subscriber("/diff/camera_top/camera_info", CameraInfo,
self.callback_camera_info)
def callback(self, data):
if data.x != -1:
self.msg_twist.angular.z = self.control_pid_yaw.pid_calculate(
0.5, self.camera_info.width / 2, int(data.x))
self.msg_twist.linear.x = self.control_pid_x.pid_calculate(
0.5, 360, int(data.z))
self.pub_cmd_vel.publish(self.msg_twist)
def callback_camera_info(self, data):
self.camera_info = data
def run(self):
self.msg = rospy.Subscriber("camera/param", Vector3, self.callback)
def __init__(self):
# focal length
self.focalLength = 937.8194580078125
# Initialize the CvBridge class
self.bridge = CvBridge()
# timer var
self.start = time.time()
# Initialize the ROS Node named 'opencv_camera', allow multiple nodes to be run with this name
rospy.init_node('opencv_camera', anonymous=True)
# Initalize a publisher to the "/camera/param" topic with the function "image_callback" as a callback
self.image_pub = rospy.Publisher('/camera/param', Image, queue_size=10)
# get camera info
rospy.Subscriber("/diff/camera_top/camera_info", CameraInfo, self.callback_camera_info)
# move to goal
self.pub_move_to_goal = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=1)
self.msg_move_to_goal = PoseStamped()
self.flag = True
self.camera_info = CameraInfo()
self.start_map = rospy.Publisher("/GetFirstMap/goal", GetFirstMapActionGoal, queue_size=1)
self.start_explore = rospy.Publisher("/Explore/goal", ExploreActionGoal, queue_size = 1)
self.cancel_map = rospy.Publisher("/GetFirstMap/cancel", GoalID, queue_size = 1)
self.cancel_explore = rospy.Publisher("/Explore/cancel", GoalID, queue_size = 1)
time.sleep(1)
self.start_map.publish()
time.sleep(5)
self.cancel_map.publish()
time.sleep(2)
self.start_explore.publish()
self.control_pid_x = ControlPid(5, -5, 0.01, 0, 0)
self.control_pid_yaw = ControlPid(3, -3, 0.001, 0, 0)
self.cancel_move_base = rospy.Publisher("/move_base/cancel", GoalID, queue_size=1)
self.flag1 = False
def __init__(self):
# Focal length
self.focalLength = 878 #937.8194580078125
# Initialize the CvBridge class
self.bridge = CvBridge()
# Timer var
self.start = time.time()
# Initialize the ROS Node named 'opencv_camera', allow multiple nodes to be run with this name
rospy.init_node('opencv_camera', anonymous=True)
# Initalize a publisher to the "/camera/param" topic with the function "image_callback" as a callback
self.image_pub = rospy.Publisher('/camera/param', Image, queue_size=1)
self.pub_cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# Get camera info
rospy.Subscriber("/diff/camera_top/camera_info", CameraInfo,
self.callback_camera_info)
# Move to goal
self.pub_move_to_goal = rospy.Publisher("/move_base_simple/goal",
PoseStamped,
queue_size=1)
self.msg_move_to_goal = PoseStamped()
# self.flag = True
self.camera_info = CameraInfo()
self.control_pid_x = ControlPid(5, -5, 0.005, 0, 0)
self.control_pid_yaw = ControlPid(3, -3, 0.001, 0, 0)
self.cancel_move_base = rospy.Publisher("/move_base/cancel",
GoalID,
queue_size=1)
self.controller_flag = False
# Nav2d setup
self.start_map = rospy.Publisher("/GetFirstMap/goal",
GetFirstMapActionGoal,
queue_size=1)
self.start_explore = rospy.Publisher("/Explore/goal",
ExploreActionGoal,
queue_size=1)
self.cancel_map = rospy.Publisher("/GetFirstMap/cancel",
GoalID,
queue_size=1)
self.cancel_explore = rospy.Publisher("/Explore/cancel",
GoalID,
queue_size=1)
self.cancel_move_base = rospy.Publisher("/move_base/cancel",
GoalID,
queue_size=1)
# Nav2D mapping and explore
time.sleep(1)
self.start_map.publish()
time.sleep(5)
self.cancel_map.publish()
time.sleep(2)
# Go to a specified position on map
self.msg_move_to_goal.pose.position.x = 40
self.msg_move_to_goal.pose.position.y = 0
self.msg_move_to_goal.pose.orientation.w = 1
self.msg_move_to_goal.header.frame_id = 'base_link' #self.camera_info.header.frame_id
self.pub_move_to_goal.publish(self.msg_move_to_goal)
self.start_explore.publish()
self.timer_flag = time.time()
def __init__(self):
rospy.init_node("robot_vision", anonymous=True)
self.pub_cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
self.control_pid_x = ControlPid(5, -5, 0.01, 0, 0)
self.control_pid_yaw = ControlPid(3, -3, 0.001, 0, 0)
self.msg_twist = Twist()
rospy.Subscriber("/diff/camera_top/camera_info", CameraInfo,
self.callback_camera_info)
def __init__(self):
rospy.loginfo("INIT CONTROL VISION")
self.control_pid_x = ControlPid(5, -5, 0.01, 0, 0)
self.control_pid_yaw = ControlPid(3, -3, 0.001, 0, 0)
self.pub_cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
self.msg_twist = Twist()
self.pub_quaternion = rospy.Publisher("/rotation_quaternion",
Quaternion,
queue_size=1)
self.pub_move_to_goal = rospy.Publisher("/move_base_simple/goal",
PoseStamped,
queue_size=1)
self.msg_move_to_goal = PoseStamped()
rospy.init_node("robot_vision", anonymous=True)
rospy.Subscriber("/odometry/filtered", Odometry,
self.callback_odometry)
rospy.Subscriber("/rpy_angles", Vector3, self.callback_rpy_angles)
rospy.Subscriber("/diff/camera_top/camera_info", CameraInfo,
self.callback_camera_info)
def __init__(self):
rospy.loginfo("INIT CONTROL VISION")
rospy.init_node("robot_vision", anonymous=True)
self.control_pid_x = ControlPid(5, -5, 0.01, 0, 0)
self.control_pid_yaw = ControlPid(3, -3, 0.001, 0, 0)
self.pub_cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
self.msg_twist = Twist()
self.pub_quaternion = rospy.Publisher("/rotation_quaternion",
Quaternion,
queue_size=1)
self.pub_move_to_goal = rospy.Publisher("/move_base_simple/goal",
PoseStamped,
queue_size=1)
self.msg_move_to_goal = PoseStamped()
rospy.Subscriber("/camera/obj/coordinates", Vector3, self.callback)
rospy.Subscriber("/odometry/filtered", Odometry,
self.callback_odometry)
rospy.Subscriber("/rpy_angles", Vector3, self.callback_rpy_angles)
rospy.Subscriber("/husky_cam/camera_info", CameraInfo,
self.callback_camera_info)
rospy.Subscriber("/move_base/status", GoalStatusArray,
self.callback_move_base_info)
rospy.Subscriber("/Explore/status", GoalStatusArray,
self.callback_explore_status)
self.pub_first_map_goal = rospy.Publisher("/GetFirstMap/goal",
GetFirstMapActionGoal,
queue_size=1)
cancel_first_map = rospy.Publisher("/GetFirstMap/cancel",
GoalID,
queue_size=1)
self.pub_explore_goal = rospy.Publisher("/Explore/goal",
ExploreActionGoal,
queue_size=1)
time.sleep(1)
self.pub_first_map_goal.publish()
time.sleep(1)
cancel_first_map.publish(GoalID())
class ControlVision:
control_pid_x = None
control_pid_yaw = None
pub_cmd_vel = None
msg_twist = None
camera_info = None
pub_quaternion = None
odometry_data = None
rpy_angle = None
flag_move_to_goal = False
flag_orientation = True
flag_ajustment = False
pub_move_to_goal = None
msg_move_to_goal = None
def __init__(self):
rospy.loginfo("INIT CONTROL VISION")
self.control_pid_x = ControlPid(5, -5, 0.01, 0, 0)
self.control_pid_yaw = ControlPid(3, -3, 0.001, 0, 0)
self.pub_cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
self.msg_twist = Twist()
self.pub_quaternion = rospy.Publisher("/rotation_quaternion",
Quaternion,
queue_size=1)
self.pub_move_to_goal = rospy.Publisher("/move_base_simple/goal",
PoseStamped,
queue_size=1)
self.msg_move_to_goal = PoseStamped()
rospy.init_node("robot_vision", anonymous=True)
rospy.Subscriber("/odometry/filtered", Odometry,
self.callback_odometry)
rospy.Subscriber("/rpy_angles", Vector3, self.callback_rpy_angles)
rospy.Subscriber("/diff/camera_top/camera_info", CameraInfo,
self.callback_camera_info)
def publisher_move_to_goal(self, data):
rospy.loginfo("Entrou no move base")
factor_x = 1 if (
self.rpy_angle.z <= 0 and self.rpy_angle.z >= -1.57
) or self.rpy_angle.z >= 0 and self.rpy_angle.z <= 1.57 else -1
factor_y = 1 if self.rpy_angle.z >= 0 and self.rpy_angle.z <= 3.14 else -1
angle = self.rpy_angle.z if self.rpy_angle.z >= 0 else self.rpy_angle.z * -1
self.msg_move_to_goal.pose.position.x = self.odometry_data.pose.pose.position.x + (
data.y * math.cos(angle)) * factor_x
self.msg_move_to_goal.pose.position.y = self.odometry_data.pose.pose.position.y + (
data.y * math.sin(angle)) * factor_y
self.msg_move_to_goal.header.frame_id = 'odom'
self.msg_move_to_goal.pose.orientation.z = self.odometry_data.pose.pose.orientation.z
self.msg_move_to_goal.pose.orientation.w = self.odometry_data.pose.pose.orientation.w
self.pub_move_to_goal.publish(self.msg_move_to_goal)
def orientation_to_obj(self, data):
self.msg_twist.angular.z = self.control_pid_yaw.pid_calculate(
0.5, self.camera_info.width / 2, int(data.x))
self.pub_cmd_vel.publish(self.msg_twist)
def goal_ajustment(self, data):
self.msg_twist.angular.z = self.control_pid_yaw.pid_calculate(
0.5, self.camera_info.width / 2, int(data.x))
self.msg_twist.linear.x = self.control_pid_x.pid_calculate(
0.5, 180, int(data.z))
self.pub_cmd_vel.publish(self.msg_twist)
if round(self.msg_twist.angular.z, 2) == 0 and round(
self.msg_twist.linear.x, 2) == 0:
self.flag_ajustment = False
def callback(self, data):
# msg ="\nx - " + str(self.odometry_data.pose.pose.position.x) + "\ny - " + str(self.odometry_data.pose.pose.position.y) + "\n" + str(data.y) + " - " + str((self.rpy_angle.z*180)/3.1415)
# rospy.loginfo(msg)
if data.x != -1:
if not self.flag_move_to_goal and self.flag_orientation:
self.orientation_to_obj(data)
if not self.flag_move_to_goal and self.flag_ajustment:
self.goal_ajustment(data)
if not self.flag_move_to_goal and round(
self.msg_twist.angular.z,
1) == 0 and not self.flag_ajustment:
self.flag_move_to_goal = True
self.flag_orientation = False
self.publisher_move_to_goal(data)
msg = str(round(
self.msg_move_to_goal.pose.position.x)) + " - " + str(
round(self.odometry_data.pose.pose.position.x))
rospy.loginfo(msg)
if self.flag_move_to_goal and (round(self.msg_move_to_goal.pose.position.x) == round(self.odometry_data.pose.pose.position.x) and \
round(self.msg_move_to_goal.pose.position.y) == round(self.odometry_data.pose.pose.position.y)):
self.flag_move_to_goal = False
self.flag_ajustment = True
self.flag_orientation = True
def callback_camera_info(self, data):
self.camera_info = data
def callback_odometry(self, data):
self.odometry_data = data
quaternion = Quaternion()
quaternion.x = data.pose.pose.orientation.x
quaternion.y = data.pose.pose.orientation.y
quaternion.z = data.pose.pose.orientation.z
quaternion.w = data.pose.pose.orientation.w
self.pub_quaternion.publish(quaternion)
def callback_rpy_angles(self, data):
self.rpy_angle = data
def run(self):
self.msg = rospy.Subscriber("/camera/obj/coordinates", Vector3,
self.callback)
class Robot:
camera_info = None
obj_coordinates = None
move_base_info = None
status_explore_goal = None
time_old = None
time_start = None
def __init__(self):
rospy.loginfo("INIT CONTROL VISION")
rospy.init_node("robot_vision", anonymous=True)
self.control_pid_x = ControlPid(5, -5, 0.01, 0, 0)
self.control_pid_yaw = ControlPid(3, -3, 0.001, 0, 0)
self.flag_find = False
self.flag_explore = False
self.stop = False
self.time_start = time.time()
rospy.Subscriber("/diff/camera_top/camera_info", CameraInfo,
self.callback_camera_info)
rospy.Subscriber("/move_base/status", GoalStatusArray,
self.callback_move_base_info)
rospy.Subscriber("/Explore/status", GoalStatusArray,
self.callback_explore_status)
self.pub_cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
self.pub_move_to_goal = rospy.Publisher("/move_base_simple/goal",
PoseStamped,
queue_size=1)
self.pub_first_map_goal = rospy.Publisher("/GetFirstMap/goal",
GetFirstMapActionGoal,
queue_size=1)
self.pub_explore_goal = rospy.Publisher("/Explore/goal",
ExploreActionGoal,
queue_size=1)
cancel_first_map = rospy.Publisher("/GetFirstMap/cancel",
GoalID,
queue_size=1)
time.sleep(1)
self.pub_first_map_goal.publish()
time.sleep(1)
cancel_first_map.publish(GoalID())
#
# Init of the robot
# Param data: Object parameters in the camera
# data.x - Position of the object in the camera
# data.y - Distance of the object to camera
# data.z - Radius of the object
#
def callback_main(self, data):
if not self.stop:
if data.x != -1:
self.flag_find = True
if self.flag_explore and self.status_explore_goal == 1 and data.y <= 30:
rospy.loginfo("Stop Explore and kill Operator")
rospy.Publisher("/Explore/cancel", GoalID,
queue_size=1).publish(GoalID())
os.system("rosnode kill /Operator")
time.sleep(5)
self.flag_explore = False
if (self.move_base_info.status_list
and self.move_base_info.status_list[0].status
== 1) and data.y <= 4:
rospy.Publisher('/move_base/cancel', GoalID,
queue_size=1).publish(GoalID())
self.goal_ajustment(data)
elif not self.flag_explore:
self.move_goal_to_object(data.x, data.z)
else:
if not self.flag_find and not self.flag_explore and self.status_explore_goal != 1:
rospy.loginfo("Wait..")
time.sleep(5)
self.pub_explore_goal.publish(ExploreActionGoal())
rospy.loginfo("Start Explore")
self.flag_explore = True
#
# Camera information
#
def callback_camera_info(self, data):
self.camera_info = data
#
# Move-base topic information
#
def callback_move_base_info(self, data):
self.move_base_info = data
#
# Status of the topic explore
#
def callback_explore_status(self, data):
if data.status_list:
self.status_explore_goal = data.status_list[0].status
#
# Moves the robot to the object's position on the map
# Param position_x: Position of the object in the camera
# radius: Radius of the object
#
def move_goal_to_object(self, position_x, radius):
msg_move_to_goal = PoseStamped()
if not self.time_old or (self.time_old
and time.time() - self.time_old > 10):
distance = (1 * constant.FOCAL_LENGHT) / (radius * 2)
y_move_base = -(position_x - self.camera_info.width / 2) / (
radius * 2)
x_move_base = distance if abs(y_move_base) < 0.006 else math.sqrt(
distance**2 - y_move_base**2)
msg_move_to_goal.pose.position.x = x_move_base
msg_move_to_goal.pose.position.y = y_move_base
msg_move_to_goal.pose.orientation.w = 1
msg_move_to_goal.header.frame_id = self.camera_info.header.frame_id
self.pub_move_to_goal.publish(msg_move_to_goal)
self.time_old = time.time()
#
# Center the object in the camera
# Param data: Information of the topic /camera/obj/coordinates
#
def goal_ajustment(self, data):
msg_twist = Twist()
while round(msg_twist.angular.z, 1) != 0 and round(
msg_twist.linear.x, 1) != 0:
msg_twist.angular.z = self.control_pid_yaw.pid_calculate(
0.5, self.camera_info.width / 2, int(data.x))
msg_twist.linear.x = self.control_pid_x.pid_calculate(
0.5, 200, int(data.z))
self.pub_cmd_vel.publish(msg_twist)
rospy.loginfo("Find the ball!")
rospy.loginfo(time.time() - self.time_start)
self.stop = True
#
# Start the robot
#
def run(self):
rospy.Subscriber("/camera/obj/coordinates", Vector3,
self.callback_main)
class Robot:
control_pid_x = None
control_pid_yaw = None
pub_cmd_vel = None
msg_twist = None
camera_info = None
pub_quaternion = None
odometry_data = None
rpy_angle = None
flag_move_to_goal = False
flag_orientation = True
flag_ajustment = False
flag_find = False
flag_explore = False
pub_move_to_goal = None
msg_move_to_goal = None
move_base_info = None
pub_first_map_goal = None
pub_explore_goal = None
status_explore_goal = None
flag = True
time_old = None
def __init__(self):
rospy.loginfo("INIT CONTROL VISION")
rospy.init_node("robot_vision", anonymous=True)
self.control_pid_x = ControlPid(5, -5, 0.01, 0, 0)
self.control_pid_yaw = ControlPid(3, -3, 0.001, 0, 0)
self.pub_cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
self.msg_twist = Twist()
self.pub_quaternion = rospy.Publisher("/rotation_quaternion",
Quaternion,
queue_size=1)
self.pub_move_to_goal = rospy.Publisher("/move_base_simple/goal",
PoseStamped,
queue_size=1)
self.msg_move_to_goal = PoseStamped()
rospy.Subscriber("/camera/obj/coordinates", Vector3, self.callback)
rospy.Subscriber("/odometry/filtered", Odometry,
self.callback_odometry)
rospy.Subscriber("/rpy_angles", Vector3, self.callback_rpy_angles)
rospy.Subscriber("/husky_cam/camera_info", CameraInfo,
self.callback_camera_info)
rospy.Subscriber("/move_base/status", GoalStatusArray,
self.callback_move_base_info)
rospy.Subscriber("/Explore/status", GoalStatusArray,
self.callback_explore_status)
self.pub_first_map_goal = rospy.Publisher("/GetFirstMap/goal",
GetFirstMapActionGoal,
queue_size=1)
cancel_first_map = rospy.Publisher("/GetFirstMap/cancel",
GoalID,
queue_size=1)
self.pub_explore_goal = rospy.Publisher("/Explore/goal",
ExploreActionGoal,
queue_size=1)
time.sleep(1)
self.pub_first_map_goal.publish()
time.sleep(1)
cancel_first_map.publish(GoalID())
def callback(self, data):
self.image = data
def callback_camera_info(self, data):
self.camera_info = data
def callback_odometry(self, data):
self.odometry_data = data
quaternion = Quaternion()
quaternion.x = data.pose.pose.orientation.x
quaternion.y = data.pose.pose.orientation.y
quaternion.z = data.pose.pose.orientation.z
quaternion.w = data.pose.pose.orientation.w
self.pub_quaternion.publish(quaternion)
def callback_rpy_angles(self, data):
self.rpy_angle = data
def callback_move_base_info(self, data):
self.move_base_info = data
def callback_explore_status(self, data):
if data.status_list:
self.status_explore_goal = data.status_list[0].status
def move_goal_to_object(self):
import pdb
pdb.set_trace()
if not self.time_old or (self.time_old
and time.time() - self.time_old > 10):
distance = (1 * 937.8194580078125) / (self.image.z * 2)
y_move_base = -(self.image.x - self.camera_info.width / 2) / (
self.image.z * 2)
x_move_base = distance if abs(y_move_base) < 0.006 else math.sqrt(
distance**2 - y_move_base**2)
self.msg_move_to_goal.pose.position.x = x_move_base
self.msg_move_to_goal.pose.position.y = y_move_base
self.msg_move_to_goal.pose.orientation.w = 1
self.msg_move_to_goal.header.frame_id = self.camera_info.header.frame_id
self.pub_move_to_goal.publish(self.msg_move_to_goal)
self.time_old = time.time()
def goal_ajustment(self):
while round(self.msg_twist.angular.z, 1) != 0 and round(
self.msg_twist.linear.x, 1) != 0:
self.msg_twist.angular.z = self.control_pid_yaw.pid_calculate(
0.5, self.camera_info.width / 2, int(self.image.x))
self.msg_twist.linear.x = self.control_pid_x.pid_calculate(
0.5, 180, int(self.image.z))
self.pub_cmd_vel.publish(self.msg_twist)
rospy.loginfo("Found the ball!")
rospy.loginfo(time.ctime())
exit
def run(self):
if self.image.x != -1:
self.flag_find = True
if (self.move_base_info.status_list
and self.move_base_info.status_list[0].status
== 1) and self.image.y <= 4:
rospy.Publisher('/move_base/cancel', GoalID,
queue_size=1).publish(GoalID())
self.goal_ajustment()
else:
self.move_goal_to_object()
if self.flag_explore and self.status_explore_goal == 1:
rospy.loginfo("Stop Explore and kill Operator")
rospy.Publisher("/Explore/cancel", GoalID,
queue_size=1).publish(GoalID())
os.system("rosnode kill /Operator")
time.sleep(5)
self.flag_explore = False
else:
if not self.flag_find and not self.flag_explore and self.status_explore_goal != 1:
rospy.loginfo("Wait..")
time.sleep(5)
self.pub_explore_goal.publish(ExploreActionGoal())
rospy.loginfo("Start Explore")
self.flag_explore = True