def run(self):
standing_wheel = 'right' if self.motor == 'left' else 'left'
rospy.loginfo('Place the robot with the %s wheel at the center of the T '
'and press the central button when the robot is ready',
standing_wheel)
self.ping()
rospy.wait_for_message('buttons/center', std_msgs.msg.Bool)
rospy.sleep(3)
msg = rospy.wait_for_message('aseba/events/ground', AsebaEvent)
self.ths = [d - self.gap for d in msg.data]
# count(3)
rospy.loginfo('Start calibrating %s motor using thresholds %s', self.motor, self.ths)
_n = 1
self.pick = None
for motor_speed in self.motor_speeds:
rospy.loginfo('Set motor speed to %s', motor_speed)
_n += self.target_number_of_samples
self.motor_speed = motor_speed
if self.motor == 'right':
data = [0, self.motor_speed]
else:
data = [self.motor_speed, 0]
self.pub.publish(AsebaEvent(data=data))
while len(self._samples) < _n:
rospy.sleep(0.05)
self.pub.publish(AsebaEvent(data=[0, 0]))
self.sub.unregister()
rospy.logdebug('Calculate with samples %s', self._samples)
self.save_samples()
samples = np.array(self._samples)
results = calibrate(samples)
rospy.loginfo('Calibration of motor %s done: %s', self.motor, results)
self.save_calibration(results)
def __init__(self, node_name):
ROS2OpenCV2.__init__(self, node_name)
self.match_threshold = rospy.get_param("~match_threshold", 0.7)
self.find_multiple_targets = rospy.get_param("~find_multiple_targets", False)
self.n_pyr = rospy.get_param("~n_pyr", 2)
self.min_template_size = rospy.get_param("~min_template_size", 25)
self.scale_factor = rospy.get_param("~scale_factor", 1.2)
self.scale_and_rotate = rospy.get_param("~scale_and_rotate", False)
self.use_depth_for_detection = rospy.get_param("~use_depth_for_detection", False)
self.fov_width = rospy.get_param("~fov_width", 1.094)
self.fov_height = rospy.get_param("~fov_height", 1.094)
self.max_object_size = rospy.get_param("~max_object_size", 0.28)
# Intialize the detection box
self.detect_box = None
self.detector_loaded = False
rospy.loginfo("Waiting for video topics to become available...")
# Wait until the image topics are ready before starting
rospy.wait_for_message("input_rgb_image", Image)
if self.use_depth_for_detection:
rospy.wait_for_message("input_depth_image", Image)
rospy.loginfo("Ready.")
def __init__(self):
'''
Initializes storage, gets parameters from parameter server and logs to rosinfo
'''
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard((9, 6), 0.03)
self.detector = CheckerboardDetector(self.board)
rospy.init_node(NODE)
self.counter = 0
# Get params from ros parameter server or use default
self.cams = rospy.get_param("~cameras")
self.camera = [self.cams["reference"]["topic"]]
for cam in self.cams["further"]:
self.camera.append(cam["topic"])
self.numCams = len(self.camera)
# Init images
self.image = []
self.image_received = [False] * self.numCams
for id in range(self.numCams):
self.image.append(Image())
# Subscribe to images
self.imageSub = []
for id in range(self.numCams):
self.imageSub.append(rospy.Subscriber(
self.camera[id], Image, self._imageCallback, id))
# Wait for image messages
for id in range(self.numCams):
rospy.wait_for_message(self.camera[id], Image, 5)
def __init__(self,name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
irols.msg.DoSeekAction,
auto_start=False)
self._as.register_goal_callback(self.goal_cb)
self._as.register_preempt_callback(self.preempt_cb)
rospy.wait_for_message('p3at/odom',Odometry)
self.alt_sp = 10
self.x_offset = 0
self.y_offset = 0
self.pos_sp = PoseStamped()
self.odom_pub = rospy.Subscriber('p3at/odom',
Odometry,
self.handle_odom)
self.curr_pos = rospy.wait_for_message('mavros/local_position/pose',
PoseStamped).pose
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.handle_local_pose)
self.state = rospy.wait_for_message('mavros/state',State)
self.state_sub = rospy.Subscriber('mavros/state',
State,
self.handle_state)
self.pos_sp_pub = rospy.Publisher('mavros/setpoint_position/local',
PoseStamped,
queue_size=3)
self.set_mode_client = rospy.ServiceProxy('mavros/set_mode',
SetMode)
self._as.start()
rospy.loginfo('{0}: online'.format(self._action_name))
def __init__(self, name, log_level=rospy.INFO, start=True):
rospy.init_node(name, log_level=log_level)
self.clear_octomap_service = rospy.ServiceProxy('/octomap_server/clear_bbx', BoundingBoxQuery)
self.reset_octomap_service = rospy.ServiceProxy('/octomap_server/reset', Empty)
is_sim = rospy.get_param("~is_simulation", False)
self.is_ready = True
rospy.wait_for_service('/obstacle_generator')
rospy.wait_for_service('/goal_manager/add_goal', 5)
rospy.wait_for_service('/move_base/make_plan', 5)
if not is_sim:
rospy.wait_for_message('/odometry/filtered', Odometry, timeout=None)
rospy.sleep(0.02)
self.generate_obstacle_service = rospy.ServiceProxy('/obstacle_generator', GenerateObstacle)
self.send_goal_service = rospy.ServiceProxy('/goal_manager/add_goal', AddGoal)
rospy.Subscriber("/goal_manager/current", GoalWithPriority, self.goal_callback)
rospy.Subscriber("/goal_manager/last_goal_reached", Bool, self.last_goal_callback)
self.tf_listener = tf.TransformListener()
self.goal_count = 0
self.tf_listener.waitForTransform("/odom", "/base_footprint", rospy.Time(), rospy.Duration(6.0))
self.is_ready = True
self.save_start_pos()
rospy.loginfo("StateAi {} started.".format(name))
if start:
self.on_start()
rospy.spin()
def __init__(self, node_name):
ROS2OpenCV2.__init__(self, node_name)
self.node_name = node_name
self.use_depth_for_detection = rospy.get_param("~use_depth_for_detection", False)
self.fov_width = rospy.get_param("~fov_width", 1.094)
self.fov_height = rospy.get_param("~fov_height", 1.094)
self.max_object_size = rospy.get_param("~max_object_size", 0.28)
# Intialize the detection box
self.detect_box = None
# Initialize a couple of intermediate image variables
self.grey = None
self.small_image = None
# What kind of detector do we want to load
self.detector_type = "template"
self.detector_loaded = False
rospy.loginfo("Waiting for video topics to become available...")
# Wait until the image topics are ready before starting
rospy.wait_for_message("input_rgb_image", Image)
if self.use_depth_for_detection:
rospy.wait_for_message("input_depth_image", Image)
rospy.loginfo("Ready.")
def get_image_topic_name(self, veh):
image_topic_name = veh + "/camera_node/image_rect"
try:
rospy.wait_for_message(image_topic_name, Image, timeout=3)
return image_topic_name
except rospy.ROSException, e:
print "%s" % e
def __init__(self):
rospy.init_node("position_test")
rospy.on_shutdown(self.shutdown)
rate = rospy.get_param("~rate", 100)
r = rospy.Rate(rate)
self.head_pan_joint = rospy.get_param("~head_pan_joint", "head_pan_joint")
self.joints = [self.head_pan_joint]
self.default_joint_speed = rospy.get_param("~default_joint_speed", 0.2)
self.joint_state = JointState()
rospy.Subscriber("joint_states", JointState, self.update_joint_state)
while not rospy.is_shutdown():
rospy.sleep(1)
rospy.loginfo("waiting for joint_states")
rospy.wait_for_message("joint_states", JointState)
current_pan = self.joint_state.position[self.joint_state.name.index(self.head_pan_joint)]
print "A"
print current_pan
print "B"
print current_pan * 180 / math.pi
def takeObservation(self):
# TODO: Using wait_for_message for now, might change to ApproxTimeSync later
poseData = None
while poseData is None:
try:
# poseData = rospy.wait_for_message('/ground_truth_to_tf/pose', PoseStamped, timeout=5)
poseData = rospy.wait_for_message('/ground_truth/state', Odometry, timeout=5)
except:
rospy.loginfo("Current drone pose not ready yet, retrying to get robot pose")
velData = None
while velData is None:
try:
velData = rospy.wait_for_message('/fix_velocity', Vector3Stamped, timeout=5)
except:
rospy.loginfo("Current drone velocity not ready yet, retrying to get robot velocity")
imuData = None
while imuData is None:
try:
imuData = rospy.wait_for_message('/raw_imu', Imu, timeout=5)
except:
rospy.loginfo("Current drone imu not ready yet, retrying to get robot imu")
motorData = None
while motorData is None:
try:
motorData = rospy.wait_for_message('/motor_status', MotorStatus, timeout=5)
except:
rospy.loginfo("Current drone motor status not ready yet, retrying to get robot motor status")
return poseData, imuData, velData, motorData
def __init__(self, image_topic='/spiky/retina_image'):
super(BaseNetworkIn, self).__init__()
self._postsynaptic_learning_neurons = []
# Populations
self.pop_input_image_r = None
self.pop_input_image_l = None
self.pop_encoded_image_l = None
self.pop_encoded_image_r = None
self.spikedetector_enc_image_l = None
self.spikedetector_enc_image_r = None
# Preparing sensory information subscriber
self._bridge = CvBridge()
self._last_frame = None
self._retina_image_subscriber = rospy.Subscriber(image_topic, Image, self._handle_frame, queue_size=1)
rospy.wait_for_message(image_topic, Image)
if self._last_frame is not None:
shape = np.shape(self._last_frame)
self._width = shape[0]
self._height = shape[1]
self._build_input_layer()
self._create_spike_detectors()
def test_video_camera(self):
rospy.init_node('morse_ros_video_testing', log_level=rospy.DEBUG)
motion_topic = '/robot/motion'
camera_topic = '/robot/camera/image'
camnfo_topic = '/robot/camera/camera_info'
pub_vw(motion_topic, 1, 1)
old = []
for step in range(2):
msg = rospy.wait_for_message(camnfo_topic, CameraInfo, 10)
camera_info_frame = msg.header.frame_id
# might want to add more CameraInfo test here
msg = rospy.wait_for_message(camera_topic, Image, 10)
self.assertEqual(msg.header.frame_id, camera_info_frame)
self.assertEqual(len(msg.data), 128*128*4) # RGBA
# dont use assertNotEqual here
# dumping raw image data in log is not relevant
self.assertTrue(msg.data != old)
old = msg.data
time.sleep(0.2) # wait for turning
def __init__(self):
# initialize ROS node
rospy.init_node('target_detector', anonymous=True)
# initialize publisher for target pose, PoseStamped message, and set initial sequence number
self.pub = rospy.Publisher('target_pose', PoseStamped, queue_size=1)
self.pub_pose = PoseStamped()
self.pub_pose.header.seq = 0
self.rate = rospy.Rate(1.0) # publish message at 1 Hz
# initialize values for locating target on Kinect v2 image
self.target_u = 0 # u is pixels left(0) to right(+)
self.target_v = 0 # v is pixels top(0) to bottom(+)
self.target_d = 0 # d is distance camera(0) to target(+) from depth image
self.target_found = False # flag initialized to False
self.last_d = 0 # last non-zero depth measurement
# target orientation to Kinect v2 image (Euler)
self.r = 0
self.p = 0
self.y = 0
# Convert image from a ROS image message to a CV image
self.bridge = CvBridge()
# Wait for the camera_info topic to become available
rospy.wait_for_message('/kinect2/qhd/image_color_rect', Image)
# Subscribe to registered color and depth images
rospy.Subscriber('/kinect2/qhd/image_color_rect', Image, self.image_callback, queue_size=1)
rospy.Subscriber('/kinect2/qhd/image_depth_rect', Image, self.depth_callback, queue_size=1)
self.rate.sleep() # suspend until next cycle
def run(self):
# Waits until everything has been initialized
rospy.wait_for_message("/position", PoseStamped, timeout=5)
prev = [0, 0]
r = rospy.Rate(10)
if not rospy.is_shutdown():
for i in self.contours:
for j in i:
if self.first:
self.pub_marker.publish(String("0"))
print "pen up"
else:
self.pub_marker.publish(String("1"))
print "pen down"
pos = [j[0], -j[1]]
rot = self.calc_theta(prev[0], prev[1], pos[0], pos[1])
self.push_waypoint(pos, rot)
prev = pos
self.first = False
print "contour"
self.first = True # Marker should be up when going to the first point of each new contour
self.pub_marker.publish(String("0"))
self.push_waypoint(0, 0)
self.th_f = 0
self.rotate_neato()
self.pub_vel.publish(Twist())
cv2.waitKey(0)
def move_interrupt():
g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory()
g.trajectory.joint_names = JOINT_NAMES
try:
joint_states = rospy.wait_for_message("joint_states", JointState)
joints_pos = joint_states.position
g.trajectory.points = [
JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
client.send_goal(g)
time.sleep(3.0)
print "Interrupting"
joint_states = rospy.wait_for_message("joint_states", JointState)
joints_pos = joint_states.position
g.trajectory.points = [
JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
client.send_goal(g)
client.wait_for_result()
except KeyboardInterrupt:
client.cancel_goal()
raise
except:
raise
def run(self):
"""Loop, waiting for the user to press enter to take laser+camera snapshots"""
while not rospy.is_shutdown():
print "Press enter to grab a camera-laser pair, or q to quit"
input_str = raw_input()
if "q" in input_str:
break
try:
laser_msg = rospy.wait_for_message(self.scan_topic, LaserScan, timeout=0.25)
except rospy.exceptions.ROSException as err:
print "Failed to receive laser message: " + str(err)
continue
try:
camera_msg = rospy.wait_for_message(self.camera_topic, Image, timeout=0.25)
except rospy.exceptions.ROSException as err:
print "Failed to receive camera message: " + str(err)
continue
#generate the laser scan string to write to file
laser_str = self.process_laser_scan(laser_msg)
#write the image to bmp file
try:
self.process_camera_image(camera_msg)
except CvBridgeError:
print "Failed to process camera image, tossing data"
continue
#only add the laser string to the file if the image wrote successfully
self.image_number += 1
self.laser_file.write(laser_str)
def __init__(self):
self.node_name = "cv_bridge_demo"
rospy.init_node(self.node_name)
# What we do during shutdown
rospy.on_shutdown(self.cleanup)
# Create the OpenCV display window for the RGB image
self.cv_window_name = self.node_name
cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL)
cv.MoveWindow(self.cv_window_name, 25, 75)
# And one for the depth image
cv.NamedWindow("Depth Image", cv.CV_WINDOW_NORMAL)
cv.MoveWindow("Depth Image", 25, 350)
# Create the cv_bridge object
self.bridge = CvBridge()
# Subscribe to the camera image and depth topics and set
# the appropriate callbacks
self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback, queue_size=1)
self.depth_sub = rospy.Subscriber("input_depth_image", Image, self.depth_callback, queue_size=1)
rospy.loginfo("Waiting for image topics...")
rospy.wait_for_message("input_rgb_image", Image)
rospy.loginfo("Ready.")
def run(self):
rospy.wait_for_message('/desired_joint_states', JointState)
rospy.wait_for_message('/joint_states', JointState)
while not rospy.is_shutdown():
with self.lock:
for controller in self.controller_list:
if controller.is_active():
num_joints_stopped = 0
for joint in controller.joints:
desired_position = self.get_desired_position(joint)
current_position = self.get_current_position(joint)
difference = abs(current_position - desired_position)
joint.set_position(desired_position)
#rospy.loginfo('current_position: {0}, desired_position: {1}, diff: {2}'.format(current_position, desired_position, difference))
if difference < 0.05:
num_joints_stopped += 1
#TODO: set finished if average change of each joint is little
#rospy.loginfo('num_joints_stopped: {0}, joints: {1}'.format(num_joints_stopped, len(controller.joints)))
if controller.iterations > 5 and controller.joints_stopped is False and num_joints_stopped == len(controller.joints):
controller.joints_stopped = True
controller.publish_joints_stopped()
controller.iterations += 1
self.rate.sleep()
def test_all(self):
rospy.wait_for_message('/static_image_publisher/output', Image)
rospy.sleep(2)
save_dir = rospy.get_param('/save_dir_all')
save_dir = osp.expanduser(save_dir)
self.check(save_dir, target='image')
def spin(self):
li = rospy.wait_for_message("/my_stereo/left/image_rect_color", Image)
ri = rospy.wait_for_message("/my_stereo/right/image_rect_color", Image)
left = self.callback_img(li, "left")
right = self.callback_img(ri, "right")
sub = self.sub_img(left, right)
return np.sum(sub)
def send_to_joint_vals(q, arm='right'):
pub_joint_cmd=rospy.Publisher('/robot/limb/' + arm + '/joint_command',JointCommand)
command_msg=JointCommand()
command_msg.names = generate_joint_names_for_arm(arm)
command_msg.command=q
command_msg.mode=JointCommand.POSITION_MODE
control_rate = rospy.Rate(100)
start = rospy.get_time()
joint_positions=rospy.wait_for_message("/robot/joint_states",JointState)
qc = joint_positions.position[9:16]
qc = ([qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6]])
print('q,qc')
print(q)
print(qc)
while not rospy.is_shutdown() and numpy.linalg.norm(numpy.subtract(q,qc))> 0.07:
pub_joint_cmd.publish(command_msg) # sends the commanded joint values to Baxter
control_rate.sleep()
joint_positions=rospy.wait_for_message("/robot/joint_states",JointState)
qc = (joint_positions.position[9:16])
qc = (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6])
print "joint error = ", numpy.linalg.norm(numpy.subtract(q,qc))
print((q,qc))
print("In home pose")
return (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6])
def __init__(self, goal) :
rospy.loginfo("Waiting for Topological map ...")
try:
msg = rospy.wait_for_message('/topological_map', TopologicalMap, timeout=10.0)
self.top_map = msg
self.lnodes = msg.nodes
except rospy.ROSException :
rospy.logwarn("Failed to get topological map")
return
rospy.loginfo("Waiting for Current Node ...")
try:
msg = rospy.wait_for_message('/closest_node', String, timeout=10.0)
cnode = msg.data
except rospy.ROSException :
rospy.logwarn("Failed to get closest node")
return
rsearch = TopologicalRouteSearch(self.top_map)
route = rsearch.search_route(cnode, goal)
print route
rospy.loginfo("All Done ...")
def __init__(self):
rospy.init_node('goface',anonymous=True)
rospy.on_shutdown(self.shutdown)
self.rest_time = rospy.get_param("~rest_time",10)
self.fake_test = rospy.get_param("~fake_test",False)
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED','LOST']
self.move_base = actionlib.SimpleActionClient("move_base",MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
self.initial_pose = PoseWithCovarianceStamped()
rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
self.goalpub = rospy.Publisher('thegoal',Pose)
while self.initial_pose.header.stamp == "":
rospy.sleep(1)
self.goal = MoveBaseGoal()
rospy.loginfo("Starting navigation test")
rospy.Subscriber("/destination_face",Pose,self.faceCallback)
def __init__(self):
# Initialize constants
self.error_threshold = 0.0175 # Report success if error reaches below threshold
self.signal = 1
# Initialize new node
rospy.init_node(NAME, anonymous=True)
controller_name = rospy.get_param('~controller')
# Initialize publisher & subscriber for tilt
self.laser_tilt = JointControllerState()
self.laser_tilt_pub = rospy.Publisher(controller_name + '/command', Float64)
self.laser_signal_pub = rospy.Publisher('laser_scanner_signal', LaserScannerSignal)
self.joint_speed_srv = rospy.ServiceProxy(controller_name + '/set_speed', SetSpeed, persistent=True)
rospy.Subscriber(controller_name + '/state', JointControllerState, self.process_tilt_state)
rospy.wait_for_message(controller_name + '/state', JointControllerState)
# Initialize tilt action server
self.result = HokuyoLaserTiltResult()
self.feedback = HokuyoLaserTiltFeedback()
self.feedback.tilt_position = self.laser_tilt.process_value
self.server = SimpleActionServer('hokuyo_laser_tilt_action', HokuyoLaserTiltAction, self.execute_callback)
rospy.loginfo("%s: Ready to accept goals", NAME)
def cruise(pose_number):
marker_pub = rospy.Publisher('curse_markers', Marker, queue_size=1)
marker=marker_define()
intial=rospy.wait_for_message("odom",Odometry)
intial_point=PointStamped()
intial_point.point=intial.pose.pose.position
intial_point.header.stamp=rospy.Time.now()
intial_point.header.frame_id='map'
marker.points=[intial_point.point]
pose_list=[]
for i in range(pose_number):
rospy.loginfo('请在地图上用 publish point 输入第%s个您希望机器人到达的位置'%(i+1))
pose=rospy.wait_for_message("clicked_point", PointStamped)
pose_list.append(pose)
marker.points.append(pose.point)
print 'position',1+i,'recieved'
pose_list.append(intial_point)
marker_pub.publish(marker)
quest=raw_input('您希望巡航（即机器人在选的位置之间循环往复）吗？y/n: ')
if quest=='y':
times=raw_input('请输入巡航的次数，或者机器人默认将会无限次循环：')
try:
times=int(times)
for i in range(times):
rospy.loginfo('第%s轮导航开始'%(i+1))
tasks(pose_number,pose_list)
except:
while not rospy.is_shutdown():
rospy.loginfo('无限次导航开始')
tasks(pose_number,pose_list)
else:
rospy.loginfo('单次导航开始')
tasks(pose_number,pose_list)
def __init__(self):
# Initialize constants
self.JOINTS_COUNT = 2 # Number of joints to manage
self.ERROR_THRESHOLD = 0.01 # Report success if error reaches below threshold
self.TIMEOUT_THRESHOLD = rospy.Duration(5.0) # Report failure if action does not succeed within timeout threshold
# Initialize new node
rospy.init_node(NAME, anonymous=True)
# Initialize publisher & subscriber for left finger
self.left_finger_frame = 'arm_left_finger_link'
self.left_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.left_finger_pub = rospy.Publisher('finger_left_controller/command', Float64)
rospy.Subscriber('finger_left_controller/state', JointControllerState, self.read_left_finger)
rospy.wait_for_message('finger_left_controller/state', JointControllerState)
# Initialize publisher & subscriber for right finger
self.right_finger_frame = 'arm_right_finger_link'
self.right_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.right_finger_pub = rospy.Publisher('finger_right_controller/command', Float64)
rospy.Subscriber('finger_right_controller/state', JointControllerState, self.read_right_finger)
rospy.wait_for_message('finger_right_controller/state', JointControllerState)
# Initialize action server
self.result = SmartArmGripperResult()
self.feedback = SmartArmGripperFeedback()
self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
self.server = SimpleActionServer(NAME, SmartArmGripperAction, self.execute_callback)
# Reset gripper position
rospy.sleep(1)
self.reset_gripper_position()
rospy.loginfo("%s: Ready to accept goals", NAME)
def __init__(self):
self._last_frame = None
self._bridge = CvBridge()
self.simduration = 10
self.learner = MockLearner()
rospy.init_node('SNN_Cockpit', disable_signals=True)
self._retina_image_subscriber = rospy.Subscriber('/spiky/retina_image', rosmsg.Image, self._handle_frame,
queue_size=1)
rospy.wait_for_message('/spiky/retina_image', rosmsg.Image)
if self._last_frame is not None:
shape = np.shape(self._last_frame)
self._width = shape[0]
self._height = shape[1]
self.postsynaptic_learning_neurons = [5033, 5034, 5039, 5040, 5041, 5042, 5043, 5044, 5045, 5046, 5047, 5048]
self.plastic_connections = [[5027, 5039, 0, 50, 0], [5027, 5040, 0, 50, 1], [5027, 5041, 0, 50, 2],
[5027, 5042, 0, 50, 3], [5027, 5043, 0, 50, 4], [5027, 5044, 0, 50, 5],
[5027, 5045, 0, 50, 6], [5027, 5046, 0, 50, 7], [5027, 5047, 0, 50, 8],
[5027, 5048, 0, 50, 9], [5028, 5039, 0, 50, 0], [5028, 5040, 0, 50, 1],
[5028, 5041, 0, 50, 2], [5028, 5042, 0, 50, 3], [5028, 5043, 0, 50, 4],
[5028, 5044, 0, 50, 5], [5028, 5045, 0, 50, 6], [5028, 5046, 0, 50, 7],
[5028, 5047, 0, 50, 8], [5028, 5048, 0, 50, 9], [5039, 5033, 0, 51, 0],
[5039, 5034, 0, 51, 1], [5040, 5033, 0, 51, 0], [5040, 5034, 0, 51, 1],
[5041, 5033, 0, 51, 0], [5041, 5034, 0, 51, 1], [5042, 5033, 0, 51, 0],
[5042, 5034, 0, 51, 1], [5043, 5033, 0, 51, 0], [5043, 5034, 0, 51, 1],
[5044, 5033, 0, 51, 0], [5044, 5034, 0, 51, 1], [5045, 5033, 0, 51, 0],
[5045, 5034, 0, 51, 1], [5046, 5033, 0, 51, 0], [5046, 5034, 0, 51, 1],
[5047, 5033, 0, 51, 0], [5047, 5034, 0, 51, 1], [5048, 5033, 0, 51, 0],
[5048, 5034, 0, 51, 1]]
self.spike_events = [{'senders': [5027, 5027, 5027]}, {'senders': [5028, 5028]}]
self.weights = np.random.rand(len(self.plastic_connections))
def main():
rospy.init_node(NAME)
# Init Globals with their type
global status_list
status_list = []
global cant_reach_list
cant_reach_list = []
global actionGoalPublisher
# Speech stuff
global soundClient
soundClient = SoundClient()
init_point2pont()
actionStatus = rospy.Subscriber('move_base/status', GoalStatusArray, status_callback)
actionGoalPublisher = rospy.Publisher('move_base/goal', MoveBaseActionGoal, queue_size=10)
frontierSub = rospy.Subscriber('grid_frontier', GridCells, frontier_callback)
rospy.wait_for_message('grid_frontier', GridCells)
rospy.sleep(1)
driveTo.goalID = getHighestStatus()
soundClient.say("Starting Driving")
global closestGoal
while not rospy.is_shutdown():
rospy.loginfo("Waiting for a little while")
rospy.sleep(2) #Allow the goal to be calculated
rospy.loginfo("Getting the closest goal")
closesGoalCopy = getClosestGoal(2)
rospy.loginfo("Getting the current location")
driveTo(closesGoalCopy[0], closesGoalCopy[1], closesGoalCopy[2])
rospy.spin()
def detect(self, timeout=10):
marker_message = rospy.wait_for_message(self.marker_topic, MarkerArray, timeout=timeout)
camera_message = rospy.wait_for_message(self.camera_topic, CameraInfo, timeout=timeout)
camera_matrix = np.array([0])
for marker in marker_message.markers:
if marker.id == self.marker_number:
marker_pose = np.array(
quaternion_matrix(
[
marker.pose.orientation.x,
marker.pose.orientation.y,
marker.pose.orientation.z,
marker.pose.orientation.w,
]
)
)
marker_pose[0, 3] = marker.pose.position.x
marker_pose[1, 3] = marker.pose.position.y
marker_pose[2, 3] = marker.pose.position.z
marker_pose = np.delete(marker_pose, 3, 0)
camera_intrinsics = np.array(camera_message.K).reshape(3, 3)
print camera_intrinsics
print marker_pose
camera_matrix = np.dot(camera_intrinsics, marker_pose)
print camera_matrix
return camera_matrix
def listener1():
#Initializations
global bridge
rospy.init_node('listener', anonymous=True)
while not rospy.is_shutdown():
res = raw_input("Press enter to continue")
#Get IR image
messageir = rospy.wait_for_message("/camera/ir/image_raw", Image)
print 'received image of type: "%s"' % messageir.header
print 'received image width = %s and height = %s' %(messageir.width, messageir.height)
# print 'data type: %s' % data.encoding
try:
cv_image_ir = bridge.imgmsg_to_cv(messageir, "mono16")
except CvBridgeError, e:
print e
cv_image_ir = np.array(cv_image_ir, dtype=np.uint16)
cv_image_ir = cv2.convertScaleAbs(cv_image_ir, alpha=1.0)
#cv_image8 = cv2.medianBlur(cv_image8,3)
#Get RGB image
messagergb = rospy.wait_for_message("/camera/rgb/image_color", Image)
print 'received image of type: "%s"' % messagergb.header
print 'received image width = %s and height = %s' %(messagergb.width, messagergb.height)
try:
cv_image_rgb = bridge.imgmsg_to_cv(messagergb, "bgr8")
except CvBridgeError, e:
print e
def feedback_cb(self, feedback):
if not self.in_feedback :
self.in_feedback=True
try:
current = rospy.wait_for_message('/current_node', std_msgs.msg.String, timeout=10.0)
except rospy.ROSException :
rospy.logwarn("Failed to get current node")
return
print current.data
if current.data == 'none':
try:
pos = rospy.wait_for_message('/robot_pose', Pose, timeout=10.0)
except rospy.ROSException :
rospy.logwarn("Failed to get robot pose")
return
add_node = rospy.ServiceProxy('/topological_map_manager/add_topological_node', strands_navigation_msgs.srv.AddNode)
add_node('',pos)
map_update = rospy.Publisher('/update_map', std_msgs.msg.Time)
map_update.publish(rospy.Time.now())
else:
rospy.loginfo("I will NOT add a node within the influence area of another! Solve this and try again!")
self.timer = Timer(1.0, self.timer_callback)
self.timer.start()
def get_question(self, client_context):
"""Wait for new message"""
return rospy.wait_for_message(self._subscription, String)
def __init__(self):
rospy.init_node('nav_test', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 10)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
locations = dict()
locations['couloir_milieu'] = Pose(Point(-1.35, 9.31, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743))
locations['i5_droite'] = Pose(Point(1, 13, 0.000), Quaternion(0.000, 0.000, 0.892, -0.451))
locations['i5_gauche'] = Pose(Point(1.13, 6.94, 0.000), Quaternion(0.000, 0.000, 0.480, 0.877))
locations['goal'] = Pose(Point(-1.82, 11.3, 0.00641), Quaternion(0.000, 0.000, 0.892, -0.451))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 10 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(10))
# subscribe au topic chatter, avec un appel a la fonction de callback
rospy.Subscriber("/chatter",String, Move)
rospy.Subscriber("/arret",String, Stop)
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = 3
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# Get the initial pose from the user
rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown() :
rospy.loginfo("*** MODE SEQUENTIEL")
pub_mode = rospy.Publisher("activity_mode", String, queue_size=15)
pub_mode.publish("SEQUENTIEL")
# If we've gone through the current sequence,
# start with a new random sequence
if i == n_locations:
i = 0
#sequence = [sample(locations, n_locations)]
# On remplace l'aleatoire des positions par un ordre de position bien defini
sequence = ["i5_droite","couloir_milieu","i5_gauche"]
rospy.loginfo("sequence "+str(sequence))
# Skip over first location if it is the same as
# the last location
#if sequence[0] == last_location:
# i = 1
# Get the next location in the current sequence
location = sequence[i]
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(pow(locations[location].position.x -
locations[last_location].position.x, 2) +
pow(locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(pow(locations[location].position.x -
initial_pose.pose.pose.position.x, 2) +
pow(locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes/n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " + str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def main():
# Init ROS node
rospy.init_node('task', anonymous=True)
# Create subscriber for position, goal, boost points, and obstacles
rospy.Subscriber('/goal', Pose, goal_callback)
rospy.Subscriber('/boost', PoseArray, boost_callback)
rospy.Subscriber("/dynamic_obstacles", PoseArray,
dynamic_obstacles_callback)
# Wait for resources to become active
goal = rospy.wait_for_message("/goal", Pose).position
boosts = rospy.wait_for_message("/boost", PoseArray).poses
obstacles = rospy.wait_for_message("/dynamic_obstacles", PoseArray).poses
# Create map service client
getMap = rospy.ServiceProxy('/GlobalMap', GlobalMap)
rospy.wait_for_service('/GlobalMap')
try:
raw_map = getMap()
except rospy.ServiceException as e:
print("Map service error: " + str(e))
return
# Get map as 2D list
world_map = util.parse_map(raw_map)
# Print resources
print("Wall layout:")
util.print_map(world_map)
print("Boost points:")
util.print_positions(boosts)
print("Obstacles at start:")
util.print_positions(obstacles)
# Initialize drone
drone = Drone()
drone.takeoff()
# -- For example code --
target_x = 0
target_y = 0
rate = rospy.Rate(30)
while not rospy.is_shutdown():
rate.sleep()
# -------------------------------
# ------Replace this example-----
# ---------with your code!-------
# -------------------------------
# Find how far we are away from target
distance_to_target = ((target_x - drone.position.x)**2 +
(target_y - drone.position.y)**2)**0.5
# Do special action if we are close
if distance_to_target < 0.5:
# Print current distance to goal. Note that we
# wont reach the goal, since we just move randomly
distance_to_goal = ((drone.position.x - goal.x)**2 +
(drone.position.y - goal.y)**2)**0.5
print("Distance to goal is now", distance_to_goal)
# Generate some random point and rotation
target_x = random.randint(-3, 3)
target_y = random.randint(-3, 3)
# Move to random point
drone.set_target(target_x, target_y)
else:
print("distance to target:", distance_to_target)
def inferred_point_callback(msg):
one_shot = False
while not one_shot:
data = rospy.wait_for_message(
'/dish_edge_detector/output_curvature_edge', PointCloud2)
one_shot = True
gen = point_cloud2.read_points(data,
field_names=("x", "y", "z"),
skip_nans=True)
length = 1
A = np.empty(3, dtype=float).reshape(1, 3)
## Whole edge (x,y,z) points
for l in gen:
l = np.array(l, dtype='float')
l = l.reshape(1, 3)
A = np.append(A, l, axis=0)
length += 1
#
## Randomly choose one grasp point
#idx = np.random.randint(length, size=1) #To do : change 10 to data length
#Ax = A[idx, 0]
#Ay = A[idx, 1]
#Az = A[idx, 2]
Ax = msg.point.x
Ay = msg.point.y
Az = msg.point.z
print("==============================")
print(Ax, Ay, Az)
print("==============================")
"""
Romdomly choose rotation theta from 0, 45, 90. (currently, 90 for test).
Other roatation angle is fixed toward middle point.
But currently, rotation is fixed for test.
"""
# euler angle will be strange when converting in eus program. Adjust parameter until solving this problem.
#phi_list = [math.pi/2, math.pi*4/6, math.pi*5/6]
phi_list = [math.pi / 2, math.pi * 4 / 6, math.pi * 5 / 6]
theta = 0 #-1.54
phi = random.choice(phi_list) #1.2(recentry, 2.0)
psi = 0
phi = math.pi * 5 / 6
random_grasp_posrot = np.array(
(Ax, Ay, Az, phi), dtype='float').reshape(1, 1, 4) #reshape(1,4)
print("random grasp posrost", random_grasp_posrot)
random_grasp_posrot_arr = InflateGraspPoint(
random_grasp_posrot.reshape(1, 4)).calc()
print(random_grasp_posrot_arr)
# Inference!
inferred_posrot_list = []
grasp_posrot_list = []
posearray = PoseArray()
header = posearray.header
header.stamp = rospy.Time(0)
header.frame_id = "head_mount_kinect_rgb_optical_frame"
for i in range(8):
ts = TestSystem()
inferred_grasp_point = ts.test(random_grasp_posrot_arr[i])
inferred_grasp_point = inferred_grasp_point.reshape(4)
# Find nearest edge point based on inferred point
nearest = nearest_point(A, inferred_grasp_point[:3:])
Ax = nearest[0]
Ay = nearest[1]
Az = nearest[2]
phi = inferred_grasp_point[3] - 0.5
# When converting from here to eus,
"""
if phi < 1.6:
q_phi = 0
else:
q_phi = phi
"""
if (phi < 1.6):
phi = 1.6
elif (phi > 2.6):
phi = 2.6
q = tf.transformations.quaternion_from_euler(theta, phi, psi)
pose = Pose()
pose.position.x = Ax
pose.position.y = Ay
pose.position.z = Az
pose.orientation.x = q[0]
pose.orientation.y = q[1]
pose.orientation.z = q[2]
pose.orientation.w = q[3]
posearray.poses.append(pose)
# grasp_posrot is actual grasp point
grasp_posrot = np.array((Ax, Ay, Az, phi), dtype='float').reshape(1, 4)
grasp_posrot_list.append(grasp_posrot)
print("nearest_inferred_grasp_point", grasp_posrot)
# inferred_posrot is a inferred point but not for grasping
inferred_posrot = np.array(
(inferred_grasp_point[0], inferred_grasp_point[1],
inferred_grasp_point[2], inferred_grasp_point[3]))
inferred_posrot_list.append(inferred_posrot)
# Save grasp_posrot and inferred_posrot
now = datetime.datetime.now()
walltime = str(int(time.time() * 1000000000))
filename = 'Data/inferred_grasp_point/' + walltime + '.pkl'
with open(filename, "wb") as f:
pickle.dump(grasp_posrot, f)
print("saved inferred grasp point")
filename = 'Data/inferred_point/' + walltime + '.pkl'
with open(filename, "wb") as ff:
pickle.dump(inferred_posrot, ff)
print("saved inferred point")
pub.publish(posearray)
class GazeboTurtlebotComplicatedMazeEnv(gazebo_env.GazeboEnv):
def __init__(self):
# Launch the simulation with the given launchfile name
gazebo_env.GazeboEnv.__init__(self,
"TurtlebotComplicatedMaze_v0.launch")
self.vel_pub = rospy.Publisher('/mobile_base/commands/velocity',
Twist,
queue_size=5)
self.unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
self.pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
self.reset_proxy = rospy.ServiceProxy('/gazebo/reset_simulation',
Empty)
self.name_model = 'mobile_base'
# self.name_target = 'TargetCylinder'
self.name_hint = 'Hint'
#get model state service
self.model_state = rospy.ServiceProxy('gazebo/get_model_state',
GetModelState)
self.physics_properties = rospy.ServiceProxy(
'gazebo/get_physics_properties', GetPhysicsProperties)
self.action_space = spaces.Discrete(19)
self.reward_range = (-np.inf, np.inf)
self.count_loop = [0] * 50
self.target_pos = [[-3, -3.5], [-3.5, 0], [3.5, -3.5], [3, 1.5]]
self.rand_target = 0
self.hint = [[3, 1], [4, 3]]
self.set_model = rospy.ServiceProxy('gazebo/set_model_state',
SetModelState)
rospy.wait_for_service('gazebo/set_model_state')
self.channel = 1
self.width = 32
self.height = 32
self.num_action = 7
self.last_distance = 10000
self.old_pose = 0
self.num_state = 105
self.num_target = 4
self._seed()
def calculate_observation(self, data, distance):
min_range = 0.21
target_min_range = 0.21
done = 0
for i, item in enumerate(data.ranges):
if (min_range > data.ranges[i] > 0):
done = 1
if (distance < target_min_range):
done = 2
return done
def setTarget(self):
state = ModelState()
state.model_name = self.name_target
state.reference_frame = "world"
state.pose.position.x = self.target_pos[self.rand_target][0]
state.pose.position.y = self.target_pos[self.rand_target][1]
self.set_model(state)
def discretize_observation(self, data, vel_cmd, pos, new_pose, twist,
num_step, distance):
observation = []
mod = len(data.ranges) / 100
for i, item in enumerate(data.ranges):
if (i % mod == 0):
if data.ranges[i] == float('Inf') or np.isinf(data.ranges[i]):
observation.append(21)
elif np.isnan(data.ranges[i]):
observation.append(0)
else:
observation.append(data.ranges[i])
observation.append(twist.linear.x)
observation.append(twist.angular.z)
observation.append(self.target_pos[self.rand_target][0] - pos.x)
observation.append(self.target_pos[self.rand_target][1] - pos.y)
observation.append(distance)
#observation.append(num_step)
# print vel_cmd
# print twist
# observation = [self.old_pose.position.x, self.old_pose.position.y, self.old_pose.orientation.x, self.old_pose.orientation.y, self.old_pose.orientation.z, self.old_pose.orientation.w, new_pose.position.x, new_pose.position.y, new_pose.orientation.x, new_pose.orientation.y, new_pose.orientation.z, new_pose.orientation.w]
"""image_data = None
cv_image = None
n = 0
while image_data is None:
try:
image_data = rospy.wait_for_message('/camera/rgb/image_raw', Image, timeout=5)
h = image_data.height
w = image_data.width
cv_image = CvBridge().imgmsg_to_cv2(image_data, "bgr8")
except:
n += 1
if n == 10:
print "Camera error"
state = []
done = True
return state
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
cv_image = cv2.resize(cv_image, (self.height, self.width))
cv2.imshow("image", cv_image)
cv2.waitKey(3)
cv_image = cv_image.reshape(1, 32, 32, 1)
# print cv_image.shape"""
return np.asarray(observation)
def _seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def calculate_distance(self, x, y, a, b):
return math.sqrt((x - a) * (x - a) + (y - b) * (y - b))
def calculate_reward(self, done, pos, vel_cmd, num_step):
min_range = 0.21
target_min_range = 0.21
reward = -1
distance = self.calculate_distance(
pos.x, pos.y, self.target_pos[self.rand_target][0],
self.target_pos[self.rand_target][1])
reward = (self.last_distance - distance) * 100
if done == 2:
reward = 200
elif done == 1:
reward = -200
"""else:
for i in range(2):
if (self.calculate_distance(pos.x, pos.y, self.hint[i][0], self.hint[i][1]) < min_range):
reward = 10"""
return reward
def _step(self, action):
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except rospy.ServiceException, e:
print("/gazebo/unpause_physics service call failed")
#print action
num_step = action[1]
action = action[0]
vel_cmd = Twist()
max_linear_speed = 2
linear_vel = (math.floor(
action / 7)) * max_linear_speed * 0.1 #(0, 0.2, 0.4)
vel_cmd.linear.x = 0.2
max_ang_speed = 3.3
ang_vel = (action - 3) * max_ang_speed * 0.1 #from (-1 to + 1)
vel_cmd.angular.z = ang_vel
self.vel_pub.publish(vel_cmd)
data = None
while data is None:
try:
data = rospy.wait_for_message('/scan', LaserScan, timeout=5)
except:
pass
pos = self.model_state(self.name_model, "world").pose
twist = self.model_state(self.name_model, "world").twist
new_pose = pos
pos = pos.position
#print pos
distance = math.sqrt((pos.x - self.target_pos[self.rand_target][0]) *
(pos.x - self.target_pos[self.rand_target][0]) +
(pos.y - self.target_pos[self.rand_target][1]) *
(pos.y - self.target_pos[self.rand_target][1]))
done = self.calculate_observation(data, distance)
state = self.discretize_observation(data, vel_cmd, pos, new_pose,
twist, num_step, distance)
rospy.wait_for_service('/gazebo/pause_physics')
try:
#resp_pause = pause.call()
self.pause()
except rospy.ServiceException, e:
print("/gazebo/pause_physics service call failed")
#reset_proxy.call()
self.reset_proxy()
except rospy.ServiceException, e:
print("/gazebo/reset_simulation service call failed")
# Unpause simulation to make observation
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except rospy.ServiceException, e:
print("/gazebo/unpause_physics service call failed")
#read laser data
data = None
while data is None:
try:
data = rospy.wait_for_message('/scan', LaserScan, timeout=5)
except:
pass
pos = self.model_state(self.name_model, "world").pose
twist = self.model_state(self.name_model, "world").twist
self.old_pose = pos
pos = pos.position
self.last_distance = math.sqrt(
(pos.x - self.target_pos[self.rand_target][0]) *
(pos.x - self.target_pos[self.rand_target][0]) +
(pos.y - self.target_pos[self.rand_target][1]) *
(pos.y - self.target_pos[self.rand_target][1]))
vel_cmd = Twist()
state = self.discretize_observation(data, vel_cmd, pos, self.old_pose,
twist, 0, self.last_distance)
rospy.wait_for_service('/gazebo/pause_physics')
def take_picture(title):
rospy.sleep(1)
img = bridge.imgmsg_to_cv2(
rospy.wait_for_message('main_camera/image_raw', Image), 'bgr8')
cv2.imwrite(title, img)
return img
pub_nbcomponents = rospy.Publisher("/pose_estimation/nb_components",
std_msgs.msg.String,
queue_size=1)
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
broadcaster = tf2_ros.TransformBroadcaster()
static_broadcaster = tf2_ros.StaticTransformBroadcaster()
## Parameters
distance_threshold_plane = 0.004
voxel_size = 1.0
print(" :: Waiting for the scene reconstructed point cloud.")
data = rospy.wait_for_message("/pose_estimation/scene_reconstructed",
sensor_msgs.msg.PointCloud2,
rospy.Duration(1000.0))
scene_pcd = orh.rospc_to_o3dpc(data, remove_nans=True)
print(":: Loading cad_model point cloud.")
cad_file_path = "/home/carlos/git/3DVisionMobileManipulation/catkin_ws/src/pose_estimation_pkg/data/m200.ply"
cad_model_pcd, origin_frame = load_cad_model(path=cad_file_path)
o3d.visualization.draw_geometries([cad_model_pcd, origin_frame])
source, source_down, source_fpfh, source_down_fpfh = preprocess_source_pcd(
source=cad_model_pcd, voxel_size=voxel_size)
distances = source.compute_nearest_neighbor_distance()
avg_dist = np.mean(distances)
print(":: Cad model has %d points and avg_dist = %.6f" %
(len(source.points), avg_dist))
distances = scene_pcd.compute_nearest_neighbor_distance()
def __init__(self):
rospy.init_node('arm_tracker')
rospy.on_shutdown(self.shutdown)
# Maximum distance of the target before the arm will lower
self.max_target_dist = 1.2
# Arm length to center of gripper frame
self.arm_length = 0.4
# Distance between the last target and the new target before we move the arm
self.last_target_threshold = 0.01
# Distance between target and end-effector before we move the arm
self.target_ee_threshold = 0.025
# Initialize the move group for the right arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Set the reference frame for pose targets
self.reference_frame = REFERENCE_FRAME
# Keep track of the last target pose
self.last_target_pose = PoseStamped()
# Set the right arm reference frame accordingly
self.right_arm.set_pose_reference_frame(self.reference_frame)
# Allow replanning to increase the chances of a solution
self.right_arm.allow_replanning(False)
# Set a position tolerance in meters
self.right_arm.set_goal_position_tolerance(0.05)
# Set an orientation tolerance in radians
self.right_arm.set_goal_orientation_tolerance(0.2)
# What is the end effector link?
self.ee_link = self.right_arm.get_end_effector_link()
# Create the transform listener
self.listener = tf.TransformListener()
# Queue up some tf data...
rospy.sleep(3)
# Set the gripper target to closed position using a joint value target
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
# Plan and execute the gripper motion
right_gripper.go()
rospy.sleep(1)
# Subscribe to the target topic
rospy.wait_for_message('/target_pose', PoseStamped)
# Use queue_size=1 so we don't pile up outdated target messages
self.target_subscriber = rospy.Subscriber('/target_pose',
PoseStamped,
self.update_target_pose,
queue_size=1)
rospy.loginfo("Ready for action!")
while not rospy.is_shutdown():
try:
target = self.target
except:
rospy.sleep(0.5)
continue
# Timestamp the target with the current time
target.header.stamp = rospy.Time()
# Get the target pose in the right_arm shoulder lift frame
#target_arm = self.listener.transformPose('right_arm_shoulder_pan_link', target)
target_arm = self.listener.transformPose('right_rotate', target)
# Convert the position values to a Python list
p0 = [
target_arm.pose.position.x, target_arm.pose.position.y,
target_arm.pose.position.z
]
# Compute the distance between the target and the shoulder link
dist_target_shoulder = euclidean(p0, [0, 0, 0])
# If the target is too far away, then lower the arm
if dist_target_shoulder > self.max_target_dist:
rospy.loginfo("Target is too far away")
self.right_arm.set_named_target('r_start')
self.right_arm.go()
rospy.sleep(1)
continue
# Transform the pose to the base reference frame
target_base = self.listener.transformPose(self.reference_frame,
target)
# Compute the distance between the current target and the last target
p1 = [
target_base.pose.position.x, target_base.pose.position.y,
target_base.pose.position.z
]
p2 = [
self.last_target_pose.pose.position.x,
self.last_target_pose.pose.position.y,
self.last_target_pose.pose.position.z
]
dist_last_target = euclidean(p1, p2)
# Move the arm only if we are far enough away from the previous target
if dist_last_target < self.last_target_threshold:
rospy.loginfo("Still close to last target")
rospy.sleep(0.5)
continue
# Get the pose of the end effector in the base reference frame
ee_pose = self.right_arm.get_current_pose(self.ee_link)
# Convert the position values to a Python list
p3 = [
ee_pose.pose.position.x, ee_pose.pose.position.y,
ee_pose.pose.position.z
]
# Compute the distance between the target and the end-effector
dist_target = euclidean(p1, p3)
# Only move the arm if we are far enough away from the target
if dist_target < self.target_ee_threshold:
rospy.loginfo("Already close enough to target")
rospy.sleep(1)
continue
# We want the gripper somewhere on the line connecting the shoulder and the target.
# Using a parametric form of the line, the parameter ranges from 0 to the
# minimum of the arm length and the distance to the target.
t_max = min(self.arm_length, dist_target_shoulder)
# Bring it back 10% so we don't collide with the target
t = 0.9 * t_max
# Now compute the target positions from the parameter
try:
target_arm.pose.position.x *= (t / dist_target_shoulder)
target_arm.pose.position.y *= (t / dist_target_shoulder)
target_arm.pose.position.z *= (t / dist_target_shoulder)
except:
rospy.sleep(1)
rospy.loginfo("Exception!")
continue
# Transform to the base_footprint frame
target_ee = self.listener.transformPose(self.reference_frame,
target_arm)
# Set the target gripper orientation to be horizontal
target_ee.pose.orientation.x = 0
target_ee.pose.orientation.y = 0
target_ee.pose.orientation.z = 0
target_ee.pose.orientation.w = 1
# Update the current start state
self.right_arm.set_start_state_to_current_state()
# Set the target pose for the end-effector
self.right_arm.set_pose_target(target_ee, self.ee_link)
# Plan and execute the trajectory
success = self.right_arm.go()
if success:
# Store the current target as the last target
self.last_target_pose = target
# Pause a bit between motions to keep from locking up
rospy.sleep(0.5)
def give_human(req):
p.publish("start give")
data = rospy.wait_for_message("/arm_controller/state",
JointTrajectoryControllerState, 5)
positions_arm = data.actual.positions
global client
goal = TtsGoal()
goal.rawtext.text = str("please take the cup")
goal.rawtext.lang_id = "EN"
goal.wait_before_speaking = float(0)
if positions_arm[5] < 0 and positions_arm[6] < 0:
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "arm"
group = moveit_commander.MoveGroupCommander(group_name)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
waypoints = []
wpose = group.get_current_pose().pose
scaley = 0.2 - wpose.position.y
# wpose.position.z -= scale * 0.1 # First move up (z)
wpose.position.y += scaley * 1.0 # and sideways (y)
waypoints.append(copy.deepcopy(wpose))
scalex = 0.7 - wpose.position.x
print(wpose.position.x)
wpose.position.x += scalex * 1.0 # Second move forward/backwards in (x)
waypoints.append(copy.deepcopy(wpose))
scalez = 1.0 - wpose.position.z
wpose.position.z += scalez * 1.0 # Second move forward/backwards in (x)
waypoints.append(copy.deepcopy(wpose))
# We want the Cartesian path to be interpolated at a resolution of 1 cm
# which is why we will specify 0.01 as the eef_step in Cartesian
# translation. We will disable the jump threshold by setting it to 0.0 disabling:
(plan, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.005, # eef_step
0.0) # jump_threshold
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
# Publish
display_trajectory_publisher.publish(display_trajectory)
group.execute(plan, wait=True)
group.execute(plan, wait=True)
group.execute(plan, wait=True)
client.send_goal(goal, feedback_cb="")
client.wait_for_result()
return ser_messageResponse(True)
else:
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "arm"
group = moveit_commander.MoveGroupCommander(group_name)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
waypoints = []
wpose = group.get_current_pose().pose
scaley = -0.2 - wpose.position.y
# wpose.position.z -= scale * 0.1 # First move up (z)
wpose.position.y += scaley * 1.0 # and sideways (y)
waypoints.append(copy.deepcopy(wpose))
scalex = 0.7 - wpose.position.x
print(wpose.position.x)
wpose.position.x += scalex * 1.0 # Second move forward/backwards in (x)
waypoints.append(copy.deepcopy(wpose))
scalez = 1.0 - wpose.position.z
wpose.position.z += scalez * 1.0 # Second move forward/backwards in (x)
waypoints.append(copy.deepcopy(wpose))
# We want the Cartesian path to be interpolated at a resolution of 1 cm
# which is why we will specify 0.01 as the eef_step in Cartesian
# translation. We will disable the jump threshold by setting it to 0.0 disabling:
(plan, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.005, # eef_step
0.0) # jump_threshold
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
# Publish
display_trajectory_publisher.publish(display_trajectory)
group.execute(plan, wait=True)
# group.execute(plan, wait=True)
# group.execute(plan, wait=True)
client.send_goal(goal, feedback_cb="")
client.wait_for_result()
p.publish("finish give")
return ser_messageResponse(True)
def main():
global bridge, saveImage, saveImageFinish
rospy.init_node("baxter_pick_and_place_demo")
# Set up your subscriber and define its callback
rospy.Subscriber("/camera/depth/image_raw", Image, depth_image_callback)
rospy.Subscriber("/camera/rgb/image_raw", Image, color_image_callback)
# Load Gazebo Models via Spawning Services
# Note that the models reference is the /world frame
# and the IK operates with respect to the /base frame
# Wait for the All Clear from emulator startup
rospy.wait_for_message("/robot/sim/started", Empty)
limb = 'left'
hover_distance = 0.15 # meters
# Starting Joint angles for left arm
starting_joint_angles = {
'left_w0': 0.6699952259595108,
'left_w1': 1.030009435085784,
'left_w2': -0.4999997247485215,
'left_e0': -1.189968899785275,
'left_e1': 1.9400238130755056,
'left_s0': -0.08000397926829805,
'left_s1': -0.9999781166910306
}
pnp = PickAndPlace(limb, hover_distance)
# An orientation for gripper fingers to be overhead and parallel to the obj
overhead_orientation = Quaternion(0, 1, 0, 0)
# Feel free to add additional desired poses for the object.
# Each additional pose will get its own pick and place.
print('save image')
cv2.imwrite('./depth.png', depthImage)
cv2.imwrite('./color.png', colorImage)
rospy.sleep(1)
saveImageFinish = True
print('calculate pose')
try:
rospy.wait_for_service('gqcnn_grasp')
gqcnn_grasp = rospy.ServiceProxy('gqcnn_grasp', GQCNNGrasp)
req = GQCNNGraspRequest()
req.color_img_file_path = './color.png'
req.depth_img_file_path = './depth.png'
res = gqcnn_grasp(req)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
res_size = 0
if res:
res_size = len(res.q_values)
if res_size:
q_values = 0.0
grasps = copy.deepcopy(res.grasps[0])
canGrasp = False
for i in range(0, res_size):
res.grasps[i].pose.orientation = overhead_orientation
x = res.grasps[i].pose.position.x
y = res.grasps[i].pose.position.y
z = res.grasps[i].pose.position.z
res.grasps[i].pose.position.x = z + 0.18
res.grasps[i].pose.position.y = -x - 0.01
res.grasps[i].pose.position.z = -y + 0.02
# res.grasps[i].pose.position.y += 0.02
# res.grasps[i].pose.position.z -= 0.55
canSolveIK1 = pnp.ik_request(res.grasps[i].pose)
res.grasps[i].pose.position.z += pnp._hover_distance
canSolveIK2 = pnp.ik_request(res.grasps[i].pose)
res.grasps[i].pose.position.z -= pnp._hover_distance
canSolveIK = False
if canSolveIK1 and canSolveIK2:
canSolveIK = True
if canSolveIK and q_values < res.q_values[i]:
q_values = res.q_values[i]
grasps = copy.deepcopy(res.grasps[i])
canGrasp = True
print("INFO: Get pose from gqcnn_grasp with max q_values")
print(grasps, '\n q_value = ', q_values)
print(pnp.ik_request(res.grasps[i].pose))
res.grasps[i].pose.position.z += pnp._hover_distance
print(pnp.ik_request(res.grasps[i].pose))
res.grasps[i].pose.position.z -= pnp._hover_distance
else:
print('ERROR: No return from gqcnn!')
if not canGrasp:
print('ERROR: Can not find the grasping pose that can solve IK')
grasps.pose = Pose(position=Point(x=0.70, y=0.15, z=-0.129),
orientation=overhead_orientation)
pose1 = copy.deepcopy(grasps.pose)
pose2 = copy.deepcopy(grasps.pose)
pose2.position.y += 0.1
# Move to the desired starting angles
pnp.move_to_start(starting_joint_angles)
pnp.pick(pose1)
pnp.place(pose2)
pnp.move_to_start(starting_joint_angles)
def measure():
global dist
telem = get_telemetry(frame_id='aruco_map')
data = rospy.wait_for_message('rangefinder/range', Range)
dist = telem.z - data.range
print(dist)
#!/usr/bin/python
# -*- coding:utf-8 -*-
import rospy
from sensor_msgs.msg import LaserScan # tipo da mensagem do sensor
# Inicia o node
rospy.init_node("rplidar_plot")
# Configura um Subscriber do tópico /laser/scan
scan = LaserScan()
def scan_cb(msg):
global scan
scan = msg
scan_sub = rospy.Subscriber("/laser/scan", LaserScan, scan_cb)
# Após a primeira leitura do sensor, printar a medidas imediatamente
# à frente a uma frequência de 10 Hz.
rospy.wait_for_message("/laser/scan", LaserScan)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# scan.ranges é um array de 360 medidas de distância
print(scan.ranges[0], len(scan.ranges))
rate.sleep()
def execute(self, userdata):
# Only do it once.
move_objects_mode = userdata.data['move_objects_mode']
userdata.data['move_objects_mode'] = False
# Lift the gripper up slowly so that it gets a good grip.
curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
res = m.move_to_global(curr.position.x,
curr.position.y,
max(curr.position.z - 0.05,
GRIPPER_MIN_GLOBAL_Z),
'gripper',
velo_scale=0.02)
curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
# Update current pose with new orientation.
grip_height = curr.position.z
curr.position.z = GRIPPER_LIFT_HEIGHT_STRAIGHT
#t.publish_pose_as_transform(curr, 'global_xyz_link', 'MOVE HERE', 1.0)
res = m.move_to_global(curr.position.x,
curr.position.y,
curr.position.z,
'gripper',
orientation=curr.orientation,
velo_scale=0.5)
if not res:
rospy.logerr('Failed to perform a move (lift object gripper)')
return 'failed'
hold_gripper()
scales_topic = userdata.data['picking_from'] + '_scales/weight'
if move_objects_mode and userdata.data[
'move_objects_between_bins_mode'] is None:
mid_x, mid_y = POSITION_LIMITS[
userdata.data['picking_from']]['centre']
if curr.position.x > mid_x:
curr.position.x -= 0.15
else:
curr.position.x += 0.15
if curr.position.y > mid_y:
curr.position.y -= 0.15
else:
curr.position.y += 0.15
res, _, _ = m.move_to_global_monitor_weight(curr.position.x,
curr.position.y,
grip_height - 0.13,
'gripper',
scales_topic,
velo_scale=0.1)
return 'grip_failed'
if userdata.data['weight_before'] == -1:
rospy.logerr('No pre-pick weight, ignoring weight check')
return 'succeeded'
try:
weight = rospy.wait_for_message(scales_topic, Int16)
except:
rospy.logerr('Failed to get weight from %s, ignoring weight check')
return 'succeeded'
userdata.data['weight_after'] = weight.data
dw = userdata.data['weight_before'] - weight.data
if item_meta[userdata.data['item_to_pick']['label']].get(
'weight') * 1000.0 < 5:
# We can't do a weight check.
return 'succeeded'
if userdata.data['weight_failures'].get(
userdata.data['item_to_pick']['label'],
0) > 3 and (len(userdata.data['visible_objects']) < 6
or userdata.data.get('i_think_im_done')):
# We've failed weight check on this item heaps of times and we're almost finished so just pick it.
return 'succeeded'
if dw < 4:
# We haven't got anything, go back to the camera position and try again.
rospy.logerr('No detected change in weight, failed grasp.')
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_type'] == 'grip_suck':
return 'try_sucker'
userdata.data['last_failed'][userdata.data['item_to_pick']
['label']] = 0
return 'grip_failed'
if item_meta[userdata.data['item_to_pick']['label']].get(
'ignore_weight') == True:
return 'succeeded'
weight_correct, possible_matches = w.weight_matches(
userdata.data['item_to_pick']['label'], dw,
userdata.data['visible_objects'], 3)
if not weight_correct:
rospy.logerr(possible_matches)
if userdata.data.get('i_think_im_done', False) == True:
# Don't do weight reclassifications at the end.
pass
elif userdata.data['move_objects_between_bins_mode'] is not None:
pass
elif len(possible_matches) == 1 and possible_matches[0][0] < 5:
# There's only one thing it could actually be!
if userdata.data['task'] == 'stow' or (
userdata.data['task'] == 'pick'
and possible_matches[0][1]
in userdata.data['wanted_items']):
rospy.logerr("I'M CHANGING THE OBJECT TO %s" %
possible_matches[0][1])
userdata.data['weight_reclassifications'].append(
(userdata.data['item_to_pick']['label'],
possible_matches[0][1]))
userdata.data['item_to_pick']['label'] = possible_matches[
0][1]
return 'succeeded'
# Put the item back down and bail.
if userdata.data['item_to_pick']['label'] not in userdata.data[
'weight_failures']:
userdata.data['weight_failures'][userdata.data['item_to_pick']
['label']] = 1
else:
userdata.data['weight_failures'][userdata.data['item_to_pick']
['label']] += 1
userdata.data['last_failed'][userdata.data['item_to_pick']
['label']] = 0
rospy.logerr("WEIGHT DOESN'T MATCH: MEASURED: %s, EXPECTED: %s" %
(dw, item_meta[userdata.data['item_to_pick']
['label']].get('weight') * 1000.0))
# Move towards the centre.
mid_x, mid_y = POSITION_LIMITS[
userdata.data['picking_from']]['centre']
if curr.position.x > mid_x:
curr.position.x -= 0.08
else:
curr.position.x += 0.08
if curr.position.y > mid_y:
curr.position.y -= 0.08
else:
curr.position.y += 0.08
res, _, _ = m.move_to_global_monitor_weight(curr.position.x,
curr.position.y,
grip_height - 0.13,
'gripper',
scales_topic,
velo_scale=0.1)
if not res:
rospy.logerr('Failed to perform a move')
return 'failed'
return 'grip_failed'
return 'succeeded'
def callback_no_topic(self):
data = rospy.wait_for_message("/camera/rgb/image_raw", Image)
#data = rospy.wait_for_message("usb_cam/image_raw", Image)
try:
cv_image_full = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
if self.first:
self.r = cv2.selectROI(cv_image_full)
self.first = False
if cv2.getTrackbarPos(self.switch, 'camera') == 1:
self.r = cv2.selectROI(cv_image_full)
cv_image = cv_image_full[int(self.r[1]):int(self.r[1] + self.r[3]),
int(self.r[0]):int(self.r[0] + self.r[2])]
# BACKGROUND SUBSTRACTION
output = cv_image
cv_image = cv2.filter2D(cv_image, -1, self.blur)
# CONVERT TO HSV
hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
# BLUR TO GET RID OF NOISE
cv_image = cv2.filter2D(cv_image, -1, self.blur)
# get info from track bar and appy to result
h_low = cv2.getTrackbarPos('H_low', 'camera')
s_low = cv2.getTrackbarPos('S_low', 'camera')
v_low = cv2.getTrackbarPos('V_low', 'camera')
h_high = cv2.getTrackbarPos('H_high', 'camera')
s_high = cv2.getTrackbarPos('S_high', 'camera')
v_high = cv2.getTrackbarPos('V_high', 'camera')
lower_green = np.array([h_low, s_low, v_low])
upper_green = np.array([h_high, s_high, v_high])
mask = cv2.inRange(hsv, lower_green, upper_green)
mask2 = mask
#mask = cv2.medianBlur(mask, 5)
#mask = cv2.medianBlur(mask, 5)
mask3 = mask
mask3 = cv2.bitwise_not(mask3)
invert = mask3
#cv_image3 = cv_image
#cv_image2 = cv2.bitwise_and(cv_image, cv_image, mask=mask)
#cv_image = cv_image2
#imgray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
# TEST EXTRAIRE OBJET
im2, contours, hierarchy = cv2.findContours(mask3, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
#im2, contours, hierarchy = cv2.findContours(imgray, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
if len(contours) != 0:
# draw in blue the contours that were founded
#cv2.drawContours(output, contours, -1, 255, 3)
# find the biggest area
c = max(contours, key=cv2.contourArea)
cv2.drawContours(output, c, -1, 255, 3)
x, y, w, h = cv2.boundingRect(c)
# draw the book contour (in green)
cv2.rectangle(output, (x, y), (x + w, y + h), (0, 255, 0), 2)
#cv2.drawContours(cv_image, contours, -1, (0, 255, 0), 3)
#cv2.grabCut(cv_image, mask, rect, bgdModel, fgdModel, 5, cv2.GC_INIT_WITH_RECT)
#mask2 = np.where((mask == 2) | (mask == 0), 0, 1).astype('uint8')
#cv_image = cv_image * mask2[:, :, np.newaxis]
#cv_image = self.add_blobs(cv_image)
#cv2.rectangle(cv_image, (x1,y1), (x2,y2), (255,0,0), 2)
cv2.imshow('inRange', mask2)
cv2.imshow('invert', invert)
cv2.imshow('camera', output)
cv2.waitKey(1)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
else:
speed.angular.z = 0.3
speed.linear.x = 0.3 * min(self.regions['frontLeft'], self.regions['frontRight'])
if speed.angular.z > 1:
speed.angular.z = 0.9
elif speed.angular.z < -1:
speed.angular.z = -0.9
self.pubVel.publish(speed)
if __name__ == '__main__':
#instantiate the class and set some parameters
app = Robot()
print "receiving goal points..."
msg = rospy.wait_for_message('goals', PointArray) #wait until first message of 'goals'
app.goals = msg.goals #fill array with all goals from message
app.noGoals = len(app.goals) #get number of goals to reach
app.goal.x = app.goals[app.currentGoal].x #start with first goal at index 0 of goals array
app.goal.y = app.goals[app.currentGoal].y
print "First goal is goal nr 1 with the coordinates: \n" + str(app.goal)
print "I am starting to navigate to my goal points"
#run the script in an infite loop to continously read and process laserdata
while(True):
app.run()
app.r.sleep()
def get_frame():
frame = rospy.wait_for_message("/camera/image_color", Image, 0.5)
return frame
if __name__ == '__main__':
rospy.init_node("rgbd_capturer")
rospy.loginfo("Starting rgbd_capturer node")
bridge = cv_bridge.CvBridge()
img_id = 0
save_root = "./"
min_depth, max_depth = 0.25, 3.5
print("min_depth: " + str(min_depth) + " max_depth: " + str(max_depth))
while True:
# get rgb-depth from camera
rgb_msg = rospy.wait_for_message("/rgb/image_raw", Image)
depth_msg = rospy.wait_for_message("/depth_to_rgb/image_raw", Image)
rgb = bridge.imgmsg_to_cv2(rgb_msg,desired_encoding='bgr8')
depth = bridge.imgmsg_to_cv2(depth_msg, desired_encoding='32FC1')
# depth normalization (0.25 ~ 1.25 -> 0 ~ 255)
depth_im = depth
depth_im[depth_im > max_depth] = max_depth
depth_im[depth_im < min_depth] = min_depth
depth_im = np.uint8(255 * (depth_im - min_depth) / (max_depth - min_depth))
depth_im = np.repeat(np.expand_dims(depth_im, -1), 3, -1)
rgbd_im = np.hstack([rgb, depth_im])
rgbd_im = cv2.resize(rgbd_im, (1280, 360))
cv2.imshow("rgbd - img_id: " + str(img_id) + " S: Save / Q: Quit", rgbd_im )
key = cv2.waitKey(1)
def main(arg):
adapt_node_name = arg
global msg_list, node_list, bus
#load parameters
try:
#can_dev = rospy.get_param("/" + adapt_node_name + "/bus/device") #,'can0')
#default_vel_ratio = rospy.get_param("/" + adapt_node_name + "/defaults/vel_to_device")
default_resolution = rospy.get_param("/" + adapt_node_name +
"/defaults/encoder_resolution")
CAN_nodes = rospy.get_param("/" + adapt_node_name + "/nodes")
controlled_joint_names = rospy.get_param(
"/joint_group_position_controller")['joints']
description = rospy.get_param("robot_description")
except KeyError:
rospy.loginfo("Parameter value not set")
# get transmission from URDF
robot = xml.dom.minidom.parseString(description).getElementsByTagName(
'robot')[0]
for child in robot.childNodes:
if child.nodeType is child.TEXT_NODE:
continue
if child.localName == 'transmission':
try:
actuator = child.getElementsByTagName('actuator')[0]
reduction = actuator.childNodes[
1] #getElementByTagName('mechanicalReduction')[0]
reduction_ratio = float(reduction.childNodes[0].nodeValue)
trans_joint = child.childNodes[
3] #getElementByTagName('joint')[0]
trans_joint_name = trans_joint.getAttribute('name')
except KeyError:
rospy.logwarn("No mechanical reduction specified in URDF")
ratio = reduction_ratio * default_resolution * 0.5 / pi * 10
for node in CAN_nodes:
if node['name'] == trans_joint_name:
node_list[trans_joint_name] = {
'id': node['id'],
'ratio': ratio
}
rospy.init_node('module_state_adaptor')
rospy.Subscriber("/joint_states", JointState, callback)
pub = rospy.Publisher('/joint_group_position_controller/command',
Float64MultiArray,
queue_size=1)
r = rospy.Rate(20) # 10hz
#initialize pos
pos = []
for i in range(len(controlled_joint_names)):
pos.append(0)
rospy.wait_for_message("/joint_states", JointState)
rospy.sleep(2)
while not rospy.is_shutdown():
i = 0
for jnt in controlled_joint_names:
pos[i] = msg_list[jnt]['pos']
i = i + 1
pub.publish(Float64MultiArray(data=pos))
r.sleep()
def __init__(self):
rospy.init_node('nav_test', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 2)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
locations = dict()
locations['A'] = Pose(Point(1.279, -0.265, 0.000),
Quaternion(0.000, 0.000, 0.318, 0.948))
locations['B'] = Pose(Point(2.539, 1.657, 0.000),
Quaternion(00.000, 0.000, 0.000, 1.000))
locations['C'] = Pose(Point(0.134, 1.541, 0.000),
Quaternion(0.000, 0.000, 0.000, -1.000))
locations['D'] = Pose(Point(-0.324, -0.276, 0.000),
Quaternion(0.000, 0.000, 0.000, 1.000))
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
self.Dist = 0
# Get the initial pose from the user (how to set)
rospy.loginfo(
"*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..."
)
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped,
self.update_initial_pose)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
###### Modify by XF 2017. 4.21 #########
dict_keys = ['A', 'B', 'C', 'D']
pub1 = rospy.Publisher('/voice/xf_tts_topic', String, queue_size=5)
pub2 = rospy.Publisher('/voice/xf_asr_topic', Int32, queue_size=5)
pub3 = rospy.Publisher('arm', String, queue_size=5)
pub4 = rospy.Publisher('tracking', Int32, queue_size=5)
#######################################
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# while i<4:
if i == n_locations:
i = 0
# sequence = sample(locations, n_locations)
# Skip over first location if it is the same as
# the last location
if dict_keys[0] == last_location:
i = 1
#####################################
####### Modify by XF 2017.4.14 ######
# location = sequence[i]
# rospy.loginfo("location= " + str(location))
# location = locations.keys()[i]
location = dict_keys[i]
#####################################
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(
pow(
locations[location].position.x -
locations[last_location].position.x, 2) + pow(
locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(
pow(
locations[location].position.x -
initial_pose.pose.pose.position.x, 2) + pow(
locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
# Allow 6 minutes to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(360))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.ABORTED: # can not find a plan,give feedback
rospy.loginfo("please get out")
status_n = "please get out"
pub1.publish(status_n)
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
############ add voice by XF 4.25 ################## The 1st
# pub1 = rospy.Publisher('/voice/xf_tts_topic', String, queue_size=5)
loc = "I arrived the location " + str(location)
pub1.publish(loc)
rospy.Subscriber(
'dist', Int32,
self.callbackDist) #now robot already adjust correctly
rospy.loginfo("Dist:" + str(self.Dist))
if str(location) == 'A':
commandA1 = "what can I do for you?"
pub1.publish(commandA1)
############# now next asr ######################
awake = 1
pub2.publish(awake)
rospy.sleep(13)
commandA2 = "ok,I get the task , please wait."
pub1.publish(commandA2)
################################################## The 2nd
if str(location) == 'B':
############## add by XF 5.27 ##################
track = 1
pub4.publish(track)
rospy.sleep(10)
rospy.Subscriber('dist', Int32, self.callbackDist
) #now robot already adjust correctly
rospy.loginfo("Dist2:" + str(self.Dist))
rospy.loginfo("go away Dist:" +
str(self.Dist / 1000.0 - 0.3))
self.goal = MoveBaseGoal()
self.goal.target_pose.pose.position.x = self.Dist / 1000.0 - 0.3
self.goal.target_pose.pose.orientation.w = 1.0
self.goal.target_pose.header.frame_id = 'base_footprint'
self.goal.target_pose.header.stamp = rospy.Time.now()
self.move_base.send_goal(self.goal)
rospy.sleep(10)
################################################
commandB1 = "pick"
pub3.publish(commandB1)
rospy.sleep(10)
commandB2 = "Now I find the object,and already grabbed"
pub1.publish(commandB2)
################################################## The 3rd
if str(location) == 'C':
#subscribe the angle
# rospy.Subscriber('xf_xfm_node', Int32, self.callbackXFM)
rospy.sleep(10)
commandC2 = "Master,give your drink."
pub1.publish(commandC2)
rospy.sleep(3)
commandC1 = "release"
pub3.publish(commandC1)
##################################################
else:
rospy.loginfo("Goal failed with error code: " +
str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes / n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " +
str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def meas_cb(self, msg):
with self.lock:
# check if cam_info is valid
for camera in msg.M_cam:
P = camera.cam_info.P
all_zero = True
for i in range(12):
if P[i] != 0:
all_zero = False
if all_zero:
rospy.logfatal(
"Camera info of %s is all zero. You should calibrate your camera intrinsics first "
% camera.camera_id)
exit(-1)
# Modify all the camera info messages to make sure that ROI and binning are removed, and P is updated accordingly
# Update is done in place
#for camera in msg.M_cam:
# camera.cam_info = camera_info_converter.unbin(camera.cam_info)
if rospy.has_param('optimizer_conditional_resetter_enable'):
self.reset_flag = rospy.get_param(
'optimizer_conditional_resetter_enable')
else:
self.reset_flag = False
if self.reset_flag == True:
urdf_cam_ns = rospy.get_param("urdf_cam_ns")
new_cam_ns = rospy.get_param("new_cam_ns")
u_info = rospy.wait_for_message(urdf_cam_ns + '/camera_info',
CameraInfo)
n_info = rospy.wait_for_message(new_cam_ns + '/camera_info',
CameraInfo)
urdf_cam_frame = u_info.header.frame_id
new_cam_frame = n_info.header.frame_id
mounting_frame = rospy.get_param("mounting_frame")
# if any of the frames has changed...
if self.prev_3_frames != ():
if self.prev_3_frames != (urdf_cam_frame, new_cam_frame,
mounting_frame):
self.frames_changed = True
if self.frames_changed:
self.state = None
self.meas = [] # clear cache
self.timestamps = []
self.measurement_count = 0
self.prev_tf_transforms = {}
rospy.loginfo('Reset calibration state')
print "\033[43;1mTarget frames have changed. Clean up everything and start over! \033[0m\n\n"
self.frames_changed = False
if len(self.tf_check_pairs) > 0:
self.prev_tf_pair = self.tf_check_pairs[0]
self.tf_check_pairs = []
self.tf_check_pairs.append(
(mounting_frame, urdf_cam_frame
)) # only keep one pair in the list at a time
self.prev_3_frames = (urdf_cam_frame, new_cam_frame,
mounting_frame)
self.prev_tf_pair = (mounting_frame, urdf_cam_frame)
TheMoment = msg.header.stamp
# check for tf transform changes
for (frame1, frame2) in self.tf_check_pairs:
transform = None
print "\nLooking up transformation \033[34;1mfrom\033[0m %s \033[34;1mto\033[0m %s ..." % (
frame1, frame2)
while not rospy.is_shutdown():
try:
self.tf_listener.waitForTransform(
frame1, frame2, TheMoment, rospy.Duration(10))
transform = self.tf_listener.lookupTransform(
frame1, frame2, TheMoment) #((),())
print "found\n"
break
except (tf.LookupException, tf.ConnectivityException):
print "transform lookup failed, retrying..."
if self.measurement_count > 0:
prev_transform = self.prev_tf_transforms[(frame1, frame2)]
dx = transform[0][0] - prev_transform[0][0]
dy = transform[0][1] - prev_transform[0][1]
dz = transform[0][2] - prev_transform[0][2]
dAngle = self.getAngle(transform[1]) - self.getAngle(
prev_transform[1])
a = self.getAxis(transform[1])
b = self.getAxis(prev_transform[1])
dAxis = (a[0] * b[0] + a[1] * b[1] +
a[2] * b[2]) / 1. # a dot b
#print "\n"
#print "\033[34;1m-- Debug Info --\033[0m"
print "checking for pose change..."
print "measurement_count = %d" % self.measurement_count
print " dx = %10f | < 0.0002 m" % dx
print " dy = %10f | < 0.0002 m" % dy
print " dz = %10f | < 0.0002 m" % dz
print "dAngle = %10f | < 0.00174 rad" % dAngle
print " dAxis = %10f | > 0.99999\n" % dAxis
if ((math.fabs(dx) > 0.0002) or (math.fabs(dy) > 0.0002)
or (math.fabs(dz) > 0.0002)
or (math.fabs(dAngle) > 0.00174) or
(math.fabs(dAxis) <
0.99999)): # if transform != previous_transform:
print "\033[44;1mDetect pose change: %s has changed pose relative to %s since last time!\033[0m\n\n" % (
frame1, frame2)
###self.reset() # no, you cannot call reset() here.
self.state = None
count = len(self.meas)
self.meas = [] # clear cache
self.timestamps = []
rospy.loginfo('Reset calibration state')
print "\033[33;1m%ld\033[0m previous measurements in the cache are deleted.\n\n" % count
self.measurement_count = 0
self.prev_tf_transforms[(frame1, frame2)] = transform
# add timestamp to list
self.timestamps.append(msg.header.stamp)
# add measurements to list
self.meas.append(msg)
print "MEAS", len(self.meas)
for m in self.meas:
print " - stamp: %f" % m.header.stamp.to_sec()
print "\n"
self.measurement_count += 1
print "\nNo. of measurements fed to optimizer: %d\n\n" % self.measurement_count
## self.last_stamp_pub.publish(self.meas[-1].header.stamp)
# initialize state if needed
if True: #not self.state:
self.state = CalibrationEstimate()
camera_poses, checkerboard_poses = init_optimization_prior.find_initial_poses(
self.meas)
self.state.targets = [
posemath.toMsg(checkerboard_poses[i])
for i in range(len(checkerboard_poses))
]
self.state.cameras = [
CameraPose(camera_id, posemath.toMsg(camera_pose))
for camera_id, camera_pose in camera_poses.iteritems()
]
print "Proceed to optimization...\n"
# run optimization
self.state = estimate.enhance(self.meas, self.state)
print "\nOptimized!\n"
# publish calibration state
res = CameraCalibration() ## initialize a new Message instance
res.camera_pose = [camera.pose for camera in self.state.cameras]
res.camera_id = [camera.camera_id for camera in self.state.cameras]
res.time_stamp = [timestamp
for timestamp in self.timestamps] #copy
res.m_count = len(res.time_stamp)
# self.measurement_count
self.pub.publish(res)
def main():
global img_ball
global img_arr
global depth_ball
global img_ballx
global img_bally
rospy.init_node("colorOfBall")
image_sub = rospy.Subscriber("/camera/rgb/image_raw/compressed",
CompressedImage, imageCallback)
depth_sub = rospy.Subscriber(
"/camera/depth_registered/image_raw/compressedDepth", CompressedImage,
depthCallback)
print("before wait")
rospy.wait_for_message('/camera/rgb/image_raw/compressed', CompressedImage)
rospy.wait_for_message(
'/camera/depth_registered/image_raw/compressedDepth', CompressedImage)
print("after wait")
rate = rospy.Rate(20)
while not rospy.is_shutdown():
rospy.wait_for_message("/camera/rgb/image_raw/compressed",
CompressedImage)
cv_image_hsv = cv2.cvtColor(img_ball, cv2.COLOR_BGR2HSV)
(rows, cols, channels) = img_ball.shape
#print([rows,cols,channels])
#cv2.circle(cv_image,(320,240),10,(255,0,0))
#blurred = cv2.GaussianBlur(cv_image,(11,11),0)
mask = cv2.inRange(cv_image_hsv, orange_lower, orange_upper)
mask = cv2.erode(mask, None, iterations=1)
mask = cv2.dilate(mask, None, iterations=1)
# find contours in the mask and initialize the current
# (x, y) center of the ball
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
center = None
#print (cnts)
# only proceed if at least one contour was found
if len(cnts) > 0:
# find the largest contour in the mask, then use
# it to compute the minimum enclosing circle and
# centroid
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
#center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
img_ballx = x
img_bally = y
#depth_ball = 0
depth_ball = toDistance(float(img_arr[y, x]))
# only proceed if the radius meets a minimum size
if radius > 5 and radius < 160 and y > 140:
# draw the circle and centroid on the frame,
# then update the list of tracked points
cv2.circle(img_ball, (int(x), int(y)), int(radius),
(0, 0, 255), 2)
print([int(x), int(y), int(radius), depth_ball])
#cv2.circle(mask, center, 5, (0, 0, 255), -1)
cv2.imshow("Image window", img_ball)
#print([0,0,0] + cv_image_hsv[320,240,:])
cv2.waitKey(1) & 0xFF
plt.imshow(img_arr[:, :])
plt.draw()
plt.show()
rate.sleep()
cv2.destroyAllWindows()
rospy.Subscriber("/sim/left_arm/cartesianforce", WrenchStamped,
subscribe_force_callback_left)
rate.sleep()
global rf
global lf
pub_r = rospy.Publisher('/movo/right_arm/cartesianforce',
JacoCartesianVelocityCmd,
queue_size=10)
pub_l = rospy.Publisher('/movo/left_arm/cartesianforce',
JacoCartesianVelocityCmd,
queue_size=10)
while not rospy.is_shutdown():
pub_r.publish(rf)
rate.sleep()
pub_l.publish(lf)
rate.sleep()
if __name__ == "__main__":
# Create a node
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node("lx_cartesianforce")
rate = rospy.Rate(10)
rospy.wait_for_message('/sim_initialized', Bool)
get_force()
rospy.spin()
# also last_goal flag
if enable_p2p == True or last_goal == True:
i += 1
if i == total_wp - 1:
last_goal = True
if __name__ == '__main__':
rospy.init_node('p2p_py')
location_target = -1
pix_bot_center = Pose()
pix_bot_theta = 0
rospy.Subscriber(ODOM_INF, Odometry, vehicleStateCallback)
rospy.wait_for_message(ODOM_INF, Odometry, 5)
#rospy.Subscriber("/state", Int8, stateCallback)
rospy.Subscriber("SM_output", StateOut, stateCallback)
rospy.wait_for_message("SM_output", StateOut, 3)
print("[P2P] Started")
sm_pub = rospy.Publisher("SM_input", StateIn, queue_size=1)
last_loc_target = -1
while not rospy.is_shutdown():
if (location_target != -1 and enable_p2p == True):
print("Target ID:", location_target)
# if location_target == 12:
# waypoints = np.array([[0,0,0],[10,0,0],[20,0,0], [25,0,0],[32.5,10,np.pi/2], [32.5,28,np.pi/2], [32.5,30,np.pi/2]])
# elif location_target == 3:
# waypoints = np.array([[32.5,38,np.pi/2], [32.5,40,np.pi/2], [25, 47, np.pi], [12.3,47, np.pi], [-10.87, 46.7, np.pi], [-24, 40, -np.pi/2], [-24, 31.5, -np.pi/2], [-50.7,28,-np.pi], [-51.7,28,-np.pi]])
# else:
# print("Unknown Goal Given")
def launch_simulation(self):
# Wait for Gazebo services
rospy.loginfo("Starting unreal MAV simulation setup coordination...")
rospy.wait_for_service(self.ns_gazebo + "/unpause_physics")
rospy.wait_for_service(self.ns_gazebo + "/set_model_state")
# Prepare initialization trajectory command
traj_pub = rospy.Publisher(self.ns_mav + "/command/trajectory",
MultiDOFJointTrajectory,
queue_size=10)
traj_msg = MultiDOFJointTrajectory()
traj_msg.joint_names = ["base_link"]
transforms = Transform()
transforms.translation.x = self.initial_position[0]
transforms.translation.y = self.initial_position[1]
transforms.translation.z = self.initial_position[2]
point = MultiDOFJointTrajectoryPoint([transforms], [Twist()],
[Twist()], rospy.Duration(0))
traj_msg.points.append(point)
# Prepare initialization Get/SetModelState service
set_model_srv = rospy.ServiceProxy(self.ns_gazebo + "/set_model_state",
SetModelState)
get_model_srv = rospy.ServiceProxy(self.ns_gazebo + "/get_model_state",
GetModelState)
mav_name = self.ns_mav[np.max(
[i
for i in range(len(self.ns_mav)) if self.ns_mav[i] == "/"]) + 1:]
pose = Pose()
pose.position.x = self.initial_position[0]
pose.position.y = self.initial_position[1]
pose.position.z = self.initial_position[2]
model_state_set = ModelState(mav_name, pose, Twist(), "world")
# Wake up gazebo
rospy.loginfo("Waiting for gazebo to wake up ...")
unpause_srv = rospy.ServiceProxy(self.ns_gazebo + "/unpause_physics",
Empty)
while not unpause_srv():
rospy.sleep(0.1)
rospy.loginfo("Waiting for gazebo to wake up ... done.")
# Wait for drone to spawn (imu is publishing)
rospy.loginfo("Waiting for MAV to spawn ...")
rospy.wait_for_message(self.ns_mav + "/imu", Imu)
rospy.loginfo("Waiting for MAV to spawn ... done.")
# Initialize drone stable at [0, 0, 0]
rospy.loginfo("Waiting for MAV to stabilize ...")
dist = 10 # Position and velocity
while dist >= 0.1:
traj_msg.header.stamp = rospy.Time.now()
traj_pub.publish(traj_msg)
set_model_srv(model_state_set)
rospy.sleep(0.1)
state = get_model_srv(mav_name, "world")
pos = state.pose.position
twist = state.twist.linear
dist = np.sqrt((pos.x - self.initial_position[0])**2 +
(pos.y - self.initial_position[1])**2 +
(pos.z - self.initial_position[2])**2) + np.sqrt(
twist.x**2 + twist.y**2 + twist.z**2)
rospy.loginfo("Waiting for MAV to stabilize ... done.")
# Wait for unreal client
rospy.loginfo("Waiting for unreal client to setup ...")
rospy.wait_for_message("ue_out_in", PointCloud2)
rospy.loginfo("Waiting for unreal client to setup ... done.")
# Finish
self.ready_pub.publish(String("Simulation Ready"))
rospy.loginfo("Unreal MAV simulation setup successfully.")
cur_pose = msg
mode_proxy = None
arm_proxy = None
rospy.init_node('multi', anonymous=True)
#Comm for drones
mode_proxy = rospy.ServiceProxy('mavros/set_mode', SetMode)
arm_proxy = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
print 'communication initialization complete'
try:
data = rospy.wait_for_message('mavros/global_position/rel_alt',
Float64,
timeout=5)
except:
pass
print "wait for service"
rospy.wait_for_service('mavros/set_mode')
print "got service"
rate = rospy.Rate(10)
while not rospy.is_shutdown():
success = None
try:
success = mode_proxy(1, 'OFFBOARD')
except rospy.ServiceException, e:
def main():
"""RSDK Inverse Kinematics Pick and Place Example
A Pick and Place example using the Rethink Inverse Kinematics
Service which returns the joint angles a requested Cartesian Pose.
This ROS Service client is used to request both pick and place
poses in the /base frame of the robot.
Note: This is a highly scripted and tuned demo. The object location
is "known" and movement is done completely open loop. It is expected
behavior that Baxter will eventually mis-pick or drop the block. You
can improve on this demo by adding perception and feedback to close
the loop.
"""
rospy.init_node("ik_pick_and_place_demo")
# Load Gazebo Models via Spawning Services
# Note that the models reference is the /world frame
# and the IK operates with respect to the /base frame
load_gazebo_models()
# Remove models from the scene on shutdown
rospy.on_shutdown(delete_gazebo_models)
# Wait for the All Clear from emulator startup
rospy.wait_for_message("/robot/sim/started", Empty)
limb = 'left'
hover_distance = 0.15 # meters
# Starting Joint angles for left arm
starting_joint_angles = {'left_w0': 0.6699952259595108,
'left_w1': 1.030009435085784,
'left_w2': -0.4999997247485215,
'left_e0': -1.189968899785275,
'left_e1': 1.9400238130755056,
'left_s0': -0.08000397926829805,
'left_s1': -0.9999781166910306}
pnp = PickAndPlace(limb, hover_distance)
# An orientation for gripper fingers to be overhead and parallel to the obj
overhead_orientation = Quaternion(
x=-0.0249590815779,
y=0.999649402929,
z=0.00737916180073,
w=0.00486450832011)
block_poses = list()
# The Pose of the block in its initial location.
# You may wish to replace these poses with estimates
# from a perception node.
block_poses.append(Pose(
position=Point(x=0.7, y=0.15, z=-0.129),
orientation=overhead_orientation))
# Feel free to add additional desired poses for the object.
# Each additional pose will get its own pick and place.
block_poses.append(Pose(
position=Point(x=0.75, y=-0.2, z=-0.129),
orientation=overhead_orientation))
# NEW - BLOCK 2 POSES
block_poses.append(Pose(
position=Point(x=0.7, y=-0.1, z=-0.129),
orientation=overhead_orientation))
block_poses.append(Pose(
position=Point(x=0.8, y=0.15, z=-0.129),
orientation=overhead_orientation))
# Move to the desired starting angles
pnp.move_to_start(starting_joint_angles)
idx = 0
while not rospy.is_shutdown():
#if idx!=0:
# idx = (idx+1) % len(block_poses)
print("\nPicking...")
pnp.pick(block_poses[idx])
print("\nPlacing...")
idx = (idx+1) % len(block_poses)
pnp.place(block_poses[idx])
# BLOCK 2
print("\nPicking...")
idx = (idx+1) % len(block_poses)
pnp.pick(block_poses[idx])
print("\nPlacing...")
idx = (idx+1) % len(block_poses)
pnp.place(block_poses[idx])
return 0
self.magreader.mag_data.y > 0
): # If the robot is pointing towards the North then we stop its rotation
self.vel_data.angular.z = 0
self._vel_pub.publish(self.vel_data)
# We've left a case untouched - if (abs(self.magreader.mag_data.x) < self.tol) and (self.magreader.mag_data.y < 0) - this means that the robot is practically fully aligned with the South direction. If this happens, by skipping it we neither bring the robot to a stop, nor do we change the angular_speed, hence the robot continues moving at the
# angular_speed at which it entered this "window". This is a form of hysteresis, added here to prevent rapid switching of the angular_speed when the robot goes through the state of looking South - if not it could start jittering in place.
# However, we need to consider the case when the robot is looking South and it isn't moving. This could happen at startup, and if it happens we have to tell the robot to start moving:
else:
if self.vel_data.angular.z == 0:
self.vel_data.angular.z = self.max_angular_speed
self._vel_pub.publish(self.vel_data)
print(
'Robot angular speed: ' + str(self.vel_data.angular.z)
) # We print the angular_speed of the robot for debugging purposes
self.r.sleep() # wait so that robot motion is smoother
if __name__ == '__main__':
rospy.init_node('orient_north', anonymous=False)
northorienter = NorthOrienter()
rospy.wait_for_message(
'/imu/mag', MagneticField, timeout=10
) # wait for an initial message of type MagneticField to be published to the /imu/mag topic that we will be listening to. Timeout of 10s
while not rospy.is_shutdown():
try:
northorienter.orientate_robot()
except rospy.ROSInterruptException:
pass
def execute_cb(self, goal):
# move the ptu
self.send_feedback("Moving PTU to %f, %f" %
(goal.ptu_pan, goal.ptu_tilt))
self.command_ptu(goal.ptu_pan, goal.ptu_tilt)
observation = Observation.make_observation(
topics=[("/amcl_pose", PoseWithCovarianceStamped
), ("/head_xtion/rgb/image_color",
Image), ("/head_xtion/rgb/camera_info", CameraInfo),
("/head_xtion/depth_registered/points",
PointCloud2), ("/head_xtion/depth/camera_info",
CameraInfo), ("/ptu/state", JointState)])
self.send_feedback("Getting pointcloud")
try:
# cloud = rospy.wait_for_message("/head_xtion/depth_registered/points", PointCloud2, 5)
cloud = observation.get_message(
'/head_xtion/depth_registered/points')
except:
self.send_feedback("Failed to get a pointcloud")
self._result.found = 0
self._as.set_succeeded(self._result)
return
self.send_feedback("Returning PTU home")
self.command_ptu(0, 0)
object_searched = goal.object_id
rospy.loginfo("Looking for object with id: %s" % object_searched)
self.send_feedback("Calling object identification service.")
try:
result = self.id_service(cloud)
except:
self._result.found = 0
self._as.set_succeeded(self._result)
return
rospy.loginfo("Number of objects found: %s" % len(result.ids))
found = False
world = World()
objects = ""
for i in result.ids:
rospy.loginfo(" - found %s" % i.data)
objects += "* %s\n" % i.data[:-4]
# TODO: Should project all other objects into observation and "kill" if not visible
new_object = world.create_object()
# TODO: store object pose etc
# TODO: the classification should include class info
classification = {}
identification = {i.data[:-4]: 1}
new_object.add_identification(
"ObjectInstanceRecognition",
ObjectIdentification(classification, identification))
if i.data == object_searched:
found = True
break
if found:
rospy.loginfo("Found %s" % object_searched)
self._result.found = 1
self._as.set_succeeded(self._result)
else:
rospy.loginfo("Did not find %s" % object_searched)
self._result.found = 0
self._as.set_succeeded(self._result)
try:
waypoint = rospy.wait_for_message("/current_node", String, 5)
except:
waypoint = String("-")
title = 'Fire Extinguisher Check'
body = 'I checked for `%s` at %s and it was ' % (object_searched[:-4],
waypoint.data)
if found:
body += '*present*'
else:
body += '*not in place*'
body += '.\n\nHere is an image:\n\n'
msg_store_blog = MessageStoreProxy(collection='robblog')
img = observation.get_message('/head_xtion/rgb/image_color')
img_id = msg_store_blog.insert(img)
body += ')' % img_id
if len(objects) > 0:
body += '\n\nI found:\n\n'
body += objects
e = rb_utils.create_timed_entry(title=title, body=body)
self.blog_msg_store.insert(e)
def start(self):
groundtruth_topics = self.param.topics.groundtruth
# Create groundtruth service proxies
section_proxy = rospy.ServiceProxy(
groundtruth_topics.section, SectionSrv, persistent=True
)
lane_proxy = rospy.ServiceProxy(groundtruth_topics.lane, LaneSrv, persistent=True)
obstacle_proxy = rospy.ServiceProxy(
groundtruth_topics.obstacle, LabeledPolygonSrv, persistent=True
)
surface_marking_proxy = rospy.ServiceProxy(
groundtruth_topics.surface_marking, LabeledPolygonSrv, persistent=True
)
traffic_sign_proxy = rospy.ServiceProxy(
groundtruth_topics.traffic_sign, LabeledPolygonSrv, persistent=True
)
self.pause_physics_proxy = rospy.ServiceProxy(
self.param.topics.pause_gazebo, EmptySrv
)
self.unpause_physics_proxy = rospy.ServiceProxy(
self.param.topics.unpause_gazebo, EmptySrv
)
self._proxies = [
section_proxy,
lane_proxy,
obstacle_proxy,
surface_marking_proxy,
traffic_sign_proxy,
self.unpause_physics_proxy,
self.pause_physics_proxy,
]
rospy.wait_for_service(groundtruth_topics.section)
self.groundtruth_status_subscriber = rospy.Subscriber(
self.param.topics.groundtruth.status,
GroundtruthStatus,
callback=self.receive_groundtruth_update,
)
# Add all speakers
self.label_speaker = LabelSpeaker(
section_proxy=section_proxy,
lane_proxy=lane_proxy,
obstacle_proxy=obstacle_proxy,
surface_marking_proxy=surface_marking_proxy,
sign_proxy=traffic_sign_proxy,
)
# Subscribe to carstate
self.car_state_subscriber = rospy.Subscriber(
self.param.topics.car_state.car_state,
CarStateMsg,
self.receive_car_state,
queue_size=1,
)
self.image_subscriber = rospy.Subscriber(
"/camera/image_raw", ImageMsg, self.receive_image, queue_size=1
)
self.image_publisher = rospy.Publisher(
self.param.topics.debug_image, ImageMsg, queue_size=1
)
self.label_publisher = rospy.Publisher(
self.param.topics.image_labels, ImageLabelsMsg, queue_size=100
)
self.listener = tf2_ros.Buffer()
tf2_ros.TransformListener(self.listener)
rospy.wait_for_message(self.param.topics.car_state.car_state, CarStateMsg)