def _feedback_cb(self, msg):
"""
Monitor action feedback
"""
if msg.status == 'action out of duration':
self.cancel_plan(Empty())
rospy.sleep(self._rate.sleep_dur * 30)
self.resume_plan(Empty())
def reset(self, randomize=True):
"""
Shuffles objects at all environment locations.
"""
if randomize:
self.objects = np.random.randint(0, self.num_categories,
self.num_objects)
if not self.robot_server: self.reset_current_location(Empty())
else: self.robot_server.reset_current_location(Empty())
self.current_location = 0
self.current_state = 'init'
def MoveKeys(self, req):
action = req.action
# Open and close
if action == 'o':
msg = close()
self.CloseGripper(msg)
return True
if action == 'p':
msg = Empty()
self.OpenGripper(msg)
return True
# Start recording
if action == 'u':
self.record_trigger = True
self.memory = []
self.T = rospy.get_time()
return True
# Stop recording
if action == 'j':
self.record_trigger = False
with open(self.path + 'modelO_actionSeq_d_roll_' + str(np.random.randint(100000)) + '.pkl', 'wb') as f:
pickle.dump(self.memory, f)
return True
if np.any(action == np.array(['n','m'])):
if action == 'n':
self.df += 1.0
print "Increased velocity by 1"
if action == 'm':
self.df -= 1.0
print "Decreased velocity by 1"
return False
def reset(self):
# reset environment
os.system('rosservice call /gazebo/reset_world "{}"')
# set up the odometry reset publisher
reset_odom = rospy.Publisher('/mobile_base/commands/reset_odometry',
Empty,
queue_size=10)
# reset odometry (these messages take a few iterations to get through)
timer = time()
while time() - timer < 0.25:
reset_odom.publish(Empty())
laser_scan_data = None
# make sure laser scan values exist
while laser_scan_data is None:
try:
laser_scan_data = rospy.wait_for_message('scan',
LaserScan,
timeout=5)
except:
pass
# States last 4 elements are important -4 is heading, -3 is current distance, -2 is obstacle min range, and the -1 is obstacle angle.
# if has reached a previous goal, set a new goal
if self.initGoal:
self.goal_x, self.goal_y = self.respawn_goal.getPosition()
self.initGoal = False
self.goal_distance = self.getGoalDistace()
state, done = self.getState(laser_scan_data)
return np.asarray(state)
def __init__(self, callbacks):
smach.State.__init__(self,
outcomes=['done4', 'push', 'box1', 'return'])
self.callbacks = callbacks
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
#self.client.wait_for_server()
self.goal = MoveBaseGoal()
self.clear_costmap = rospy.ServiceProxy('/move_base/clear_costmaps',
Empty())
def set_init_pos(self):
self.error = deque([1, 100], 2)
self.distancia = deque([1]*100, 100)
self._is_ready = False
r = rospy.Rate(10)
while not self._is_ready:
r.sleep()
return Empty()
def __init__(self, *args):
self.current_state = State()
#self.curr_global_pos = NavSatFix()
rospy.init_node('offboard_ctrl')
self.offb_set_mode = SetMode
self.prev_state = self.current_state
# Publishers
self.local_pos_pub = rospy.Publisher('/mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
# Subscribers
self.state_sub = rospy.Subscriber('/mavros/state', State,
self.cb_state)
self.sub_target = rospy.Subscriber('/mavros/offbctrl/target',
PoseStamped, self.cb_target)
# Services
self.arming_client = rospy.ServiceProxy('/mavros/cmd/arming',
CommandBool)
self.takeoff_client = rospy.ServiceProxy('/mavros/cmd/takeoff',
CommandTOL)
self.set_mode_client = rospy.ServiceProxy('/mavros/set_mode', SetMode)
## Create services
self.setpoint_controller_server()
# Init msgs
self.target = PoseStamped()
self.target.pose.position.x = 0
self.target.pose.position.y = 0
self.target.pose.position.z = 2
self.last_request = rospy.get_rostime()
self.state = "INIT"
self.rate = rospy.Rate(20.0) # MUST be more then 2Hz
self.t_run = threading.Thread(target=self.navigate)
self.t_run.start()
print(">> SetPoint controller is running (Thread)")
# Spin until the node is stopped
time.sleep(5)
tmp = Empty()
self.switch2offboard(tmp)
rospy.spin()
def __init__(self, callbacks):
smach.State.__init__(self, outcomes=['success4', 'done4'])
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
# self.client.wait_for_server()
'''
position:
x: -1.10226629048
y: -2.87965063952
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.548724477596
w: 0.83600325818
'''
'''
position:
x: 0.857642769587
y: -3.15814141971
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.168549594567
w: 0.985693174457
'''
self.target0 = MoveBaseGoal()
self.target0.target_pose.header.frame_id = "map"
self.target0.target_pose.header.stamp = rospy.Time.now()
self.target0.target_pose.pose.position.x = -1.10226629048
self.target0.target_pose.pose.position.y = -2.87965063952
self.target0.target_pose.pose.orientation.x = 0.0
self.target0.target_pose.pose.orientation.y = 0.0
self.target0.target_pose.pose.orientation.z = -0.548724477596
self.target0.target_pose.pose.orientation.w = 0.83600325818
self.target = MoveBaseGoal()
self.target.target_pose.header.frame_id = "map"
self.target.target_pose.header.stamp = rospy.Time.now()
self.target.target_pose.pose.position.x = 0.857642769587
self.target.target_pose.pose.position.y = -3.15814141971
self.target.target_pose.pose.orientation.x = 0.0
self.target.target_pose.pose.orientation.y = 0.0
self.target.target_pose.pose.orientation.z = 0.168549594567
self.target.target_pose.pose.orientation.w = 0.985693174457
self.callbacks = callbacks
self.led_pub = rospy.Publisher('/mobile_base/commands/led1',
Led,
queue_size=1)
self.clear_costmap = rospy.ServiceProxy('/move_base/clear_costmaps',
Empty())
def check_collisions(self):
"""To check that the minimal distance between each CF is okay.
If actual distance between CF is too small, calls emergency service.
"""
position_dist = []
goal_dist = []
scaling_matrix_inv = inv(np.diag([1, 1, 2]))
for cf_id, each_cf in self._crazyflies.items():
cf_position = each_cf.pose.pose
cf_position = np.array([
cf_position.position.x, cf_position.position.y,
cf_position.position.z
])
cf_goal = each_cf.goals["goal"]
cf_goal = np.array([cf_goal.x, cf_goal.y, cf_goal.z])
for other_id, other_cf in self._crazyflies.items():
if other_id != cf_id:
other_pos = other_cf.pose.pose
other_pos = np.array([
other_pos.position.x, other_pos.position.y,
other_pos.position.z
])
other_goal = other_cf.goals["goal"]
other_goal = np.array(
[other_goal.x, other_goal.y, other_goal.z])
p_dist = norm(
dot(scaling_matrix_inv, cf_position - other_pos))
g_dist = norm(dot(scaling_matrix_inv,
cf_goal - other_goal))
position_dist.append(p_dist)
goal_dist.append(g_dist)
min_distances = [min(goal_dist), min(position_dist)]
if min_distances[0] < MIN_GOAL_DIST:
rospy.logwarn("Goals are too close")
if min_distances[1] < MIN_CF_DIST:
rospy.logerr("CF too close, emergency")
self._swarm_emergency_srv(Empty())
def switch2offboard(self, r):
print(">> Starting OFFBOARD mode")
last_request = rospy.get_rostime()
while self.current_state.mode != "OFFBOARD":
now = rospy.get_rostime()
if (now - last_request > rospy.Duration(5)):
print("Trying: OFFBOARD mode")
self.set_mode_client(base_mode=0, custom_mode="OFFBOARD")
last_request = now
tmp = Empty()
self.arm(tmp)
return {}
def timer_cb(self, msg):
if self.exec_traj:
print("Exec DMP")
offset = 0
if len(self.traj[0]) == 8:
print("stamped traj")
offset = 1
count = 0
prev = 0
#start_time = rospy.Time.now()
for d in self.traj:
cmd = PoseStamped()
grip_cmd = Int()
suck_cmd = Empty()
cmd.pose.position.x = d[offset + 0]
cmd.pose.position.y = d[offset + 1]
# Adding boundary value on z to avoid collision with the table
cmd.pose.position.z = d[offset +
2] if d[offset + 2] > 0.001 else 0.001
cmd.pose.orientation.x = d[offset + 3]
cmd.pose.orientation.y = d[offset + 4]
cmd.pose.orientation.z = d[offset + 5]
cmd.pose.orientation.w = d[offset + 6]
#cmd.header.stamp = start_time + rospy.Duration.from_sec(d[0])
cmd.header.stamp = rospy.Time.now()
cmd.header.frame_id = 'iiwa_link_0'
#cmd.header.seq = count
timeout = 0.1
minimal_walltime_timeout = 0.1
try:
self.cart_arm_command_pub.publish(grip_cmd)
self.gripper_proxy.publish(cmd)
self.suctiontool_proxy.publish(suck_cmd)
rospy.sleep(d[0] - prev)
prev = d[0]
# Waiting is not needed: it creates shaky movements
#while not self.dest_reached:
# pass
#self.dest_reached = False
except Exception as e:
print(e)
self.exec_traj = False
print("Done")
def __init__(self):
# # Creates a node with name 'turtlebot_controller' and make sure it is a
# # unique node (using anonymous=True).
# rospy.init_node('turtlebot_controller', anonymous=True)
# Publisher which will publish to the topic '/turtle1/cmd_vel'.
self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel',
Twist,
queue_size=10)
# A subscriber to the topic '/turtle1/pose'. self.update_pose is called
# when a message of type Pose is received.
self.pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose,
self.update_pose)
self.clear_srv = rospy.ServiceProxy("/clear", Empty())
self.pose = Pose()
self.rate = rospy.Rate(10000)
def initialize_measurement_evaluation(self, request=Empty()):
self.particles = -1.5 + 9.5 * np.random.uniform(
size=(self.num_particles, 2))
self.weights = np.repeat(1.0 / float(self.num_particles),
self.num_particles)
#self.particles, self.weights = initialize(self.num_particles, -1.5, -1.5, 7.0, 8.0)
rospy.loginfo(len(self.particles))
rospy.loginfo(len(self.weights))
self.num_of_measurements = 0
self.logging_code = str(rospy.get_time())
data = None
with open(self.log_file_path) as json_file:
data = json.load(json_file)
data[self.logging_code] = []
if data is not None:
self.write_json(data, self.log_file_path)
self.visualizer.visualize(self.particles, self.weights)
rospy.loginfo('Successfully initialized particle filter')
return True
def save_map():
map_path = "/home/abdul/catkin_ws/src/native-master/native/maps"
# Put rtabmap in localization mode so it does not continue to update map after we save it
rtabmap_localization_mode = rospy.ServiceProxy(
'rtabmap/set_mode_localization', Empty())
rtabmap_localization_mode()
# create timestamp for map file
time_stamp = "%s" % time.ctime().replace(":", "_").replace(" ", "_")
map_file = "native_2Dmap_%s" % time_stamp
db_file = "native_3Dmap_%s.db" % time_stamp
# save map file using map_server
print "Saving 2D map to file '%s'" % map_file
sts = subprocess.call('cd "%s"; rosrun map_server map_saver -f "%s"' %
(map_path, map_file),
shell=True)
# Save a default copy just named "map" also
if sts == 0:
sts = subprocess.call('cd "%s"; rosrun map_server map_saver -f "%s"' %
(map_path, "map"),
shell=True)
# make backup of the rtabmap DB
dbPath = os.environ["HOME"]
if sts == 0:
sts = subprocess.call('cp "%s/.ros/rtabmap.db" "%s/.ros/%s"' %
(dbPath, dbPath, db_file),
shell=True)
print "\n\tSave Map returned sts %d" % sts
# let user know what happened
if sts == 0:
print "\n\n\t\t*** Map Saved ok"
else:
print "\n\n\t\t*** Save map failed. Status = %d" % sts
historysize=LAST_DAY + 1,
loop=opts.loop)
else:
frmbuf = ROSCamBuffer(opts.camtopic + "/image_raw",
historysize=LAST_DAY + 1,
buffersize=30)
# start the node and control loop
rospy.init_node("flownav", anonymous=False)
datalog = DataLogger() if opts.publish else None
kbctrl = None
if opts.camtopic == "/ardrone" and not opts.video:
kbctrl = KeyboardController(max_speed=0.5, cmd_period=100)
if kbctrl:
FlatTrim = rospy.ServiceProxy("/ardrone/flattrim", Empty())
Calibrate = rospy.ServiceProxy("/ardrone/imu_recalib", Empty())
gmain_win = frmbuf.name
cv2.namedWindow(gmain_win, flags=cv2.WINDOW_OPENGL | cv2.WINDOW_NORMAL)
if opts.showmatches:
cv2.namedWindow(gtemplate_win, flags=cv2.WINDOW_OPENGL | cv2.WINDOW_NORMAL)
smatch.MAIN_WIN = gmain_win
smatch.TEMPLATE_WIN = gtemplate_win
# ==========================================================
# Additional setup before main loop
# ==========================================================
# initialize the feature description and matching methods
bfmatcher = cv2.BFMatcher()
surf_ui = cv2.SURF(hessianThreshold=opts.threshold,
def handle_reset_simulation(self, req):
"""Callback to handle the service offered by this node to reset the simulation"""
rospy.loginfo('reseting simulation now')
self.pb.resetSimulation()
return Empty()
def pr2_mover(object_list, collision_point=None):
'''function to load parameters and request PickPlace service'''
# Initialize variables
global use_collision_map
dict_list = []
labels = []
centroids = []
object_list_param = []
dropbox_param = []
pick_position = []
dropbox_position = []
test_scene_num = Int32()
object_name = String()
arm_name = String()
pick_pose = Pose()
place_pose = Pose()
test_scene_num.data = 3
# Read yaml parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
# Loop through the pick list
target_count_left = 0
target_count_right = 0
for target in object_list:
labels.append(target.label)
points_arr = ros_to_pcl(target.cloud).to_array()
pick_position = np.mean(points_arr, axis=0)[:3]
pick_pose.position.x = np.float(pick_position[0])
pick_pose.position.y = np.float(pick_position[1])
pick_pose.position.z = np.float(pick_position[2])
centroids.append(pick_position[:3])
object_name.data = str(target.label)
# Assign the arm and 'place_pose' to be used for pick_place
for index in range(0, len(object_list_param)):
if object_list_param[index]['name'] == target.label:
object_group = object_list_param[index]['group']
for ii in range(0, len(dropbox_param)):
if dropbox_param[ii]['group'] == object_group:
arm_name.data = dropbox_param[ii]['name']
dropbox_position = dropbox_param[ii]['position']
dropbox_x = -0.1 # dropbox_position[0]
# Add olace pose bias for each object
if arm_name.data == 'right':
dropbox_y = dropbox_position[
1] - 0.10 + target_count_right * 0.1
else:
dropbox_y = dropbox_position[
1] - 0.10 + target_count_left * 0.03
dropbox_z = dropbox_position[2] + 0.1
place_pose.position.x = np.float(dropbox_x)
place_pose.position.y = np.float(dropbox_y)
place_pose.position.z = np.float(dropbox_z)
# Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name,
pick_pose, place_pose)
dict_list.append(yaml_dict)
if use_collision_map:
rospy.wait_for_service('/clear_octomap')
try:
collision_map_prox = rospy.ServiceProxy(
'/clear_octomap', Empty())
resp = collision_map_prox()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
# Delete the target clound from collision map
del collision_point[target.label]
# Creating collision map
points_list = np.empty((0, 4), float)
for index, target_pts in collision_point.iteritems():
points_list = np.append(points_list, target_pts[:, :4], axis=0)
collision_cloud = pcl.PointCloud_PointXYZRGB()
collision_cloud.from_list(np.ndarray.tolist(points_list))
collision_point_pub.publish(pcl_to_ros(collision_cloud))
# Wait for 'pick_place_routine' service to come up
print("Target: ", target.label)
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine',
PickPlace)
# Insert message variables to be sent as a service request
resp = pick_place_routine(test_scene_num, object_name, arm_name,
pick_pose, place_pose)
print("Complete: ", resp.success)
# Count number to set bias value for the object arrangement
if resp.success:
if arm_name.data == 'right':
target_count_right += 1
if target_count_right == 3:
target_count_right = 0.5
else:
target_count_left += 1
except rospy.ServiceException, e:
print "Service call failed: %s" % e
rospy.init_node('controller')
pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
#sub = rospy.Subscriber('odom', Odometry, getOdom)
rospy.wait_for_service('/my_service')
my_service_client = rospy.ServiceProxy('/my_service', Trigger)
# Create an object of type EmptyRequest
my_service_object = TriggerRequest()
# Send through the connection the name of the trajectory to be executed by the robot
# create the connection to the action server
client = actionlib.SimpleActionClient('/rec_odom_as', record_odomAction)
# waits until the action server is up and running
client.wait_for_server()
client.send_goal(Empty())
#client.wait_for_result()
#print (result)
Controller = CmdVelPub()
rate = rospy.Rate(5)
counter = 0
while not rospy.is_shutdown():
state_result = client.get_state()
if state_result == 1:
result = my_service_client(my_service_object)
Controller.move_robot(result.message)
rate.sleep()
def save_map(self):
self.say("Saving map")
# Put rtabmap in localization mode so it does not continue to update map after we save it
rtabmap_localization_mode = rospy.ServiceProxy('rtabmap/set_mode_localization',Empty())
rtabmap_localization_mode()
# create timestamp for map file
time_stamp = "%s" % time.ctime().replace(":","_").replace(" ","_")
map_file = "map_%s" % time_stamp
db_file = "kc_map_%s.db" % time_stamp
# save map file using map_server
rospy.loginfo( "Saving map to file '%s'" % map_file)
sts = subprocess.call('cd "%s"; rosrun map_server map_saver -f "%s"' % (self.map_path, map_file), shell=True)
# Save a default copy just named "map" also
if sts == 0:
sts = subprocess.call('cd "%s"; rosrun map_server map_saver -f "%s"' % (self.map_path, "map"), shell=True)
# make backup of the rtabmap DB
dbPath = os.environ["HOME"]
if sts == 0:
sts = subprocess.call('cp "%s/.ros/rtabmap.db" "%s/.ros/%s"' % (dbPath, dbPath, db_file), shell=True)
rospy.loginfo( "Save Map returned sts %d" % sts)
# let user know what happened
if sts==0:
self.say("Map Saved ok" )
else:
self.say("Save map failed. Status = %d" % sts)
#! /usr/bin/env python
import rospy
from std_srvs.srv import Empty, EmptyResponse
print('In client')
rospy.init_node(
'bb8_move_client') # Initialise a ROS node with the name service_client
rospy.wait_for_service('/move_bb8_in_square')
execBB8 = rospy.ServiceProxy('/move_bb8_in_square', Empty)
request = Empty()
result = execBB8()
print result
#rospy.spin()
def Move(self, ch):
step = 0.01
if ch == 's': # Record fingers
return 1
if ch == 'x': # Down
self.initial_pose.position.z -= step
return 1
if ch == 'w': # Up
self.initial_pose.position.z += step
return 1
if ch == 'a': # Left
self.initial_pose.position.y -= step
return 1
if ch == 'd': # Right
self.initial_pose.position.y += step
return 1
if ch == 'q': # Backward
self.initial_pose.position.x -= step
return 1
if ch == 'e': # Fardward
self.initial_pose.position.x += step
return 1
step = 0.1
if ch == 'i': # roll -
self.rpy[0] -= step
print(self.rpy)
return 1
if ch == 'o': # roll +
self.rpy[0] += step
return 1
if ch == 'k': # pitch -
self.rpy[1] -= step
return 1
if ch == 'l': # pitch +
self.rpy[1] += step
return 1
if ch == ',': # yaw -
self.rpy[2] -= step
return 1
if ch == '.': # yaw +
self.rpy[2] += step
return 1
if ch == '[': # close hand
self.close_srv()
return 2
if ch == ']': # open hand
self.open_srv()
return 2
if ch == 'r': # Random spawn
msg = Empty()
self.random_spawn(msg)
return 2
if ch >= '1' and ch <= '5':
res = self.delete_model_proxy('sph')
initial_pose = Pose()
f = ['1', '2', '3', '4', '5'].index(ch)
print(self.fingers[f, :])
initial_pose.position.x = self.fingers[f, 0]
initial_pose.position.y = self.fingers[f, 1]
initial_pose.position.z = self.fingers[f, 2] + 1.0
self.spawn_model_proxy('sph', self.sphere_file, "", initial_pose,
'world')
return 2
if ch == 't': # Log coordinates
f = open(
r'/home/avishai/catkin_ws/src/pisa_hand/object_spawn/data/finger_logs_o'
+ str(self.obj_num) + '_f' + str(self.fingers.shape[0]) +
'.txt', "a")
for s in self.fingers.flatten():
f.write(' ' + str(s))
f.write('\n')
f.close()
self.log_count += 1
rospy.loginfo('Point logged, total %d.' % self.log_count)
return 2
def __init__(self):
rospy.init_node('spawn_object', log_level=rospy.INFO)
self.initial_pose = Pose()
self.initial_pose.position.x = self.position[0]
self.initial_pose.position.y = self.position[1]
self.initial_pose.position.z = self.position[2]
self.rpy = np.array([0.1, 0.1, 0.1])
f = open(
'/home/avishai/catkin_ws/src/pisa_hand/object_spawn/urdf/object.urdf',
'r')
self.urdf_file = f.read()
f = open(
'/home/avishai/catkin_ws/src/pisa_hand/object_spawn/urdf/sphere.urdf',
'r')
self.sphere_file = f.read()
# Find object number
f_copy = open(
'/home/avishai/catkin_ws/src/pisa_hand/object_spawn/urdf/object.urdf',
'r')
datafile = f_copy.readlines()
for line in datafile:
l = line.find('/meshes/')
if l > -1:
self.obj_num = line[l + 8:l + 10]
break
f_copy.close()
rospy.loginfo('Spawning object number %s.' % self.obj_num)
self.log_count = 0
rospy.wait_for_service('gazebo/spawn_urdf_model')
self.spawn_model_proxy = rospy.ServiceProxy('gazebo/spawn_urdf_model',
SpawnModel)
self.delete_model_proxy = rospy.ServiceProxy('gazebo/delete_model',
DeleteModel)
rospy.Subscriber('finger_state', Float32MultiArray,
self.FingerStatesCallback)
self.open_srv = rospy.ServiceProxy('/OpenGripper', Empty)
self.close_srv = rospy.ServiceProxy('/CloseGripper', Empty)
rospy.Service('/respawn', Empty, self.respawn)
rospy.Service('/spawn_random', Empty, self.random_spawn)
msg = Empty()
rate = rospy.Rate(100)
rospy.loginfo('Ready to spawn...')
while not rospy.is_shutdown():
if self.use_keyboard:
ch = self.getchar()
print(ch)
if ord(ch) == 27:
break
k = self.Move(ch)
if k == 1:
self.respawn(msg)
rate.sleep()
def __init__(self, callbacks):
smach.State.__init__(self, outcomes=['success4', 'done4'])
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
# self.client.wait_for_server()
'''
target1
position:
x: -1.52175857821
y: -1.38008777639
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.570304326137
w: 0.821433488232
'''
'''
target2
position:
x: -0.472800379645
y: -3.4958956586
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.153404103418
w: 0.98816353963
'''
'''
target3
position:
x: 0.857642769587
y: -3.15814141971
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.168549594567
w: 0.985693174457
'''
self.target1 = MoveBaseGoal()
self.target1.target_pose.header.frame_id = "map"
self.target1.target_pose.header.stamp = rospy.Time.now()
self.target1.target_pose.pose.position.x = -1.52175857821
self.target1.target_pose.pose.position.y = -1.38008777639
self.target1.target_pose.pose.orientation.x = 0.0
self.target1.target_pose.pose.orientation.y = 0.0
self.target1.target_pose.pose.orientation.z = -0.570304326137
self.target1.target_pose.pose.orientation.w = 0.821433488232
self.target2 = MoveBaseGoal()
self.target2.target_pose.header.frame_id = "map"
self.target2.target_pose.header.stamp = rospy.Time.now()
self.target2.target_pose.pose.position.x = -0.472800379645
self.target2.target_pose.pose.position.y = -3.4958956586
self.target2.target_pose.pose.orientation.x = 0.0
self.target2.target_pose.pose.orientation.y = 0.0
self.target2.target_pose.pose.orientation.z = 0.153404103418
self.target2.target_pose.pose.orientation.w = 0.98816353963
self.target3 = MoveBaseGoal()
self.target3.target_pose.header.frame_id = "map"
self.target3.target_pose.header.stamp = rospy.Time.now()
self.target3.target_pose.pose.position.x = 0.857642769587
self.target3.target_pose.pose.position.y = -3.15814141971
self.target3.target_pose.pose.orientation.x = 0.0
self.target3.target_pose.pose.orientation.y = 0.0
self.target3.target_pose.pose.orientation.z = 0.168549594567
self.target3.target_pose.pose.orientation.w = 0.985693174457
self.callbacks = callbacks
self.cmd_vel_pub = rospy.Publisher('mobile_base/commands/velocity',
Twist,
queue_size=1)
self.twist = Twist()
self.clear_costmap = rospy.ServiceProxy('/move_base/clear_costmaps',
Empty())
#! /usr/bin/env python
import rospy
from path_exam.srv import Motion_Service, Motion_ServiceResponse
from std_srvs.srv import Empty, EmptyResponse # you import the service message python classes generated from Empty.srv.
from std_msgs.msg import Empty as Empty_msg
from geometry_msgs.msg import Twist
rospy.init_node("motion_service")
rate = rospy.Rate(2) # Set a publish rate of 2 Hz
empty = Empty()
empty_msg = Empty_msg()
emptyResponse = EmptyResponse()
ctrl_c = False
pub = rospy.Publisher('drone/takeoff', Empty_msg, queue_size=1)
forward = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
land = rospy.Publisher('drone/land', Empty_msg, queue_size=1)
move_front = Twist()
move_front.linear.x = 1
stop = Twist()
def move_forward():
while not ctrl_c:
connections = pub.get_num_connections()
if connections > 0:
pub.publish(empty_msg)
rospy.loginfo("takeoff Published")
break
else: