def recentBlob(self):
try:
if(rospy.time() - self.lastDetection < 1):
return self.blob_color
else:
return "lol jk fam"
except Exception:
return "Wow, you made quite the error dummy"
def recentBlob(self):
try:
if (rospy.time() - self.lastDetection < 1):
return self.blob_color
else:
return "lol jk fam"
except Exception:
return "Wow, you made quite the error dummy"
def __init__(self):
#setup node
rospy.init_node('Vel_Control_Node', anonymous=True)
self.rate = rospy.Rate(60) # refresh rate (Hz)
#topics and services
self.setpoint_velocity_pub = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=1)
self.camera_angle_pub = rospy.Publisher('/bebop/camera_control', Twist, queue_size=10)
self.running = rospy.get_param('~running',True)
self.vso_on = rospy.get_param('~vso_on',True)
self.config_file = rospy.get_param('~config_file',"default.json")
rospy.Subscriber('/orb_slam2_mono/pose', PoseStamped, self.vso_position_callback)
rospy.Subscriber('/bebop/states/ardrone3/PilotingState/AltitudeChanged', Ardrone3PilotingStateAltitudeChanged, self.altitude_callback)
rospy.Subscriber('/bebop/odom/', Odometry, self.odometry_callback)
rospy.Subscriber('/bebop/land', Empty, self.land)
rospy.Subscriber('/bebop/takeoff', Empty, self.takeoff)
rospy.Subscriber('/control/position', Point, self.position_callback)
rospy.Subscriber('/control/position_relative', Point, self.position_relative_callback)
rospy.Subscriber('/control/velocity', Point, self.velocity_callback)
rospy.Subscriber('/control/rotation', Float64, self.rotation_callback)
# rospy.Subscriber('/control/land', Empty, self.land)
rospy.Service('/control/reset_vso_coords', Trigger , self.reset_vso_position_service)
self.running_sub = rospy.Subscriber(
"control/set_running_state", Bool, self.set_running_state, queue_size=1)
self.current_pose_pub = rospy.Publisher(
"control/current_position", Point, queue_size=1)
#dynamic parameters serve
srv = Server(ControlConfig, self.parameters_callback)
t = rospy.time()
rospy.loginfo("aligning camera")
while rospy.time() - t < 3:
self.align_camera()
rospy.loginfo("setup ok")
self.goal_pose = self.current_pose
self.pid_setpoint(self.goal_pose)
def calculate_euclidean_distance(self, x_goal, y_goal):
(linear,
angular) = self.tf_listener.lookupTransform('base_link', 'map',
rospy.time(0))
current_x = linear[0]
current_y = linear[1]
distance = abs(
sqrt(((x_goal - current_x)**2) + ((y_goal - current_y)**2)))
return distance
def pub_joint_states(self):
joint_position = []
Xmin = -90
Xmax = 90
Ymin = 0
Ymax = 180
joints = self.get_object_handle()
for joint in joints:
joint_position = vrep.simxGetJointPosition(self.clientID, joint, vrep.simx_opmode_oneshot_wait)
joint_position.append(map_joint_angle(joint_position,Xmin,Xmax,Ymin,Ymax))
joint_state_msg = JointState()
joint_state_msg.header.stamp = rospy.time()
joint_state_msg.header.frame_id = "Robot_frame"
joint_state_msg.position = joint_position
self.joint_state_pub.publish(joint_state_msg)
args, unknown = parser.parse_known_args()
rospy.init_node("pick_and_place_demo")
move_group = MoveGroupInterface("manipulator", "base_link", plan_only = args.plan)
# if all we want to do is prepare the arm, do it now
if args.ready:
move_to_ready(move_group)
exit(0)
scene = PlanningSceneInterface("base_link")
pickplace = PickPlaceInterface("manipulator", "endeffector", plan_only = args.plan, verbose = True)
rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
find_objects.wait_for_server(rospy.time(5.0))
rospy.loginfo("...connected")
while not rospy.is_shutdown():
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
find_objects.send_goal(goal)
find_objects.wait_for_result(rospy.Duration(5.0))
find_result = find_objects.get_result()
rospy.loginfo("Found %d objects" % len(find_result.objects))
# remove all previous objects
for name in scene.getKnownCollisionObjects():
scene.removeCollisionObject(name, False)
for name in scene.getKnownAttachedObjects():