def go_position(self,desired_goal):
# compensate using drift and vision position
goal_position = np.zeros(3)
goal_position[0] = desired_goal[0] + (self.position[0] - self.vision_pose[0])
goal_position[1] = desired_goal[1] + (self.position[1] - self.vision_pose[1])
goal_position[2] = desired_goal[2]
self.command_pose[0] = goal_position[0]
self.command_pose[1] = goal_position[1]
self.command_pose[2] = goal_position[2]
rate = rospy.Rate(20)
start_time = rospy.Time.now()
while (self.position_error2(self.command_pose, self.px4_position) >= self.position_error_threshold ) or ((rospy.Time.now() - start_time) < rospy.Duration(self.goal_wait_time)):
if ((self.position_error2(self.command_pose, self.px4_position) >= self.position_error_threshold )):
print("Position error: {:.4f} > {:.4f}".format(self.position_error2(self.command_pose, self.px4_position), self.position_error_threshold) )
self.publish_command(self.command_pose)
rate.sleep()
print("Finish ")
return True
def main():
rospy.init_node('ur_driver', disable_signals=True)
if rospy.get_param("use_sim_time", False):
rospy.logwarn("use_sim_time is set!!!")
global prevent_programming
prevent_programming = rospy.get_param("prevent_programming", False)
prefix = rospy.get_param("~prefix", "")
print "Setting prefix to %s" % prefix
global joint_names
joint_names = [prefix + name for name in JOINT_NAMES]
# Parses command line arguments
parser = optparse.OptionParser(
usage="usage: %prog robot_hostname [reverse_port]")
(options, args) = parser.parse_args(rospy.myargv()[1:])
if len(args) < 1:
parser.error("You must specify the robot hostname")
elif len(args) == 1:
robot_hostname = args[0]
reverse_port = DEFAULT_REVERSE_PORT
elif len(args) == 2:
robot_hostname = args[0]
reverse_port = int(args[1])
if not (0 <= reverse_port <= 65535):
parser.error("You entered an invalid port number")
else:
parser.error("Wrong number of parameters")
# Reads the calibrated joint offsets from the URDF
global joint_offsets
joint_offsets = load_joint_offsets(joint_names)
if len(joint_offsets) > 0:
rospy.loginfo("Loaded calibration offsets from urdf: %s" %
joint_offsets)
else:
rospy.loginfo("No calibration offsets loaded from urdf")
# Reads the maximum velocity
# The max_velocity parameter is only used for debugging in the ur_driver. It's not related to actual velocity limits
global max_velocity
max_velocity = rospy.get_param("~max_velocity", MAX_VELOCITY) # [rad/s]
rospy.loginfo("Max velocity accepted by ur_driver: %s [rad/s]" %
max_velocity)
# Reads the minimum payload
global min_payload
min_payload = rospy.get_param("~min_payload", MIN_PAYLOAD)
# Reads the maximum payload
global max_payload
max_payload = rospy.get_param("~max_payload", MAX_PAYLOAD)
rospy.loginfo("Bounds for Payload: [%s, %s]" % (min_payload, max_payload))
# Sets up the server for the robot to connect to
server = TCPServer(("", reverse_port), CommanderTCPHandler)
thread_commander = threading.Thread(name="CommanderHandler",
target=server.serve_forever)
thread_commander.daemon = True
thread_commander.start()
with open(roslib.packages.get_pkg_dir('ur_driver') + '/prog') as fin:
program = fin.read() % {
"driver_hostname": get_my_ip(robot_hostname, PORT),
"driver_reverseport": reverse_port
}
connection = URConnection(robot_hostname, PORT, program)
connection.connect()
connection.send_reset_program()
connectionRT = URConnectionRT(robot_hostname, RT_PORT)
connectionRT.connect()
set_io_server()
service_provider = None
action_server = None
try:
while not rospy.is_shutdown():
# Checks for disconnect
if getConnectedRobot(wait=False):
time.sleep(0.2)
prevent_programming = rospy.get_param("prevent_programming",
False)
if prevent_programming:
print "Programming now prevented"
connection.send_reset_program()
else:
print "Disconnected. Reconnecting"
if action_server:
action_server.set_robot(None)
rospy.loginfo("Programming the robot")
while True:
# Sends the program to the robot
while not connection.ready_to_program():
print "Waiting to program"
time.sleep(1.0)
prevent_programming = rospy.get_param(
"prevent_programming", False)
connection.send_program()
r = getConnectedRobot(wait=True, timeout=1.0)
if r:
break
rospy.loginfo("Robot connected")
#provider for service calls
if service_provider:
service_provider.set_robot(r)
else:
service_provider = URServiceProvider(r)
if action_server:
action_server.set_robot(r)
else:
action_server = URTrajectoryFollower(
r, rospy.Duration(1.0))
action_server.start()
except KeyboardInterrupt:
try:
r = getConnectedRobot(wait=False)
rospy.signal_shutdown("KeyboardInterrupt")
if r: r.send_quit()
except:
pass
raise
def __init__(self):
self.homing_target = ['head/pan', 'body/body_rotate', 'body/arm_base', 'body/elevation']
for controller in self.homing_target:
self.current_target = controller
client = actionlib.SimpleActionClient('%s/homing'%self.current_target, HomingAction)
client.wait_for_server()
goal = HomingActionGoal()
client.send_goal(goal, done_cb=self.func_done, active_cb=self.func_active)
if not client.wait_for_result():
rospy.logerr('%s homing failed. please check the error message'%self.current_target)
quit()
rospy.sleep(1.0)
rospy.sleep(2.0)
rospy.loginfo("Move to initial pose.")
#
# Head Parts
#
pub = rospy.Publisher('/head/pan_controller/command', Float64, queue_size=10)
rospy.sleep(0.4)
pub.publish(data=0.0)
rospy.sleep(1.0)
pub = rospy.Publisher('/head/tilt_controller/command', Float64, queue_size=10)
rospy.sleep(0.4)
pub.publish(data=0.0)
rospy.sleep(1.0)
pub = rospy.Publisher('/head/screen_tilt_controller/command', Float64, queue_size=10)
rospy.sleep(0.4)
pub.publish(data=0.0)
rospy.sleep(1.0)
#
# Body Parts
#
pub = rospy.Publisher('/body/arm_base_controller/command', Float64, queue_size=10)
rospy.sleep(0.4)
pub.publish(data=0.0)
rospy.sleep(1.0)
pub = rospy.Publisher('/body/gripper_controller/command', Float64, queue_size=10)
rospy.sleep(0.4)
pub.publish(data=0.0)
rospy.sleep(1.5)
pub = rospy.Publisher('/body/gripper_controller/command', Float64, queue_size=10)
rospy.sleep(0.4)
pub.publish(data=1.0)
rospy.sleep(1.5)
# Move arm to ready pose
client = actionlib.SimpleActionClient('/body/arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
client.wait_for_server()
goal = FollowJointTrajectoryGoal()
goal.trajectory.header.stamp = rospy.Time.now()
goal.trajectory.joint_names = ['body_rotate_joint', 'elevation_joint', 'arm1_joint', 'arm2_joint', 'arm3_joint', 'arm4_joint', 'arm5_joint', 'arm6_joint']
point = JointTrajectoryPoint()
point.positions = [0.0, 0.0, 0.0, 1.5708, -3.14, 0.0, 0.0, 0.0]
point.time_from_start = rospy.Duration(1.0)
goal.trajectory.points.append(point)
client.send_goal(goal)
client.wait_for_result()
rospy.sleep(1.0)
goal.trajectory.header.stamp = rospy.Time.now()
point.positions = [0.0, -0.215, 0.0, 1.57, -3.14, 0.0, 0.0, 0.0]
point.time_from_start = rospy.Duration(4.0)
client.send_goal(goal)
client.wait_for_result()
rospy.sleep(1.0)
pub = rospy.Publisher('/body/arm_base_controller/command', Float64, queue_size=10)
rospy.sleep(0.4)
pub.publish(data=-0.15)
rospy.sleep(1.0)
rospy.loginfo("All homing procedure was done successfully.")
quit()
eef = MoveGroupCommander("endeffector")
rospy.sleep(1)
#Start State Gripper
group_variable_values = eef.get_current_joint_values()
# group_variable_values[0] = 0.0
eef.set_joint_value_target(group_variable_values)
plan2 = eef.plan()
arm.execute(plan2)
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("table")
scene.remove_world_object("part")
client = actionlib.SimpleActionClient('gripper_controller/gripper_action',
FollowJointTrajectoryAction)
if not client.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
# publish a demo scene
k = PoseStamped()
k.header.frame_id = robot.get_planning_frame()
k.pose.position.x = 0.0
k.pose.position.y = 0.0
k.pose.position.z = -0.05
scene.add_box("table", k, (2, 2, 0.0001))
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.35
p.pose.position.y = 0.35
p.pose.position.z = 0.05
def tf_transform_pose(self, ps, frame):
output_pose = geometry_msgs.msg.PointStamped
self.tf_listener.waitForTransform(frame, ps.header.frame_id,
rospy.Time(), rospy.Duration(2.0))
output_pose = self.tf_listener.transformPose(frame, ps)
return output_pose
def process_trajectory(self, traj):
num_points = len(traj.points)
# make sure the joints in the goal match the joints of the controller
if set(self.joint_names) != set(traj.joint_names):
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_JOINTS
msg = 'Incoming trajectory joints do not match the joints of the controller'
rospy.logerr(msg)
rospy.logerr(' self.joint_names={%s}' % (set(self.joint_names)))
rospy.logerr(' traj.joint_names={%s}' % (set(traj.joint_names)))
self.action_server.set_aborted(result=res, text=msg)
return
# make sure trajectory is not empty
if num_points == 0:
msg = 'Incoming trajectory is empty'
rospy.logerr(msg)
self.action_server.set_aborted(text=msg)
return
# correlate the joints we're commanding to the joints in the message
# map from an index of joint in the controller to an index in the trajectory
lookup = [traj.joint_names.index(joint) for joint in self.joint_names]
durations = [0.0] * num_points
# find out the duration of each segment in the trajectory
durations[0] = traj.points[0].time_from_start.to_sec()
for i in range(1, num_points):
durations[i] = (traj.points[i].time_from_start -
traj.points[i - 1].time_from_start).to_sec()
if not traj.points[0].positions:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_GOAL
msg = 'First point of trajectory has no positions'
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
trajectory = []
time = rospy.Time.now() + rospy.Duration(0.01)
for i in range(num_points):
seg = Segment(self.num_joints)
if traj.header.stamp == rospy.Time(0.0):
seg.start_time = (time + traj.points[i].time_from_start
).to_sec() - durations[i]
else:
seg.start_time = (
traj.header.stamp +
traj.points[i].time_from_start).to_sec() - durations[i]
seg.duration = durations[i]
# Checks that the incoming segment has the right number of elements.
if traj.points[i].velocities and len(
traj.points[i].velocities) != self.num_joints:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_GOAL
msg = 'Command point %d has %d elements for the velocities' % (
i, len(traj.points[i].velocities))
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
if len(traj.points[i].positions) != self.num_joints:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_GOAL
msg = 'Command point %d has %d elements for the positions' % (
i, len(traj.points[i].positions))
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
for j in range(self.num_joints):
if traj.points[i].velocities:
seg.velocities[j] = traj.points[i].velocities[lookup[j]]
if traj.points[i].positions:
seg.positions[j] = traj.points[i].positions[lookup[j]]
trajectory.append(seg)
rospy.loginfo('Trajectory start requested at %.3lf, waiting...',
traj.header.stamp.to_sec())
rate = rospy.Rate(self.update_rate)
while traj.header.stamp > time:
time = rospy.Time.now()
rate.sleep()
end_time = traj.header.stamp + rospy.Duration(sum(durations))
seg_end_times = [
rospy.Time.from_sec(trajectory[seg].start_time + durations[seg])
for seg in range(len(trajectory))
]
rospy.loginfo(
'Trajectory start time is %.3lf, end time is %.3lf, total duration is %.3lf',
time.to_sec(), end_time.to_sec(), sum(durations))
self.trajectory = trajectory
traj_start_time = rospy.Time.now()
for seg in range(len(trajectory)):
rospy.logdebug(
'current segment is %d time left %f cur time %f' %
(seg, durations[seg] -
(time.to_sec() - trajectory[seg].start_time), time.to_sec()))
rospy.logdebug('goal positions are: %s' %
str(trajectory[seg].positions))
# first point in trajectories calculated by OMPL is current position with duration of 0 seconds, skip it
if durations[seg] == 0:
rospy.logdebug(
'skipping segment %d with duration of 0 seconds' % seg)
continue
multi_packet = {}
for port, joints in self.port_to_joints.items():
vals = []
for joint in joints:
j = self.joint_names.index(joint)
start_position = self.joint_states[joint].current_pos
if seg != 0:
start_position = trajectory[seg - 1].positions[j]
desired_position = trajectory[seg].positions[j]
desired_velocity = max(
self.min_velocity,
abs(desired_position - start_position) /
durations[seg])
self.msg.desired.positions[j] = desired_position
self.msg.desired.velocities[j] = desired_velocity
# probably need a more elegant way of figuring out if we are dealing with a dual controller
if hasattr(self.joint_to_controller[joint], "master_id"):
master_id = self.joint_to_controller[joint].master_id
slave_id = self.joint_to_controller[joint].slave_id
# added motor bias [AHC: 20171127]
master_pos, slave_pos = self.joint_to_controller[
joint].pos_rad_to_raw(desired_position +
self.joint_to_bias[joint])
spd = self.joint_to_controller[joint].spd_rad_to_raw(
desired_velocity)
vals.append((master_id, master_pos, spd))
vals.append((slave_id, slave_pos, spd))
else:
motor_id = self.joint_to_controller[joint].motor_id
# added motor bias [AHC: 20171127]
pos = self.joint_to_controller[joint].pos_rad_to_raw(
desired_position + self.joint_to_bias[joint])
spd = self.joint_to_controller[joint].spd_rad_to_raw(
desired_velocity)
vals.append((motor_id, pos, spd))
multi_packet[port] = vals
for port, vals in multi_packet.items():
self.port_to_io[port].set_multi_position_and_speed(vals)
while time < seg_end_times[seg]:
# check if new trajectory was received, if so abort current trajectory execution
# by setting the goal to the current position
if self.action_server.is_preempt_requested():
msg = 'New trajectory received. Aborting old trajectory.'
multi_packet = {}
for port, joints in self.port_to_joints.items():
vals = []
for joint in joints:
cur_pos = self.joint_states[joint].current_pos
motor_id = self.joint_to_controller[joint].motor_id
# added motor bias [AHC: 20171127]
pos = self.joint_to_controller[
joint].pos_rad_to_raw(
cur_pos + self.joint_to_bias[joint])
vals.append((motor_id, pos))
multi_packet[port] = vals
for port, vals in multi_packet.items():
self.port_to_io[port].set_multi_position(vals)
self.action_server.set_preempted(text=msg)
rospy.logwarn(msg)
return
rate.sleep()
time = rospy.Time.now()
# Verifies trajectory constraints
for j, joint in enumerate(self.joint_names):
if self.trajectory_constraints[
j] > 0 and self.msg.error.positions[
j] > self.trajectory_constraints[j]:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED
msg = 'Unsatisfied position constraint for %s, trajectory point %d, %f is larger than %f' % \
(joint, seg, self.msg.error.positions[j], self.trajectory_constraints[j])
rospy.logwarn(msg)
self.action_server.set_aborted(result=res, text=msg)
return
# let motors roll for specified amount of time
rospy.sleep(self.goal_time_constraint)
for i, j in enumerate(self.joint_names):
rospy.logdebug(
'desired pos was %f, actual pos is %f, error is %f' %
(trajectory[-1].positions[i], self.joint_states[j].current_pos,
self.joint_states[j].current_pos -
trajectory[-1].positions[i]))
# Checks that we have ended inside the goal constraints
for (joint, pos_error, pos_constraint) in zip(self.joint_names,
self.msg.error.positions,
self.goal_constraints):
if pos_constraint > 0 and abs(pos_error) > pos_constraint:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED
msg = 'Aborting because %s joint wound up outside the goal constraints, %f is larger than %f' % \
(joint, pos_error, pos_constraint)
rospy.logwarn(msg)
self.action_server.set_aborted(result=res, text=msg)
break
else:
msg = 'Trajectory execution successfully completed'
rospy.loginfo(msg)
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.SUCCESSFUL
self.action_server.set_succeeded(result=res, text=msg)
rospy.init_node('rosbag_filter', anonymous=True)
pubber = rospy.Publisher("/move_group/filtered_cloud",
sensor_msgs.msg.PointCloud2,
queue_size=5)
time_now = rospy.get_rostime()
time_prev = rospy.Time()
# i = 0
while not rospy.is_shutdown():
# i += 1
# rospy.loginfo(i)
bag = rosbag.Bag('/home/ash/filtered_cloud1.bag')
for topic, org_msg, t in bag.read_messages(
topics='/move_group/filtered_cloud'):
msg = copy.deepcopy(org_msg)
if time_prev.secs == 0:
d_time = rospy.Duration()
time_prev = copy.deepcopy(msg.header.stamp)
msg.header.stamp = copy.deepcopy(time_now)
else:
d_time = msg.header.stamp - time_prev
time_now += d_time
time_prev = copy.deepcopy(msg.header.stamp)
msg.header.stamp = copy.deepcopy(time_now)
# rospy.loginfo(msg.header.stamp.secs)
rospy.sleep(d_time)
try:
pubber.publish(msg)
except:
break
else:
pass
def __init__(self):
rospy.init_node('Arduino', log_level=rospy.DEBUG)
# Get the actual node name in case it is set in the launch file
self.name = rospy.get_name()
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0")
self.baud = int(rospy.get_param("~baud", 57600))
self.timeout = rospy.get_param("~timeout", 0.5)
self.base_frame = rospy.get_param("~base_frame", 'base_link')
# Overall loop rate: should be faster than fastest sensor rate
self.rate = int(rospy.get_param("~rate", 50))
r = rospy.Rate(self.rate)
# Rate at which summary SensorState message is published. Individual sensors publish
# at their own rates.
self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
self.use_base_controller = rospy.get_param("~use_base_controller", False)
# Set up the time for publishing the next SensorState message
now = rospy.Time.now()
self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
self.t_next_sensors = now + self.t_delta_sensors
# Initialize a Twist message
self.cmd_vel = Twist()
# A cmd_vel publisher so we can stop the robot when shutting down
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# The SensorState publisher periodically publishes the values of all sensors on
# a single topic.
self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState, queue_size=5)
# A service to position a PWM servo
rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
# A service to read the position of a PWM servo
rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
# A service to turn set the direction of a digital pin (0 = input, 1 = output)
rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
# A service to turn a digital sensor on or off
rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
# A service to set pwm values for the pins
rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
# Initialize the controlller
self.controller = Arduino(self.port, self.baud, self.timeout)
# Make the connection
self.controller.connect()
rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
# Reserve a thread lock
mutex = thread.allocate_lock()
# Initialize any sensors
self.mySensors = list()
sensor_params = rospy.get_param("~sensors", dict({}))
for name, params in sensor_params.iteritems():
# Set the direction to input if not specified
try:
params['direction']
except:
params['direction'] = 'input'
if params['type'] == "Ping":
sensor = Ping(self.controller, name, params['pin'], params['echopin'], params['rate'], params['frame_id'])
elif params['type'] == "PointCloudPing":
sensor = PointCloudPing(self.controller, name, params['pin'], params['echopin'],params['rate'], params['frame_id'])
elif params['type'] == "GP2D12":
sensor = GP2D12(self.controller, name, params['pin'], params['rate'])
elif params['type'] == 'Digital':
sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
elif params['type'] == 'Analog':
sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
elif params['type'] == 'PololuMotorCurrent':
sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'] )
elif params['type'] == 'PhidgetsVoltage':
sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'])
elif params['type'] == 'PhidgetsCurrent':
sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'])
# if params['type'] == "MaxEZ1":
# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
# self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
try:
self.mySensors.append(sensor)
rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
except:
rospy.logerr("Sensor type " + str(params['type']) + " not recognized.")
# Initialize the base controller if used
if self.use_base_controller:
self.myBaseController = BaseController(self.controller, self.base_frame, self.name + "_base_controller")
# Start polling the sensors and base controller
while not rospy.is_shutdown():
for sensor in self.mySensors:
mutex.acquire()
sensor.poll()
mutex.release()
if self.use_base_controller:
mutex.acquire()
self.myBaseController.poll()
mutex.release()
# Publish all sensor values on a single topic for convenience
now = rospy.Time.now()
if now > self.t_next_sensors:
msg = SensorState()
msg.header.frame_id = 'sensor_base'
msg.header.stamp = now
for i in range(len(self.mySensors)):
msg.name.append(self.mySensors[i].name)
msg.value.append(self.mySensors[i].value)
try:
self.sensorStatePub.publish(msg)
except:
pass
self.t_next_sensors = now + self.t_delta_sensors
r.sleep()
def update_average_detection(self, m_array):
"""
Using poses v1
"""
if self.old_msg == m_array:
# Message old
return
if not self.cf1_pose:
# need pose
return
if self.is_measuring:
return
self.is_measuring = True
self.old_msg = m_array
kalman_gains = []
filtered_poses = []
n_markers = len(m_array.markers)
#time_stamp = None
for marker in m_array.markers:
# TODO: make this general (no hardcoded Qs)
if marker.id > 9:
frame_detected = "sign_detected/stop"
frame_marker = "sign/stop"
print("Using stop sign!!!")
Q = np.diag([0.5, 0.5, 0.5])
else:
frame_detected = "aruco/detected" + str(marker.id)
frame_marker = "aruco/marker" + str(marker.id)
Q = np.diag([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
try:
# just to get correct time stamp
time_stamp = self.tf_buf.lookup_transform(
frame_marker, frame_detected, rospy.Time(0)).header.stamp
except:
print("Wait a bit...")
self.is_measuring = False
return
try:
believed_trans = self.tf_buf.lookup_transform(
"map", "cf1/base_link", time_stamp, rospy.Duration(.1))
except:
self.is_measuring = False
print("Wait a bit...")
return
believed_pose = self.transform_to_pose_stamped(believed_trans)
self.believed_pub.publish(believed_pose)
believed_state = self.pose_stamped_to_state(believed_pose)
measured_pose = self.get_measured_pose_filtered(
believed_pose, frame_marker, frame_detected) #DEBUG THIS
if not measured_pose:
print("Nah")
self.is_measuring = False
return
if len(m_array.markers) == 1:
self.measurement_pub.publish(
measured_pose) # for vizualisation
if measured_pose is None:
print("Failed1")
self.is_measuring = False
return
measured_state = self.pose_stamped_to_state(measured_pose)
diff = self.kf.inovation(believed_state * self.filter_config,
measured_state * self.filter_config)
maha_dist = self.maha_dist(diff, Q)
print("Mahalanobis dist (kinda): {}".format(maha_dist))
if maha_dist > 5:
# outlier
print("Outlier")
self.is_measuring = False
return
K = self.kf.kalman_gain(Q)
kalman_gains.append(K)
filtered_state = self.filter_state(believed_state, measured_state,
K)
#filtered_state = believed_state + self.kf.inovation(believed_state, measured_state)*self.filter_config*0.5
filtered_pose = self.state_to_pose_stamped(
filtered_state, believed_pose.header.frame_id, time_stamp)
filtered_poses.append(filtered_pose)
print("Using {}/{} markers measurements".format(
len(filtered_poses), n_markers))
K = sum(kalman_gains) / len(filtered_poses)
filtered_pose = self.average_poses(filtered_poses)
self.filtered_pub.publish(filtered_pose) # for visualization
self.broadcast_pose_frame(filtered_pose, "cf1/base_link/filtered")
odom_new_pose = self.get_odom_new_pose(believed_pose)
if not odom_new_pose:
print("Failed3")
self.is_measuring = False
return
self.odom_new_pub.publish(odom_new_pose)
map_to_odom = self.get_map_to_odom_transform(odom_new_pose)
if map_to_odom:
print("Updated")
self.kf.update_with_gain(K)
#self.last_transform = map_to_odom # remove if not using this as callback
self.is_measuring = False
return map_to_odom
print("SOMETHING WENT WRONG")
self.is_measuring = False
def main():
rospy.init_node('ur_driver', disable_signals=True)
if rospy.get_param("use_sim_time", False):
rospy.logwarn("use_sim_time is set!!!")
global prevent_programming
prevent_programming = rospy.get_param("prevent_programming", False)
prefix = rospy.get_param("~prefix", "")
print "Setting prefix to %s" % prefix
global joint_names
joint_names = [prefix + name for name in JOINT_NAMES]
# Parses command line arguments
parser = optparse.OptionParser(usage="usage: %prog robot_hostname")
(options, args) = parser.parse_args(rospy.myargv()[1:])
if len(args) != 1:
parser.error("You must specify the robot hostname")
robot_hostname = args[0]
# Reads the calibrated joint offsets from the URDF
global joint_offsets
joint_offsets = load_joint_offsets(joint_names)
rospy.logerr("Loaded calibration offsets: %s" % joint_offsets)
# Reads the maximum velocity
global max_velocity
max_velocity = rospy.get_param("~max_velocity", 2.0)
# Sets up the server for the robot to connect to
server = TCPServer(("", 50001), CommanderTCPHandler)
thread_commander = threading.Thread(name="CommanderHandler", target=server.serve_forever)
thread_commander.daemon = True
thread_commander.start()
with open(roslib.packages.get_pkg_dir('ur_driver') + '/prog') as fin:
program = fin.read() % {"driver_hostname": get_my_ip(robot_hostname, PORT)}
connection = UR5Connection(robot_hostname, PORT, program)
connection.connect()
connection.send_reset_program()
srv_set_io = rospy.Service('set_io_state', SetIOState, handle_set_io_state)
action_server = None
try:
while not rospy.is_shutdown():
# Checks for disconnect
if getConnectedRobot(wait=False):
time.sleep(0.2)
prevent_programming = rospy.get_param("prevent_programming", False)
if prevent_programming:
print "Programming now prevented"
connection.send_reset_program()
else:
print "Disconnected. Reconnecting"
if action_server:
action_server.set_robot(None)
rospy.loginfo("Programming the robot")
while True:
# Sends the program to the robot
while not connection.ready_to_program():
print "Waiting to program"
time.sleep(1.0)
prevent_programming = rospy.get_param("prevent_programming", False)
connection.send_program()
r = getConnectedRobot(wait=True, timeout=1.0)
if r:
break
rospy.loginfo("Robot connected")
if action_server:
action_server.set_robot(r)
else:
action_server = UR5TrajectoryFollower(r, rospy.Duration(1.0))
action_server.start()
except KeyboardInterrupt:
try:
r = getConnectedRobot(wait=False)
rospy.signal_shutdown("KeyboardInterrupt")
if r: r.send_quit()
except:
pass
raise
joint_client = actionlib.SimpleActionClient(
'/irp6ot_arm/spline_trajectory_action_joint',
FollowJointTrajectoryAction)
joint_client.wait_for_server()
print 'server ok'
# joint_client.cancel_all_goals()
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7'
]
goal.trajectory.points.append(
JointTrajectoryPoint(
[0.0, 0.0, -1.5418065817051163, 0.0, 1.5, 1.57, -1.57],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [], [], rospy.Duration(6.0)))
goal.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0.2)
joint_client.send_goal(goal)
joint_client.wait_for_result()
command_result = joint_client.get_result()
conmanSwitchIrp6ot([], ['Irp6otmSplineTrajectoryGeneratorJoint'], True)
#
# Cartesian coordinates motion
#
conmanSwitchIrp6ot(['Irp6otmPoseInt'], [], True)
def interp_cubic(p0, p1, t_abs):
T = (p1.time_from_start - p0.time_from_start).to_sec()
t = t_abs - p0.time_from_start.to_sec()
q = [0] * 6
qdot = [0] * 6
qddot = [0] * 6
for i in range(len(p0.positions)):
a = p0.positions[i]
b = p0.velocities[i]
c = (-3*p0.positions[i] + 3*p1.positions[i] - 2*T*p0.velocities[i] - T*p1.velocities[i]) / T**2
d = (2*p0.positions[i] - 2*p1.positions[i] + T*p0.velocities[i] + T*p1.velocities[i]) / T**3
q[i] = a + b*t + c*t**2 + d*t**3
qdot[i] = b + 2*c*t + 3*d*t**2
qddot[i] = 2*c + 6*d*t
return JointTrajectoryPoint(positions=q, velocities=qdot, accelerations=qddot, time_from_start=rospy.Duration(t_abs))
def execute(self, userdata):
#Construct goal and send it
status, started, stopped = self.controller_manager.joint_mode(self.arm)
goal = an.MoveArmGoal()
goal.motion_plan_request.group_name = self.group_name
goal.motion_plan_request.num_planning_attempts = 2
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
for (joint_name, joint_angle) in zip(self.joint_names, self.joints):
joint_constraint = an.JointConstraint()
joint_constraint.joint_name = joint_name
joint_constraint.position = joint_angle
joint_constraint.tolerance_below = .1
joint_constraint.tolerance_above = .1
goal.motion_plan_request.goal_constraints.joint_constraints.append(
joint_constraint)
self.move_arm_client.send_goal(goal)
#Wait for action to finish
r = rospy.Rate(30)
start_time = rospy.get_time()
state = self.move_arm_client.get_state()
preempted = False
succeeded = False
while not rospy.is_shutdown():
#we have been preempted
if self.preempt_requested():
rospy.loginfo('SafeMoveArmState: preempt requested')
self.move_arm_client.cancel_goal()
self.service_preempt()
preempted = True
break
#we timed out
if (rospy.get_time() -
start_time) > SafeMoveArmStateSmach.TIME_OUT:
self.move_arm_client.cancel_goal()
rospy.loginfo('SafeMoveArmState: timed out!')
break
if (state not in [am.GoalStatus.ACTIVE, am.GoalStatus.PENDING]):
result = self.move_arm_client.get_result()
if state == am.GoalStatus.SUCCEEDED:
if result.error_code.val == 1:
rospy.loginfo('SafeMoveArmState: Succeeded!')
succeeded = True
elif result.error_code.val == an.ArmNavigationErrorCodes.START_STATE_IN_COLLISION:
return 'start_in_collision'
elif result.error_code.val == an.ArmNavigationErrorCodes.GOAL_IN_COLLISION:
return 'goal_in_collision'
elif result.error_code.val == an.ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED:
return 'joint_limit_violated'
rospy.loginfo('Got error code %d' % result.error_code.val)
break
state = self.move_arm_client.get_state()
r.sleep()
if preempted:
return 'preempted'
if succeeded:
return 'succeeded'
return 'failed'
def go_hovering(self, height):
self.command_pose[2] = height
rate = rospy.Rate(20)
start_time = rospy.Time.now()
while (self.hovering_error(self.command_pose, self.position) > self.position_error_threshold ) or ((rospy.Time.now() - start_time) < rospy.Duration(self.goal_wait_time)):
self.publish_command(self.command_pose)
rate.sleep()
return True
def wait(self, timeout=15.0):
self._client.wait_for_result(timeout=rospy.Duration(timeout))
if __name__ == '__main__':
global rb_num
global flag11
rospy.init_node('PatrolServer', anonymous=True)
print("start!!!!!!")
rospy.Subscriber("/test", Meter, meter, queue_size=1)
waypoint_store()
#patrol()
# start = 5
# cctv=[5,7,8,9]
# search(start,cctv)
#rospy.Subscriber("/trigger",Trigger,ALERT)
rospy.Subscriber("/find", PoseStamped, Receive_Point)
rospy.Timer(rospy.Duration(0.1), tic)
rospy.Timer(rospy.Duration(0.1), tic2)
rospy.Subscriber('/robot2/move_base/result', MoveBaseActionResult,
arriverobot2)
rospy.Subscriber('/robot3/move_base/result', MoveBaseActionResult,
arriverobot3)
rospy.Subscriber('/robot1/joint_states', JointState, angle)
rospy.Publisher("robot3/move_base_simple/goal", PoseStamped, queue_size=1)
goal_publisher = rospy.Publisher('robot2/move_base_simple/goal',
PoseStamped,
queue_size=1)
goal_publisher1 = rospy.Publisher('robot3/move_base_simple/goal',
PoseStamped,
queue_size=1)
rospy.Publisher('robot2/move_base/cancel', GoalID, queue_size=1)
for i in range(0, 9):
navsat.position_covariance = [2.5, 0, 0,
0, 2.5, 0,
0, 0, 2.5]
if not origin_lat and not origin_lon:
origin_lat = gpsd.fix.latitude
origin_lon = gpsd.fix.longitude
# print ('Odometry: ')
# print (x, y)
pub_navsat.publish(navsat)
if __name__ == '__main__':
rospy.init_node("gps_comm")
gpsp = GpsPoller() # create the thread
pub_navsat = rospy.Publisher("/gps", NavSatFix, queue_size=1)
timer = rospy.Timer(rospy.Duration(0.1), publish_gps)
try:
gpsp.start() # start it up
while not rospy.is_shutdown():
pass
except (KeyboardInterrupt, SystemExit): #when you press ctrl+c
print "\nKilling Thread..."
gpsp.running = False
gpsp.join() # wait for the thread to finish what it's doing
rospy.signal_shutdown()
os._exit()
print "Done.\nExiting."
def __init__(self):
rospy.init_node('turtlebot_mapping')
## Use simulation time (i.e. get time from rostopic /clock)
rospy.set_param('use_sim_time', 'true')
rate = rospy.Rate(10)
while rospy.Time.now() == rospy.Time(0):
rate.sleep()
## Get transformation of camera frame with respect to the base frame
self.tfBuffer = tf2_ros.Buffer()
self.tfListener = tf2_ros.TransformListener(self.tfBuffer)
while True:
try:
# notably camera_link and not camera_depth_frame below, not sure why
self.raw_base_to_camera = self.tfBuffer.lookup_transform(
"base_footprint", "camera_link", rospy.Time()).transform
break
except tf2_ros.LookupException:
rate.sleep()
rotation = self.raw_base_to_camera.rotation
translation = self.raw_base_to_camera.translation
tf_theta = get_yaw_from_quaternion(rotation)
self.base_to_camera = [translation.x, translation.y, tf_theta]
## Initial state for EKF
self.EKF = None
self.EKF_time = None
self.current_control = np.zeros(2)
self.latest_pose = None
self.latest_pose_time = None
self.controls = deque()
self.scans = deque()
N_map_lines = ArenaParams.shape[1]
self.x0_map = ArenaParams.T.flatten()
self.x0_map[4:] = self.x0_map[4:] + np.vstack((
NoiseParams["std_alpha"] *
np.random.randn(N_map_lines - 2), # first two lines fixed
NoiseParams["std_r"] *
np.random.randn(N_map_lines - 2))).T.flatten()
self.P0_map = np.diag(
np.concatenate(
(np.zeros(4),
np.array([[
NoiseParams["std_alpha"]**2
for i in range(N_map_lines - 2)
], [NoiseParams["std_r"]**2
for i in range(N_map_lines - 2)]]).T.flatten())))
## Set up publishers and subscribers
self.tfBroadcaster = tf.TransformBroadcaster()
rospy.Subscriber('/scan', LaserScan, self.scan_callback)
rospy.Subscriber('/cmd_vel_mux/input/teleop', Twist,
self.control_callback)
rospy.Subscriber('/gazebo/model_states', ModelStates,
self.state_callback)
self.ground_truth_ct = 0
self.ground_truth_map_pub = rospy.Publisher("ground_truth_map",
Marker,
queue_size=10)
self.ground_truth_map_marker = Marker()
self.ground_truth_map_marker.header.frame_id = "/world"
self.ground_truth_map_marker.header.stamp = rospy.Time.now()
self.ground_truth_map_marker.ns = "ground_truth"
self.ground_truth_map_marker.type = 5 # line list
self.ground_truth_map_marker.pose.orientation.w = 1.0
self.ground_truth_map_marker.scale.x = .01
self.ground_truth_map_marker.scale.y = .01
self.ground_truth_map_marker.scale.z = .01
self.ground_truth_map_marker.color.r = 0.0
self.ground_truth_map_marker.color.g = 1.0
self.ground_truth_map_marker.color.b = 0.0
self.ground_truth_map_marker.color.a = 1.0
self.ground_truth_map_marker.lifetime = rospy.Duration(1000)
self.ground_truth_map_marker.points = sum(
[[Point(p1[0], p1[1], 0),
Point(p2[0], p2[1], 0)] for p1, p2 in ARENA], [])
self.EKF_map_pub = rospy.Publisher("EKF_map", Marker, queue_size=10)
self.EKF_map_marker = deepcopy(self.ground_truth_map_marker)
self.EKF_map_marker.color.r = 1.0
self.EKF_map_marker.color.g = 0.0
self.EKF_map_marker.color.b = 0.0
self.EKF_map_marker.color.a = 1.0
class SmartShooter(sasc_scrimmage.Tactic):
MAX_TIME_LATE = rospy.Duration(10.0)
BDA_TIME_OUT = 3.0
RQD_IN_ENVELOPE = 1
def init(self, params):
self._id = int(params['id'])
self._target_id = None
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
try:
self._safe_waypoint = self._parent._safe_waypoint
except AttributeError:
self._safe_waypoint = np.array([0, 0, 0])
self._wp = self._safe_waypoint
try:
self._vehicle_type = int(params['vehicle_type'])
except KeyError:
self._vehicle_type = enums.AC_UNKNOWN
self._action = ["None", 0]
self._name = 'SmartShooter'
self.behavior_state = States.hunt_tgt
self.pursuit_status = False
self.shot_pending = False
self.overtime = False
self.reds_pursued = []
self.wp_calc = \
pputils.PNInterceptCalculator(
self, self._id, self._reds, max_time_late=self.MAX_TIME_LATE
)
try:
topic = 'network/send_swarm_behavior_data'
# we dont need to create a subscriber here because this is taken care of in
self.pubs[topic] = self._parent._behavior_data_publisher
except AttributeError:
self.pubs[topic] = \
self.createPublisher(topic, apmsg.BehaviorParameters, 1)
self.subs[topic] = \
self.createSubscriber(
topic, apmsg.BehaviorParameters, self._process_swarm_data_msg
)
def identify_target(self, t):
self._target_id = None
self_pos = self._own_pose.pose.pose.position
min_range = np.inf
for uav_id in self._reds:
red_veh_state = self._reds[uav_id]
red_pos = red_veh_state.state.pose.pose.position
shot = uav_id in self._shot
pursued = uav_id in self.reds_pursued
active_game = \
(red_veh_state.game_status == enums.GAME_ACTIVE_DEFENSE or
red_veh_state.game_status == enums.GAME_ACTIVE_OFFENSE)
recent_pose = \
(t - red_veh_state.state.header.stamp.to_sec() <
self.MAX_TIME_LATE.to_sec())
if not shot and not pursued and recent_pose and active_game:
d = gps.gps_distance(self_pos.lat, self_pos.lon, red_pos.lat,
red_pos.lon)
if d < min_range:
self._target_id = uav_id
self.target_dist = d
min_range = d
if self._target_id:
self.pursuit_status = True
self.wp_calc.set_params(self._target_id, 0.0, 0.0, self_pos.alt,
pputils.InterceptCalculator.BASE_ALT_MODE)
self.pursuit_status_update()
def track_target(self, t):
#intercept = self.wp_calc.compute_intercept_waypoint(None, t)
tgt_pose = self.wp_calc.leader_state()
if self._target_id in self._shot:
self.behavior_state = States.hunt_tgt
self.pursuit_status = False
self.pursuit_status_update()
elif self._id in self._shot:
self.behavior_state = States.no_tgts
self.pursuit_status = False
self.pursuit_status_update()
#if intercept:
# self._wp = np.array([intercept.lat, intercept.lon, intercept.alt])
#else:
# return False
self._wp = self.intercept_target(self._target_id)
return True
def pursuit_status_update(self):
if self.overtime:
return
parser = pursueBytes.PursuitMessageParser()
parser.friendly_id = self._id
if self._target_id != None:
parser.target_id = self._target_id
parser.target_distance = self.target_dist
else:
parser.target_id = 0
parser.target_distance = self._max_range
parser.pursuit_status = self.pursuit_status
data = parser.pack()
net_msg = apmsg.BehaviorParameters()
net_msg.id = pursueBytes.PURSUIT_MESSAGE
net_msg.params = data
for _ in range(1): # Hope to get at least one through
self.pubs['network/send_swarm_behavior_data'].publish(net_msg)
def intercept_target(self, target_id, alt_deconflict=True):
'''Returns a waypoint that will intercept the target.
If the vehicle is a fixed-wing, the waypoint will be extended beyond
the waypoint loiter distance
'''
own_pos = self._own_pose.pose.pose.position
target_pos = self._reds[target_id].state.pose.pose.position
own_lla = (own_pos.lat, own_pos.lon, own_pos.alt)
tgt_lla = (target_pos.lat, target_pos.lon, target_pos.alt)
lat = tgt_lla[0]
lon = tgt_lla[1]
if self._vehicle_type == enums.AC_FIXED_WING:
# When using fixed wing, Set the waypoint past the current
# target, so we don't go into a loiter mode
bearing = gps.gps_bearing(own_lla[0], own_lla[1], tgt_lla[0],
tgt_lla[1])
lat, lon = gps.gps_newpos(own_lla[0], own_lla[1], bearing, 1000)
wp = np.array([lat, lon, tgt_lla[2]])
# Handle altitude deconfliction
if alt_deconflict:
wp[2] = self._last_ap_wp[2]
return wp
def target_hitable(self, target_id):
sp = self._own_pose.pose.pose.position
so = self._own_pose.pose.pose.orientation
quat = (so.x, so.y, so.z, so.w)
if target_id in self._reds:
target_pos = self._reds[target_id].state.pose.pose.position
return gps.hitable(sp.lat, sp.lon, sp.alt, quat, self._max_range,
self._fov_width, self._fov_height,
target_pos.lat, target_pos.lon, target_pos.alt)
else:
return False
def step_autonomy(self, t, dt):
self.wp_calc._swarm = self._reds.copy()
self.wp_calc._swarm.update(self._blues)
self._wp = self._safe_waypoint
self._action = ["None", 0]
for uav_id in self._reds:
if uav_id not in self._shot and self.target_hitable(uav_id):
self._action = ["Fire", uav_id]
if self._target_id is not None and np.any(self._wp != 0):
self.wp_calc.set_params(self._target_id, 0.0, 0.0,
self._own_pose.pose.pose.position.alt,
pputils.InterceptCalculator.BASE_ALT_MODE)
if self.behavior_state == States.hunt_tgt:
self.identify_target(t)
self.shot_pending = False
self.behavior_state = \
States.track_tgt if self._target_id is not None else States.no_tgts
elif self.behavior_state == States.track_tgt:
intercept = self.track_target(t)
if intercept:
if self.target_hitable(self._target_id):
self.envelope_counter = 1
self.behavior_state = States.shoot_tgt
else:
self.behavior_state = States.hunt_tgt
elif self.behavior_state == States.shoot_tgt:
intercept = self.track_target(t)
if intercept:
if self._target_id in self._shot:
self.behavior_state = States.hunt_tgt
else:
self.behavior_state = States.track_tgt
else:
self.behavior_state = States.hunt_tgt
elif self.behavior_state == States.no_tgts:
if len(self.reds_pursued) > 0:
self.reds_pursued = []
self.overtime = True
self.behavior_state = States.hunt_tgt
else:
self._wp = self._safe_waypoint
self.behavior_state = States.terminal_standby
elif self.behavior_state == States.terminal_standby:
if self._id not in self._shot:
self._target_id = self.identify_target(t)
if self._target_id != None:
self.shot_pending = False
self.behavior_state = States.track_tgt
return True
def _process_swarm_data_msg(self, msg):
if msg.id == pursueBytes.PURSUIT_MESSAGE:
parser = pursueBytes.PursuitMessageParser()
parser.unpack(msg.params)
if parser.friendly_id in self._reds or parser.friendly_id == self._id:
# dont process items from the other team
return
if parser.pursuit_status == True:
if parser.target_id not in self.reds_pursued:
self.reds_pursued.append(parser.target_id)
if (parser.friendly_id != self._id
and parser.target_id == self._target_id):
if parser.target_distance < self.target_dist:
self.pursuit_status = False
self.pursuit_status_update()
self._target_id = None
self.behavior_state = States.hunt_tgt
else:
self.pursuit_status_update()
else:
if parser.target_id in self.reds_pursued:
self.reds_pursued.remove(parser.target_id)
elif blueprint[layer][brickIndex] == "G":
brickcubemarker.color.g = 1.0
brickcubemarker.scale.x = 0.6
elif blueprint[layer][brickIndex] == "R":
brickcubemarker.color.r = 1.0
brickcubemarker.scale.x = 0.3
brickcubemarker.scale.y = 0.2
brickcubemarker.scale.z = 0.2
if blueprint[layer][brickIndex] == "O":
brickcubemarker.color.r = 1.0
brickcubemarker.color.g = 0.5
brickcubemarker.scale.y = 1.8
brickcubemarker.scale.x = 0.2
brickcubemarker.scale.z = 0.2
brickcubemarker.lifetime = rospy.Duration(0)
marker_pub.publish(brickcubemarker)
# rospy.sleep(1.0)
thismarker.header.seq = 0
thismarker.header.frame_id = "map"
thismarker.header.stamp = rospy.get_rostime()
thismarker.ns = "carpose"
thismarker.id = idnum
idnum += 1
thismarker.pose.position.x = target_trans[0]
thismarker.pose.position.y = target_trans[1]
thismarker.pose.position.z = 0
thismarker.pose.orientation.x = target_rot[0]
thismarker.pose.orientation.y = target_rot[1]
def main():
rospy.init_node('source', anonymous=True)
pub_marker = rospy.Publisher('agents', MarkerArray, queue_size=1)
pub_marker_field = rospy.Publisher('field', MarkerArray, queue_size=1)
rate = rospy.Rate(10) # 10hz
no_of_agents = 4
Xstart = 30.0
Ystart = 30.0
agentx = Xstart + numpy.random.rand(1, no_of_agents)
agenty = Ystart + numpy.random.rand(1, no_of_agents)
#creatng gap between the agents from th estart only
# agentx[0][1] = agentx[0][1] + 5.0
# agenty[0][1] = agenty[0][1]
agents = numpy.concatenate((agentx, agenty), axis=0)
#acc to papaer
C = 0
k = 1
k1 = 1
center_of_source = [0, 0]
field_length = 100
field_width = 100
F = MarkerArray()
point_shape = Marker.CYLINDER
source_x = center_of_source[0]
source_y = center_of_source[1]
source = [source_x, source_y]
r_big_circle = 100.0
r_small_circle = 5.0
no_of_circles = 20
thickness_of_circle = (r_big_circle - r_small_circle) / no_of_circles
red_circles = no_of_circles / 2
blue_circles = no_of_circles / 2
for i1 in range(no_of_circles):
marker = Marker()
marker.header.frame_id = "/multi_sheep_sheperd"
marker.header.stamp = rospy.get_rostime()
marker.ns = "field"
marker.id = i1
marker.type = point_shape
marker.action = Marker.ADD
marker.color.a = 1.0
marker.pose.position.x = center_of_source[0]
marker.pose.position.y = center_of_source[1]
marker.pose.position.z = 0.0
marker.scale.x = 5 #(r_big_circle - thickness_of_circle*i1)*2
marker.scale.y = 5 #(r_big_circle - thickness_of_circle*i1)*2
marker.scale.z = 0.5
# if(i1 < red_circles):
# marker.color.r = i1*(1.0)/red_circles
# else:
# marker.color.r = 1.0
# marker.color.b = (i1-red_circles)*(1.0)/blue_circles
marker.color.b = 1.0
marker.lifetime = rospy.get_rostime()
F.markers.append(marker)
# for i1 in range(field_length):
# for j1 in range(field_width):
# if(j1%2 == 0):
# marker.header.frame_id = "/multi_sheep_sheperd";
# marker.header.stamp = rospy.get_rostime()
# marker.ns = "field"
# marker.id = i1;
# marker.type = point_shape
# marker.action = Marker.ADD;
# marker.color.a = 1.0
# marker.pose.position.x = field_width - j1*level
# marker.pose.position.y = field_length - i1*level
# marker.pose.position.z = 0.0
# marker.scale.x =
M = MarkerArray()
shape = Marker.CUBE
for i1 in range(no_of_agents):
marker = Marker()
marker.header.frame_id = "/multi_sheep_sheperd"
marker.header.stamp = rospy.get_rostime()
marker.ns = "agents"
marker.id = i1
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.color.a = 1.0
if (i1 < no_of_agents):
marker.pose.position.x = agents[0][i1]
marker.pose.position.y = agents[1][i1]
marker.pose.position.z = 0
marker.scale.x = 3.0 / 2
marker.scale.y = 3.0 / 2
marker.scale.z = 1.0
marker.color.g = 1.0 #*(1-i1)
#marker.color.r = i1*0.5
marker.lifetime = rospy.Duration()
M.markers.append(marker)
#expected distance
expected_inter_agent_dist = numpy.zeros([no_of_agents, no_of_agents]) #1xN
#expected_inter_agent_dist[:no_of_agents/2][0] = 5.0
#expected_inter_agent_dist[0][1] = -5.0
dist = 5.0 / 2
for i in range(no_of_agents):
for j in range(i + 1, no_of_agents):
expected_inter_agent_dist[i][j] = dist
for j in range(i - 1, 0, -1):
expected_inter_agent_dist[i][j] = -dist
q = numpy.zeros([no_of_agents, 2])
n = numpy.zeros([no_of_agents, 2])
s = numpy.ones([
1, no_of_agents
]) #assuming all are same direction, we dont care of the diagnol
vq = numpy.zeros(no_of_agents)
vn = numpy.zeros(no_of_agents)
Vx = numpy.zeros(no_of_agents)
Vy = numpy.zeros(no_of_agents)
ini_s = numpy.ones([1, no_of_agents])
theta = numpy.zeros(no_of_agents) #angle with pos x
alpha = numpy.zeros(no_of_agents) #alpha in the paper
alpha_dot = numpy.zeros(no_of_agents)
#for source seeking, si = -sj, hence making s[0][1] = -1 as s[0][0] = 1
if (no_of_agents == 2):
s[0][1] = -s[0][0] #made them same for testing circular movement
ini_s[0][1] = -ini_s[0][0]
time_step = 0.05
delta1 = pow(10, -4)
delta2 = 1.0
count_of_algo_fail = 0
time_gap = numpy.zeros(
[1, no_of_agents]
) #this represents after how much time we take the next conc. reading to compare and make decision
time_after_conc_compared = 1
radius = 7.5
#time_gap[0][0] = 5
#time_gap[0][1] = 5
start_time = rospy.get_rostime().to_sec()
while not rospy.is_shutdown():
pub_marker_field.publish(F)
for i in range(no_of_agents):
M.markers[i].pose.position.x = agents[0][i]
M.markers[i].pose.position.y = agents[1][i]
#M.markers[i].scale.x = 10*get_conc(agents[:,i], source, thickness_of_circle, r_small_circle, r_big_circle, no_of_circles)
#M.markers[i].scale.y = 10*get_conc(agents[:,i], source, thickness_of_circle, r_small_circle, r_big_circle, no_of_circles)
pub_marker.publish(M)
#print("agents position : " , agents)
for i in range(no_of_agents):
print("i: ", i, " s_i: ", s[0][i], " % ",
numpy.arctan2(-1.0, -1.0))
ini_s[0][0] = s[0][0]
ini_s[0][1] = s[0][1]
# if(no_of_agents == 2):
# q[i] = get_q(no_of_agents,agents,i,s)
# else:
q[i] = get_q_NAgents(no_of_agents, agents, i, radius, q[i], s)
#for j in range(no_of_agents - 1):
# temp_q = get_q(i,j,a); #will give the axis q
#we can also put the inner loop in the function get_q as later on consensus is used for finding q
#q[i] = temp_q # this 2x1, has x and y both
n[i] = get_n(q[i])
#n[i] = temp_n , #do we even need n ??
theta[i] = get_theta(q[i]) # angle with x axis
conc_t = get_conc(agents[:, i], source, thickness_of_circle,
r_small_circle, r_big_circle, no_of_circles,
center_of_source)
vq[i] = get_vq(
no_of_agents, agents, expected_inter_agent_dist, i, k1, q[i], s
) #k1*(numpy.dot([a[0][j] - a[0][i] , a[1][j] - a[1][i]],[q[i]]) - a_expected[i,j])
vn[i] = get_vn(i, k, C, conc_t, agents[:, i]) # y = 0
print("q: ", q[i])
print("n: ", n[i])
print("Vn: ", vn[i])
print("Vq: ", vq[i])
print("theta: ", theta[i] / pi * 180)
Vx[i] = get_Vx(
vq[i], vn[i], theta[i]
) #q[i][0]*numpy.cos(temp_theta) - n[i][0]*numpy.sin(temp_theta)
Vy[i] = get_Vy(
vq[i], vn[i], theta[i]
) #q[i][0]*numpy.sin(temp_theta) + n[i][0]*numpy.cos(temp_theta)
print("Vxi: ", Vx[i])
print("Vyi: ", Vy[i])
# vq[i] = vqi
# vn[i] = vni
# Vx[i] = Vxi
# Vy[i] = Vyi
#print("vqi : " , vqi , " vni : " , vni )
#print("Vxi : " , Vxi , " Vyi : " , Vyi)
#CHANGE IN POSITION:
agents[0][i] = agents[0][i] + Vx[i] * time_step
agents[1][i] = agents[1][i] + Vy[i] * time_step
time_gap[0][i] = time_gap[0][i] + 1
#swaitching the directions according to conc:
change = get_conc(agents[:, i], source, thickness_of_circle,
r_small_circle, r_big_circle, no_of_circles,
center_of_source) - conc_t
print("change :", change)
if (change > 0 and time_gap[0][i] % 2 == 0):
#meaning that agent moving towards high conc, we change the s
#if(s[0][0] != s[0][1]):
print("changing the direction:")
print("initial s: ", s)
s[0][i] = -1 * (s[0][i])
ini_s = s
print("new s: ", s)
#Switching Conditions
print("time_gap : ", time_gap[0][i])
if (time_gap[0][i] % time_after_conc_compared == 0):
#below condition implies, forward movment
if (s[0][1] == -s[0][0]
): #as only two agents hence we can do this
conc_t_1 = get_conc(agents[:,
i], source, thickness_of_circle,
r_small_circle, r_big_circle,
no_of_circles, center_of_source)
if (abs(conc_t_1 - conc_t) <= delta1):
s[0][i] = s[0][i] * (-1)
else:
conc_t_1 = get_conc(agents[:,
i], source, thickness_of_circle,
r_small_circle, r_big_circle,
no_of_circles, center_of_source)
if (abs(conc_t_1 - conc_t) > delta2):
s[0][i] = s[0][i] * (-1)
print("change in conc: ", abs(conc_t_1 - conc_t))
if (s[0][0] != ini_s[0][0] and s[0][1] != ini_s[0][1]
): #algo fails when both agents change sign together
count_of_algo_fail = count_of_algo_fail + 1
print("count_of_algo_fail: ", count_of_algo_fail)
agent_in = 0
threshold = 2.0 / 2
for i in range(no_of_agents):
if (numpy.linalg.norm(agents[:, i] - center_of_source) <
threshold):
agent_in += 1
if (agent_in >= floor(0.9 * no_of_agents)):
print("STOPPPPPP")
break
rate.sleep()
stop_time = rospy.get_rostime().to_sec()
print("total time taken: ", stop_time - start_time)
def send_move_command(self):
#We want orientation to always be the same
bMove = False
#stopped
if self.bstop:
rospy.sleep(1)
bMove = False
#move to basket
elif self.btoBasket:
position = self.moveMethod[1](self)
bMove = True
#move to last cut location
elif self.btoCut:
position = self.moveMethod[2](self)
bMove = True
#move by camera
elif self.bbyCamera:
position = self.moveMethod[3](self)
bMove = True
#move by scan
elif self.bbyScan:
position = self.moveMethod[4](self)
bMove = True
if bMove:
print position
if self.bgripper:
#make new message
goal = kinova_msgs.msg.SetFingersPositionGoal()
goal.fingers.finger1 = self.finger_turn
goal.fingers.finger2 = goal.fingers.finger1
goal.fingers.finger3 = goal.fingers.finger1
print goal
#send through client
self.fingerClient.send_goal(goal)
print "Attention: moving the gripper"
if self.fingerClient.wait_for_result(rospy.Duration(5.0)):
print 'Success'
else:
self.fingerClient.cancel_all_goals()
print ' Error: the cartesian action timed-out'
self.bgripper = False
if self.finger_turn == finger_minTurn:
#if we just opened fingers return to last cut location
self.btoBasket = False
self.btoCut = True
self.finger_turn = finger_maxTurn
else:
#if we just closed to fngers move to basket
self.finger_turn = finger_minTurn
self.btoBasket = True
else:
#basket has a different orientation
orientation = EulerXYZ2Quaternion(self.currentRPY)
if self.btoBasket:
orientation = EulerXYZ2Quaternion(self.basketRPY)
#send with Moveit using location gotten above to move to the new location
goal = geometry_msgs.msg.Pose()
goal.position = geometry_msgs.msg.Point(x=position[0],
y=position[1],
z=position[2])
goal.orientation = geometry_msgs.msg.Quaternion(
x=orientation[0],
y=orientation[1],
z=orientation[2],
w=orientation[3])
# Moveit
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander("arm")
planning_scene_interface = moveit_commander.PlanningSceneInterface(
)
# Planning to a Pose goal
group.set_pose_target(goal)
# Now, we call the planner to compute the plan and visualize it.
group.plan()
# we should add something hear to handle failures
# move the robot
print "Attention: moving the arm"
group.go()
def talker():
rospy.init_node('kitti_tf_pub', anonymous=True)
listener = tf.TransformListener()
marker_pub_ = rospy.Publisher('viz_msgs_marker_publisher',
vis_msg.Marker,
latch=True,
queue_size=10)
pub = rospy.Publisher('kitti/transformStamped',
TransformStamped,
queue_size=10)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
try:
(trans, rot) = listener.lookupTransform('/world', '/camera_left',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
xform = TransformStamped()
now = rospy.Time.now()
xform.header = Header()
xform.header.frame_id = '/world'
xform.header.stamp = now
xform.child_frame_id = '/camera_left'
xform.transform.translation.x = trans[2]
xform.transform.translation.y = trans[0]
xform.transform.translation.z = trans[1]
xform.transform.rotation.x = rot[0]
xform.transform.rotation.y = rot[1]
xform.transform.rotation.z = rot[2]
xform.transform.rotation.w = rot[3]
# hello_str = "hello world %s" % rospy.get_time()
# rospy.loginfo(hello_str)
pub.publish(xform)
marker = vis_msg.Marker(type=vis_msg.Marker.SPHERE,
ns='xyz',
action=vis_msg.Marker.ADD,
id=0)
marker.header.frame_id = '/world'
marker.header.stamp = now
marker.scale.x = 1000
marker.scale.y = 1000
marker.scale.z = 100
marker.colors = [ColorRGBA(1.0, 1.0, 1.0, 1.0)]
# XYZ
marker.pose.position = xform.transform.translation
marker.pose.orientation = xform.transform.rotation
marker.lifetime = rospy.Duration()
marker_pub_.publish(marker)
rate.sleep()
def test_locations(self):
#get current location
topic_address = '/' + prefix + '_driver/out/cartesian_command'
rospy.Subscriber(topic_address, kinova_msgs.msg.KinovaPose,
self.setcurrentCartesianCommand)
rospy.wait_for_message(topic_address, kinova_msgs.msg.KinovaPose)
print 'position listener obtained message for Cartesian pose. '
#move 10cm +z
action_address = '/' + prefix + '_driver/pose_action/tool_pose'
client = actionlib.SimpleActionClient(action_address,
kinova_msgs.msg.ArmPoseAction)
client.wait_for_server()
print 'move 10cm +z'
goal = kinova_msgs.msg.ArmPoseGoal()
goal.pose.header = std_msgs.msg.Header(frame_id=(prefix +
'_link_base'))
#goal.pose.pose.position = geometry_msgs.msg.Point(
# x=self.currentLocation[0], y=self.currentLocation[1], z=(self.currentLocation[2]))
goal.pose.pose.position = geometry_msgs.msg.Point(x=0.3, y=0.3, z=0.4)
goal.pose.pose.orientation = geometry_msgs.msg.Quaternion(x=0,
y=0,
z=0,
w=1.0)
client.send_goal(goal)
if client.wait_for_result(rospy.Duration(20.0)):
print 'Success'
else:
client.cancel_all_goals()
print ' Error: the cartesian action timed-out'
#move to position A
goal = kinova_msgs.msg.ArmPoseGoal()
goal.pose.header = std_msgs.msg.Header(frame_id=(prefix +
'_link_base'))
goal.pose.pose.position = geometry_msgs.msg.Point(x=0.2, y=-0.1, z=0.3)
goal.pose.pose.orientation = geometry_msgs.msg.Quaternion(x=0,
y=0,
z=0,
w=1.0)
client.send_goal(goal)
if client.wait_for_result(rospy.Duration(20.0)):
print 'Success'
else:
client.cancel_all_goals()
print ' Error: the cartesian action timed-out'
#move back
goal = kinova_msgs.msg.ArmPoseGoal()
goal.pose.header = std_msgs.msg.Header(frame_id=(prefix +
'_link_base'))
goal.pose.pose.position = geometry_msgs.msg.Point(x=0.3, y=0.3, z=0.4)
goal.pose.pose.orientation = geometry_msgs.msg.Quaternion(x=0,
y=0,
z=0,
w=1.0)
client.send_goal(goal)
if client.wait_for_result(rospy.Duration(20.0)):
print 'Success'
else:
client.cancel_all_goals()
print ' Error: the cartesian action timed-out'
def __init__(self):
self.server = rospy.resolve_name('~server')
self.config_file = rospy.get_param('~config_file')
if not osp.exists(self.config_file):
rospy.logfatal("config_file '{}' does not exist".format(
self.config_file))
sys.exit(1)
self.config = yaml.load(open(self.config_file))
self.req_marker = rospy.ServiceProxy(
osp.join(self.server, 'request_marker_operate'),
RequestMarkerOperate)
self.req_color = rospy.ServiceProxy(osp.join(self.server, 'set_color'),
SetTransformableMarkerColor)
self.req_pose = rospy.ServiceProxy(osp.join(self.server, 'set_pose'),
SetTransformableMarkerPose)
self.req_dim = rospy.ServiceProxy(
osp.join(self.server, 'set_dimensions'), SetMarkerDimensions)
self.req_focus = rospy.ServiceProxy(osp.join(self.server, 'get_focus'),
GetTransformableMarkerFocus)
rospy.wait_for_service(self.req_marker.resolved_name)
rospy.wait_for_service(self.req_color.resolved_name)
rospy.wait_for_service(self.req_pose.resolved_name)
rospy.wait_for_service(self.req_dim.resolved_name)
rospy.wait_for_service(self.req_focus.resolved_name)
self.object_poses = {}
self.object_dimensions = {}
# TODO: support other than box: ex. torus, cylinder, mesh
# insert box
boxes = self.config['boxes']
n_boxes = len(boxes)
cmap = labelcolormap(n_boxes)
for i in xrange(n_boxes):
box = boxes[i]
name = box['name']
frame_id = box.get('frame_id', '/map')
self.insert_marker(name,
frame_id,
type=TransformableMarkerOperate.BOX)
self.set_color(name, (cmap[i][0], cmap[i][1], cmap[i][2], 0.5))
dim = box.get('dimensions', [1, 1, 1])
self.set_dimensions(name, dim)
pos = box.get('position', [0, 0, 0])
ori = box.get('orientation', [0, 0, 0, 1])
self.set_pose(box['name'], box['frame_id'], pos, ori)
self.object_poses[box['name']] = Pose(
position=Vector3(*pos),
orientation=Quaternion(*ori),
)
self.object_dimensions[box['name']] = Vector3(*dim)
rospy.loginfo("Inserted transformable box '{}'.".format(name))
self.sub_dimensions = rospy.Subscriber(
osp.join(self.server, 'marker_dimensions'), MarkerDimensions,
self._dimensions_change_callback)
self.sub_pose = rospy.Subscriber(
osp.join(self.server, 'pose_with_name'), PoseStampedWithName,
self._pose_change_callback)
do_auto_save = rospy.get_param('~config_auto_save', True)
if do_auto_save:
self.timer_save = rospy.Timer(rospy.Duration(1),
self._save_callback)
self.pub_bboxes = rospy.Publisher('~output/boxes',
BoundingBoxArray,
queue_size=1)
self.timer_pub_bboxes = rospy.Timer(rospy.Duration(0.1),
self._pub_bboxes_callback)
def __init__(self, context):
super(APCPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('APCPlugin')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QMainWindow()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_apc'),
'resource', 'apc.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('APCPlugin')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Add function calls for each of the buttons.
self._widget.button_component_general.clicked.connect(
self.button_component_general_clicked)
self._widget.button_component_0.clicked.connect(
self.button_component_0_clicked)
self._widget.button_component_1.clicked.connect(
self.button_component_1_clicked)
self._widget.button_component_2.clicked.connect(
self.button_component_2_clicked)
self._widget.button_calibration_0.clicked.connect(
self.button_calibration_0_clicked)
self._widget.button_calibration_1.clicked.connect(
self.button_calibration_1_clicked)
self._widget.button_calibration_2.clicked.connect(
self.button_calibration_2_clicked)
self._widget.toggle_arm_button.clicked[bool].connect(
self.toggle_arm_button_clicked)
self._widget.home_button.clicked[bool].connect(
self.home_button_clicked)
self._widget.bin_a_button.clicked[bool].connect(
self.bin_a_button_clicked)
self._widget.bin_b_button.clicked[bool].connect(
self.bin_b_button_clicked)
self._widget.bin_c_button.clicked[bool].connect(
self.bin_c_button_clicked)
self._widget.bin_d_button.clicked[bool].connect(
self.bin_d_button_clicked)
self._widget.bin_e_button.clicked[bool].connect(
self.bin_e_button_clicked)
self._widget.bin_f_button.clicked[bool].connect(
self.bin_f_button_clicked)
self._widget.bin_g_button.clicked[bool].connect(
self.bin_g_button_clicked)
self._widget.bin_h_button.clicked[bool].connect(
self.bin_h_button_clicked)
self._widget.bin_i_button.clicked[bool].connect(
self.bin_i_button_clicked)
self._widget.bin_j_button.clicked[bool].connect(
self.bin_j_button_clicked)
self._widget.bin_k_button.clicked[bool].connect(
self.bin_k_button_clicked)
self._widget.bin_l_button.clicked[bool].connect(
self.bin_l_button_clicked)
self._widget.move_to_bin_button.clicked[bool].connect(
self.move_to_bin_button_clicked)
self._widget.left_gripper_button.clicked[bool].connect(
self.left_gripper_button_clicked)
self._widget.right_gripper_button.clicked[bool].connect(
self.right_gripper_button_clicked)
self._widget.left_suction_off_button.clicked[bool].connect(
self.left_suction_off_button_clicked)
self._widget.right_suction_off_button.clicked[bool].connect(
self.right_suction_off_button_clicked)
self._widget.start_bt_button.clicked[bool].connect(
self.start_bt_button_clicked)
self._widget.pause_bt_button.clicked[bool].connect(
self.pause_bt_button_clicked)
self._widget.click_to_pick_button.clicked[bool].connect(
self.click_to_pick_button_clicked)
self._widget.place_button.clicked[bool].connect(
self.place_button_clicked)
#Set icons to buttons
self._widget.button_component_general.setIcon(self._icon_node_start)
self._widget.button_component_diagn.setIcon(self._icon_node_start)
self._widget.button_component_0.setIcon(self._icon_node_start)
self._widget.button_component_1.setIcon(self._icon_node_start)
self._widget.button_component_2.setIcon(self._icon_node_start)
self._widget.button_calibration_0.setIcon(self._icon_node_start)
self._widget.button_calibration_1.setIcon(self._icon_node_start)
self._widget.button_calibration_2.setIcon(self._icon_node_start)
self._json_data = None
# for storing the joint states
self._left_joint_state = [0.0 for x in xrange(7)]
self._right_joint_state = [0.0 for x in xrange(7)]
# for selecting the arm
self._arm = 'left_arm'
# subscribers
self._joint_state_sub = rospy.Subscriber('/robot/joint_states',
JointState,
self.joint_state_cb)
self._tf_listener = tf.TransformListener()
self._move_arm_action_server_name = '/apc/manipulation/move_arm'
self._move_arm_client = actionlib.SimpleActionClient(
self._move_arm_action_server_name, MoveArmAction)
self._pick_object_action_server_name = '/apc/manipulation/pick_object'
self._pick_object_client = actionlib.SimpleActionClient(
self._pick_object_action_server_name, PickObjectAction)
self._place_object_action_server_name = '/apc/manipulation/place_object'
self._place_object_client = actionlib.SimpleActionClient(
self._place_object_action_server_name, PlaceObjectAction)
# baxter grippers
self._grippers_initialized = False
self._timer = rospy.Timer(rospy.Duration(1.0), self.timer_cb)
# A simple action client calling the pose_manager
#
# Author: Armin Hornung, University of Freiburg
import rospy
import sys
import actionlib
from nao_msgs.msg import BodyPoseAction, BodyPoseGoal
if __name__ == '__main__':
if len(sys.argv) != 2:
sys.exit('\nUSAGE: %s pose_name\n\n' % sys.argv[0])
rospy.init_node('execute_pose')
poseClient = actionlib.SimpleActionClient("body_pose", BodyPoseAction)
if not poseClient.wait_for_server(rospy.Duration(3.0)):
rospy.logfatal("Could not connect to required \"body_pose\" action server, is the pose_manager node running?");
rospy.signal_shutdown();
goal = BodyPoseGoal
goal.pose_name = str(sys.argv[1])
rospy.loginfo("Calling pose_manager for pose %s...", goal.pose_name)
poseClient.send_goal_and_wait(goal, rospy.Duration(5.0))
#TODO: check for errors
exit(0)
def add_point(self, positions, time):
point = JointTrajectoryPoint()
point.positions = copy(positions)
point.velocities = [0.0] * len(self._goal.trajectory.joint_names)
point.time_from_start = rospy.Duration(time)
self._goal.trajectory.points.append(point)
def get_3d_feature(self, y1_bboxes, pointcloud_upper, pointcloud_lower):
start = time.time()
#rospy.loginfo('Processing Pointcloud with FPointNet')
# Assumed that pointclouds have 64 bit floats!
pc_upper = ros_numpy.numpify(pointcloud_upper).astype({'names':['x','y','z','intensity','ring'], 'formats':['f4','f4','f4','f4','f4'], 'offsets':[0,4,8,16,20], 'itemsize':32})
pc_lower = ros_numpy.numpify(pointcloud_lower).astype({'names':['x','y','z','intensity','ring'], 'formats':['f4','f4','f4','f4','f4'], 'offsets':[0,4,8,16,20], 'itemsize':32})
pc_upper = torch.from_numpy(pc_upper.view(np.float32).reshape(pc_upper.shape + (-1,)))[:, [0,1,2,4]]
pc_lower = torch.from_numpy(pc_lower.view(np.float32).reshape(pc_lower.shape + (-1,)))[:, [0,1,2,4]]
# move onto gpu if available
try:
pc_upper = pc_upper.cuda()
pc_lower = pc_lower.cuda()
except:
pass
# translate and rotate into camera frame using calib object
# in message pointcloud has x pointing forward, y pointing to the left and z pointing upward
# need to transform this such that x is pointing to the right, y pointing downwards, z pointing forward
# also done inside calib
pc_upper = self.calib.move_lidar_to_camera_frame(pc_upper, upper=True)
pc_lower = self.calib.move_lidar_to_camera_frame(pc_lower, upper=False)
pc = torch.cat([pc_upper, pc_lower], dim = 0)
pc[:, 3] = 1
# pc = pc.cpu().numpy()
# self.publish_pointcloud_from_array(pc, self.pc_transform_pub, header = pointcloud_upper.header)
# idx = torch.randperm(pc.shape[0]).cuda()
# pc = pc[idx]
detections_2d = []
frame_det_ids = []
count = 0
for y1_bbox in y1_bboxes.bounding_boxes:
if y1_bbox.Class == 'person':
xmin = y1_bbox.xmin
xmax = y1_bbox.xmax
ymin = y1_bbox.ymin
ymax = y1_bbox.ymax
probability = y1_bbox.probability
frame_det_ids.append(count)
count += 1
detections_2d.append([xmin, ymin, xmax, ymax, probability, -1, -1])
features_3d = detection3d_with_feature_array()
features_3d.header.stamp = y1_bboxes.header.stamp
features_3d.header.frame_id = 'occam'
boxes_3d_markers = MarkerArray()
if not detections_2d:
self.marker_box_pub.publish(boxes_3d_markers)
self.feature_3d_pub.publish(features_3d)
return
boxes_3d, valid_3d, rot_angles, _, depth_features, frustums = \
generate_detections_3d(self.depth_model, detections_2d, pc,
self.calib, (3, 480, 3760), omni=True,
peds=True)
depth_features = convert_depth_features(depth_features, valid_3d)
for box, feature, i in zip(boxes_3d, depth_features, frame_det_ids):
#frustum = frustums[i]
#frustum[:, [0,2]] = np.squeeze(np.matmul(
# np.array([[np.cos(rot_angles[i]), np.sin(rot_angles[i])],
# [-np.sin(rot_angles[i]), np.cos(rot_angles[i])]]),
# np.expand_dims(frustum[:, [0,2]], 2)), 2)
# frustum[:, 3] = np.amax(logits[i], axis = 1)
#self.publish_pointcloud_from_array(frustum, self.pc_pub, header = pointcloud_upper.header)
det_msg = detection3d_with_feature()
det_msg.header.frame_id = 'occam'
det_msg.header.stamp = features_3d.header.stamp
det_msg.valid = True if valid_3d[i] != -1 else False
det_msg.frame_det_id = i
if det_msg.valid:
det_msg.x = box[0]
det_msg.y = box[1]
det_msg.z = box[2]
det_msg.l = box[3]
det_msg.h = box[4]
det_msg.w = box[5]
det_msg.theta = box[6]
det_msg.feature = feature
features_3d.detection3d_with_features.append(det_msg)
pose_msg = Pose()
marker_msg = Marker()
marker_msg.header.stamp = pointcloud_lower.header.stamp
marker_msg.header.frame_id = 'occam'
marker_msg.action = 0
marker_msg.id = i
marker_msg.lifetime = rospy.Duration(0.2)
marker_msg.type = 1
marker_msg.scale = Vector3(box[3], box[4], box[5])
pose_msg.position.x = det_msg.x
pose_msg.position.y = det_msg.y - det_msg.h/2
pose_msg.position.z = det_msg.z
marker_msg.pose = pose_msg
marker_msg.color = ColorRGBA(g=1, a =0.5)
boxes_3d_markers.markers.append(marker_msg)
else:
det_msg.y = -1
det_msg.x = -1
det_msg.z = -1
det_msg.l = -1
det_msg.w = -1
det_msg.h = -1
det_msg.theta = -1
det_msg.feature = [-1]
features_3d.detection3d_with_features.append(det_msg)
self.marker_box_pub.publish(boxes_3d_markers)
self.feature_3d_pub.publish(features_3d)
def wait_steady(self, time):
start_time = rospy.Time.now()
while ((rospy.Time.now() - start_time) < rospy.Duration(time)):
self.publish_command(self.command_pose)
