def _cloud_cb(self, cloud):
points = np.array(list(read_points(cloud, skip_nans=True)))
if points.shape[0] == 0:
return
# Get 4x4 matrix which transforms point cloud co-ords to odometry frame
try:
points_to_map = self.tf.asMatrix('/odom', cloud.header)
except tf.ExtrapolationException:
return
transformed_points = points_to_map.dot(np.vstack((points.T, np.ones((1, points.shape[0])))))
transformed_points = transformed_points[:3,:].T
if self.fused is None:
self.fused = transformed_points
else:
self.fused = np.vstack((self.fused, transformed_points))
self.seq += 1
header = Header()
header.seq = self.seq
header.stamp = rospy.Time.now()
header.frame_id = '/odom'
output_cloud = create_cloud(header, cloud.fields, self.fused)
rospy.loginfo('Publishing new cloud')
self.cloud_pub.publish(output_cloud)
def talker():
#Initial Pose Set Up
pub_init = rospy.Publisher('initialpose', PoseWithCovarianceStamped)
rospy.init_node('Initial_Pose_Publish')
initial=PoseWithCovarianceStamped()
initial_pose=PoseWithCovariance()
initial_pose.pose=Pose(Point(0.100,0.100,0.100),Quaternion(0.000,0.000,-0.0149,0.999))
initial_pose.covariance=\
[1.0,0.0,0.0,0.0,0.0,0.0,\
0.0,1.0,0.0,0.0,0.0,0.0,\
0.0,0.0,1.0,0.0,0.0,0.0,\
0.0,0.0,0.0,1.0,0.0,0.0,\
0.0,0.0,0.0,0.0,1.0,0.0,\
0.0,0.0,0.0,0.0,0.0,1.0]
initial.pose=initial_pose
initial_info=Header()
initial_info.frame_id="map"
initial_info.stamp=rospy.Time.now()
initial.header=initial_info
pub_init.publish(initial)
r=rospy.Rate(1)
ans=str("y")
#publishing the goal
while not rospy.is_shutdown():
rospy.loginfo("Setting Initial Pose")
pub_init.publish(initial)
ans=raw_input("Enter to continue")
rospy.loginfo("Setting Goal")
def __init__(self):
#Setup subscriber for combined searched image
frontierMarkerSub = rospy.Subscriber('frontierMarkers',Marker,self.markerCallback,queue_size = 1)
#Get Number of Robots
self.numRobots = rospy.get_param('/num_robots')
#Initialize Goal Publishers
self.goalPub = []
for i in range(0, self.numRobots):
#Setup Subscribers to individual robot searched grids
topicName = 'robot_' + `i` +'/move_base_simple/goal'
self.goalPub.append(rospy.Publisher(topicName,PoseStamped))
#Setup start/stop time publishers
self.startPub = rospy.Publisher('startTime', Header)
self.stopPub = rospy.Publisher('stopTime', Header)
#Get simulation start time
rospy.sleep(0.01) #Without this line, get_rostime always returns 0
self.start_time = rospy.get_rostime()
rospy.loginfo("SIMULATION STARTED AT: %i %i", self.start_time.secs, self.start_time.nsecs)
#Publish Start TIme
startMsg = Header()
startMsg.stamp = self.start_time
startMsg.frame_id = '0'
self.startPub.publish(startMsg)
#Wait for subscribed messages to start arriving
rospy.spin()
def create_move_group_pose_goal(self, goal_pose=Pose(), group="manipulator", end_link_name=None, plan_only=True):
header = Header()
header.frame_id = 'torso_base_link'
header.stamp = rospy.Time.now()
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = header
if end_link_name != None:
position_c.link_name = end_link_name
position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 1.0
goal_c.position_constraints.append(position_c)
orientation_c = OrientationConstraint()
orientation_c.header = header
if end_link_name != None:
orientation_c.link_name = end_link_name
orientation_c.orientation = goal_pose.orientation
orientation_c.absolute_x_axis_tolerance = 0.01
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 1.0
goal_c.orientation_constraints.append(orientation_c)
moveit_goal.request.goal_constraints.append(goal_c)
moveit_goal.request.num_planning_attempts = 5
moveit_goal.request.allowed_planning_time = 5.0
moveit_goal.planning_options.plan_only = plan_only
moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary
moveit_goal.request.group_name = group
return moveit_goal
def createRandomGrasps(grasp_pose, num_grasps=1):
""" Returns a list of num_grasps Grasp's around grasp_pose """
list_grasps = []
for grasp_num in range(num_grasps):
my_pre_grasp_approach = createGripperTranslation(Vector3(1.0, 0.0, 0.0)) # rotate over here?
my_post_grasp_retreat = createGripperTranslation(Vector3(0.0, 0.0, 1.0))
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
grasp_pose_copy = copy.deepcopy(grasp_pose)
modified_grasp_pose = PoseStamped(header, grasp_pose_copy)
#modified_grasp_pose.pose.position.x -= random.random() * 0.25 # randomize entrance to grasp in 25cm in x
#modified_grasp_pose.pose.position.y -= random.random() * 0.10 # randomize entrance to grasp in 10cm in y
#modified_grasp_pose.pose.orientation.z = -1.0
grasp = createGrasp(modified_grasp_pose, # change grasp pose?
allowed_touch_objects=["part"],
pre_grasp_posture=getPreGraspPosture(),
grasp_posture=getPreGraspPosture(),
pre_grasp_approach=my_pre_grasp_approach,
post_grasp_retreat=my_post_grasp_retreat,
id_grasp="random_grasp_" + str(grasp_num)
)
list_grasps.append(grasp)
return list_grasps
def talker(): header = Header() header.stamp = rospy.Time.now() header.frame_id = 'map' g_pcloud = pc2.create_cloud_xyz32(header, wmap) g_pub_map.publish(g_pcloud) rospy.spin()
def HeaderGenerater(self): self.seq += 1 header = Header() header.seq = self.seq header.stamp = rospy.Time.now() header.frame_id = self.GoalFrame return header
def publish_data(self):
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
h.seq = self.seq
self.seq = self.seq + 1
self.imu_msg.header = h
q = self.device.read_quaternion()
self.imu_msg.orientation.x = q[0]
self.imu_msg.orientation.y = q[1]
self.imu_msg.orientation.z = q[2]
self.imu_msg.orientation.w = q[3]
g = self.device.read_gyroscope()
# convert from deg/sec to rad/sec
self.imu_msg.angular_velocity.x = g[0] * math.pi / 180.0
self.imu_msg.angular_velocity.y = g[1] * math.pi / 180.0
self.imu_msg.angular_velocity.z = g[2] * math.pi / 180.0
a = self.device.read_linear_acceleration()
self.imu_msg.linear_acceleration.x = a[0]
self.imu_msg.linear_acceleration.y = a[1]
self.imu_msg.linear_acceleration.z = a[2]
self.imu_pub.publish(self.imu_msg)
self.temp_msg.header = h
self.temp_msg.temperature = self.device.read_temp()
self.temp_pub.publish(self.temp_msg)
def main():
rospy.init_node("pose2d_to_odometry")
own_num = rospy.get_param('~own_num', 0)
global odom, head
head = Header()
head.stamp = rospy.Time.now()
head.frame_id = "robot_{}/odom_combined".format(own_num)
odom = Odometry()
odom.child_frame_id = "robot_{}/base_footprint".format(own_num)
o = 10**-9 # zero
odom.pose.covariance = [1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0,
0, 0, o, 0, 0, 0, # z doesn't change
0, 0, 0, o, 0, 0, # constant roll
0, 0, 0, 0, o, 0, # constant pitch
0, 0, 0, 0, 0, o] # guess - .3 radians rotation cov
odom.twist.covariance = [100, 0, 0, 0, 0, 0,
0, 100, 0, 0, 0, 0,
0, 0, 100, 0, 0, 0, # large covariances - no data
0, 0, 0, 100, 0, 0,
0, 0, 0, 0, 100, 0,
0, 0, 0, 0, 0, 100]
global odom_pub
odom_topic = rospy.get_param('~odometry_publish_topic', 'vo')
pose2D_topic = rospy.get_param('~pose2D_topic', 'pose2D')
odom_pub = rospy.Publisher(odom_topic, Odometry)
rospy.Subscriber(pose2D_topic, Pose2D, on_pose)
rospy.spin()
def fvToMsg(fv, now): global seq fvMsg = PersonFVMsg() header = Header() header.seq = seq header.stamp = now header.frame_id = frameID rhv = Vector3() rev = Vector3() lhv = Vector3() lev = Vector3() setV3(rhv, fv, 0) setV3(lhv, fv, 3) setV3(rev, fv, 6) setV3(lev, fv, 9) fvMsg.header = header fvMsg.rh = rhv fvMsg.re = rev fvMsg.lh = lhv fvMsg.le = lev fvMsg.distance = fv[12] return fvMsg
def create_move_group_joints_goal(joint_names, joint_values, group="right_arm_torso", plan_only=True):
""" Creates a move_group goal based on pose.
@arg joint_names list of strings of the joint names
@arg joint_values list of digits with the joint values
@arg group string representing the move_group group to use
@arg plan_only bool to for only planning or planning and executing
@return MoveGroupGoal with the desired contents"""
header = Header()
header.frame_id = 'base_link'
header.stamp = rospy.Time.now()
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
for name, value in zip(joint_names, joint_values):
joint_c = JointConstraint()
joint_c.joint_name = name
joint_c.position = value
joint_c.tolerance_above = 0.01
joint_c.tolerance_below = 0.01
joint_c.weight = 1.0
goal_c.joint_constraints.append(joint_c)
moveit_goal.request.goal_constraints.append(goal_c)
moveit_goal.request.num_planning_attempts = 1
moveit_goal.request.allowed_planning_time = 5.0
moveit_goal.planning_options.plan_only = plan_only
moveit_goal.planning_options.planning_scene_diff.is_diff = True
moveit_goal.request.group_name = group
return moveit_goal
def test_4_remote(self):
'''
Tests remote kill by publishing hearbeat, stopping and checking alarm is raised, then
publishing hearbeat again to ensure alarm gets cleared.
'''
# publishing msg to network
pub = rospy.Publisher('/network', Header, queue_size=10)
rate = rospy.Rate(10)
t_end = rospy.Time.now() + rospy.Duration(1)
while rospy.Time.now() < t_end:
h = Header()
h.stamp = rospy.Time.now()
pub.publish(h)
rate.sleep()
self.reset_update()
rospy.sleep(8.5) # Wait slighly longer then the timeout on killboard
self.assert_raised()
self.reset_update()
t_end = rospy.Time.now() + rospy.Duration(1)
while rospy.Time.now() < t_end:
h = Header()
h.stamp = rospy.Time.now()
pub.publish(h)
rate.sleep()
self.AlarmBroadcaster.clear_alarm()
self.assert_cleared()
def poseStampedToLabeledSphereMarker(cls, psdata, label, fmt="{label} %Y-%m-%d %H:%M:%S", zoffset=0.05):
"[poseStamped, meta] -> sphereMarker"
ps, meta = psdata
res = []
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = ps.header.frame_id
sphere = Marker(type=Marker.SPHERE,
action=Marker.ADD,
header=h,
id=cls.marker_id)
sphere.scale.x = 0.07
sphere.scale.y = 0.07
sphere.scale.z = 0.07
sphere.pose = ps.pose
sphere.color = ColorRGBA(1.0,0,0,1.0)
sphere.ns = "db_play"
sphere.lifetime = rospy.Time(3)
cls.marker_id += 1
res.append(sphere)
text = Marker(type=Marker.TEXT_VIEW_FACING,
action=Marker.ADD,
header=h,
id=cls.marker_id)
text.scale.z = 0.1
text.pose = deepcopy(ps.pose)
text.pose.position.z += zoffset
text.color = ColorRGBA(1.0,1.0,1.0,1.0)
text.text = meta["inserted_at"].strftime(fmt).format(label=label)
text.ns = "db_play_text"
text.lifetime = rospy.Time(300)
cls.marker_id += 1
res.append(text)
return res
def make_header(frame_id, stamp=None):
if stamp == None:
stamp = rospy.Time.now()
header = Header()
header.stamp = stamp
header.frame_id = frame_id
return header
def get_ros_header(self):
header = Header()
header.stamp = rospy.Time.now()
header.seq = self.sequence
# http://www.ros.org/wiki/geometry/CoordinateFrameConventions#Multi_Robot_Support
header.frame_id = self.frame_id
return header
def computeICPBetweenScans(no1,no2, init_x = 0 , init_y = 0, init_yaw = 0):
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'ibeo'
example = cython_catkin_example.PyCCExample()
# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_'+str(no1)+'.txt')
# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_filtered_'+str(no1)+'.txt')
# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_direct_'+str(no1)+'.txt')
# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_lm_filtered_'+str(no1)+'.txt')
# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/onenorth/20150821-114839_sss/scan_filtered_'+str(no1)+'.txt')
if opts.use_accumulated:
test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_lm_filtered_'+str(no1)+'.txt'))
if opts.icp_use_filtered_data:
test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_filtered_'+str(no1)+'.txt'))
else:
test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_'+str(no1)+'.txt'))
H_points = [[p[0], p[1],1] for p in test_cloud]
# H_points = [[np.float32(p[0]), np.float32(p[1]),1] for p in test_cloud]
pc_point_t1 = pc2.create_cloud_xyz32(header, H_points)
# print 'text1: ',len(test_cloud)
# print test_cloud
# example.load_2d_array('ref_map',np.array(test_cloud, np.float32))
# test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_'+str(no2)+'.txt')
# test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_filtered_'+str(no2)+'.txt')
# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_direct_'+str(no2)+'.txt')
# test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_lm_filtered_'+str(no2)+'.txt')
# test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/onenorth/20150821-114839_sss/scan_filtered_'+str(no2)+'.txt')
if opts.use_accumulated:
test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_lm_filtered_'+str(no2)+'.txt'))
if opts.icp_use_filtered_data:
test_cloud2 = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_filtered_'+str(no2)+'.txt'))
else:
test_cloud2 = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_'+str(no2)+'.txt'))
H_points = [[p[0], p[1],1] for p in test_cloud2]
# H_points = [[np.float32(p[0]), np.float32(p[1]),1] for p in test_cloud]
pc_point_t2 = pc2.create_cloud_xyz32(header, H_points)
# print 'text2: ',len(test_cloud)
# example.load_2d_array('que_map',np.array(test_cloud, np.float32))
# names = example.get_point_xyz_clouds_names()
# print("Current point clouds: " + ",".join(names))
# icp_ros_result = g_processICP_srv(pc_point_t1, pc_point_t2)
# print icp_ros_result
# cython_icp_result = example.processICP(np.array(test_cloud, np.float32),np.array(test_cloud2, np.float32))
cython_icp_result = example.processICP(np.array([x[0] for x in test_cloud], np.float32),np.array([x[1] for x in test_cloud], np.float32),np.array([x[0] for x in test_cloud2], np.float32),np.array([x[1] for x in test_cloud2], np.float32), init_x, init_y, init_yaw)
# print cython_icp_result
return cython_icp_result
def retreat_move(self, height, velocity, forced=False):
if forced:
self.is_forced_retreat = True
cart_pos, cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
#print "Retreat EP:", ell_pos
latitude = ell_pos[0]
if self.trim_retreat:
latitude = min(latitude, TRIM_RETREAT_LATITUDE)
#rospy.loginfo("[face_adls_manager] Retreating from current location.")
retreat_pos = [latitude, ell_pos[1], height]
retreat_pos = self.ellipsoid.enforce_bounds(retreat_pos)
retreat_quat = [0,0,0,1]
if forced:
cart_path = self.ellipsoid.ellipsoidal_to_pose((retreat_pos, retreat_quat))
cart_msg = [PoseConv.to_pose_msg(cart_path)]
else:
ell_path = self.ellipsoid.create_ellipsoidal_path(ell_pos,
ell_quat,
retreat_pos,
retreat_quat,
velocity=0.01,
min_jerk=True)
cart_path = [self.ellipsoid.ellipsoidal_to_pose(ell_pose) for ell_pose in ell_path]
cart_msg = [PoseConv.to_pose_msg(pose) for pose in cart_path]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_msg)
self.pose_traj_pub.publish(pose_array)
self.is_forced_retreat = False
def test_AG_remote(self):
self.assertEqual(self.AlarmListener.is_cleared(), True,
msg='current state of kill signal is raised')
# publishing msg to network
pub = rospy.Publisher('/network', Header, queue_size=10)
rate = rospy.Rate(10)
t_end = rospy.Time.now() + rospy.Duration(3)
while rospy.Time.now() < t_end:
hello_header = Header()
hello_header.stamp = rospy.Time.now()
rospy.loginfo(hello_header)
pub.publish(hello_header)
rate.sleep()
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='REMOTE shutdown not worked. State = {}'.format(
self._current_alarm_state))
# stop sending the msg, the remote alarm will raise when stop recieving
# the msg for 8 secs
rospy.sleep(8.2)
self.assertEqual(
self.AlarmListener.is_raised(),
True,
msg='REMOTE raised not worked. State = {}'.format(
self._current_alarm_state))
def Publish(self):
imu_data = self._hal_proxy.GetImu()
imu_msg = Imu()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self._frame_id
imu_msg.header = h
imu_msg.orientation_covariance = (-1., )*9
imu_msg.linear_acceleration_covariance = (-1., )*9
imu_msg.angular_velocity_covariance = (-1., )*9
imu_msg.orientation.w = imu_data['orientation']['w']
imu_msg.orientation.x = imu_data['orientation']['x']
imu_msg.orientation.y = imu_data['orientation']['y']
imu_msg.orientation.z = imu_data['orientation']['z']
imu_msg.linear_acceleration.x = imu_data['linear_accel']['x']
imu_msg.linear_acceleration.y = imu_data['linear_accel']['y']
imu_msg.linear_acceleration.z = imu_data['linear_accel']['z']
imu_msg.angular_velocity.x = imu_data['angular_velocity']['x']
imu_msg.angular_velocity.y = imu_data['angular_velocity']['y']
imu_msg.angular_velocity.z = imu_data['angular_velocity']['z']
# Read the x, y, z and heading
self._publisher.publish(imu_msg)
def execute(self, userdata):
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
userdata.move_it_joint_goal = MoveGroupGoal()
goal_c = Constraints()
for name, value in zip(userdata.manip_joint_names, userdata.manip_goal_joint_pose):
joint_c = JointConstraint()
joint_c.joint_name = name
joint_c.position = value
joint_c.tolerance_above = 0.01
joint_c.tolerance_below = 0.01
joint_c.weight = 1.0
goal_c.joint_constraints.append(joint_c)
userdata.move_it_joint_goal.request.goal_constraints.append(goal_c)
userdata.move_it_joint_goal.request.num_planning_attempts = 5
userdata.move_it_joint_goal.request.allowed_planning_time = 5.0
userdata.move_it_joint_goal.planning_options.plan_only = False # False = Plan + Execute ; True = Plan only
userdata.move_it_joint_goal.planning_options.planning_scene_diff.is_diff = True
userdata.move_it_joint_goal.request.group_name = userdata.manip_joint_group
rospy.loginfo("Group Name: " + userdata.manip_joint_group)
rospy.loginfo("Joints name: " + str(userdata.manip_joint_names))
rospy.loginfo("Joints Values: " + str(userdata.manip_goal_joint_pose))
rospy.loginfo("GOAL TO SEND IS:... " + str(userdata.move_it_joint_goal))
return "succeeded"
def publish_poses_grasps(grasps):
rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
grasp_publisher = rospy.Publisher("grasp_pose_from_block_bla", PoseArray)
graspmsg = Grasp()
grasp_PA = PoseArray()
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
print graspmsg
print type(graspmsg)
p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
grasp_publisher.publish(grasp_PA)
rospy.loginfo("Press a to continue...")
while True:
choice = raw_input("> ")
if choice == 'a' :
print "Continuing, a pressed"
break
else:
grasp_publisher.publish(grasp_PA)
rospy.sleep(0.1)
def cbClusters(self, ros_data):
#Wait for filename to be received (if receiving from rostopic)
if self.filename is None:
return
if self.run is False:
self.pubMessages()
return
#Initialize object feature values
if self.initialized is False:
print "Reading object features from " + self.filename
self.loadObjects()
self.initialized = True
#Read cluster message
clusterArr = ros_data
clusters = ros_data.objects
self.transforms = ros_data.transforms
#Classify clusters
#self.labeled, self.tracked, self.errors, self.ids = self.run_filter(self.initX, self.labels, clusters)
self.tracked, self.ids, self.error = self.getMatchingLabels(self.initX, self.labels, clusters)
#Publish labels
if len(clusters) is 0 or self.tracked is None:
return
self.outMsg = LabeledObjects()
msgTime = rospy.Time.now()
head = Header()
head.stamp = msgTime
self.outMsg.header = head
self.outMsg.objects = self.tracked
self.outMsg.labels = self.ids
self.pubMessages()
def get_transform(tf_listener, frame1, frame2):
temp_header = Header()
temp_header.frame_id = frame1
temp_header.stamp = rospy.Time(0)
try:
tf_listener.waitForTransform(frame1, frame2, rospy.Time(0), rospy.Duration(5))
except tf.Exception, e:
rethrow_tf_exception(e, "tf transform was not there between %s and %s"%(frame1, frame2))
def app_frame_add_callback(self,msg):
rospy.loginfo("Heard App button press %s", msg.data)
controlMessage = Header()
#command to add waypoint;
controlMessage.seq = 1;
controlMessage.frame_id = msg.data;
self._command_pub.publish(controlMessage)
def toObjects(self, points): objects = Objects header = Header() header.stamp = rospy.Time.now() header.frame_id = self.base_frame objects.header = header objects.objects = toPoints(points) return objects
def publishScan(no):
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'ibeo'
test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_'+str(no)+'.txt')
H_points = [[p[0], p[1],1] for p in test_cloud]
pc_point_t = pc2.create_cloud_xyz32(header, H_points)
g_pub_ros.publish(pc_point_t)
def make_header(frame_id, stamp=None):
''' Creates a Header object for stamped ROS objects '''
if stamp == None:
stamp = rospy.Time.now()
header = Header()
header.stamp = stamp
header.frame_id = frame_id
return header
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line))
if(len(line) > 0):
lineParts = line.split('\t')
try:
if(lineParts[0] == 'quat'):
self._qx = float(lineParts[1])
self._qy = float(lineParts[2])
self._qz = float(lineParts[3])
self._qw = float(lineParts[4])
if(lineParts[0] == 'ypr'):
self._ax = float(lineParts[1])
self._ay = float(lineParts[2])
self._az = float(lineParts[3])
if(lineParts[0] == 'areal'):
self._lx = float(lineParts[1])
self._ly = float(lineParts[2])
self._lz = float(lineParts[3])
imu_msg = Imu()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
imu_msg.header = h
imu_msg.orientation_covariance = (-1., )*9
imu_msg.angular_velocity_covariance = (-1., )*9
imu_msg.linear_acceleration_covariance = (-1., )*9
imu_msg.orientation.x = self._qx
imu_msg.orientation.y = self._qy
imu_msg.orientation.z = self._qz
imu_msg.orientation.w = self._qw
imu_msg.angular_velocity.x = self._ax
imu_msg.angular_velocity.y = self._ay
imu_msg.angular_velocity.z = self._az
imu_msg.linear_acceleration.x = self._lx
imu_msg.linear_acceleration.y = self._ly
imu_msg.linear_acceleration.z = self._lz
self.imu_pub.publish(imu_msg)
except:
rospy.logwarn("Error in Sensor values")
rospy.logwarn(lineParts)
pass
def __init__(self,tf_listener=None,arms=['R']):
#rospy.loginfo('Greg version!')
self.arms = arms
self.tf_listener = tf_listener
if self.tf_listener is None:
self.tf_listener = tfx.TransformListener.instance()
rospy.loginfo('waiting for transform')
for arm in arms:
self.tf_listener.waitForTransform('/0_link','/tool_'+arm,rospy.Time(0),rospy.Duration(5))
# ADDED, initializes the rest
self.reset()
self.pub_cmd = rospy.Publisher('raven_command', RavenCommand)
self.state_sub = rospy.Subscriber('raven_state',raven_2_msgs.msg.RavenState,self._state_callback)
self.header = Header()
self.header.frame_id = '/0_link'
self.timeout = Util.Timeout(4)
self.stageLock = threading.Lock()
self.thread = None
self.isThreadPlaying = False
#################
# PAUSE COMMAND #
#################
header = Header()
header.frame_id = MyConstants.Frames.Link0
header.stamp = rospy.Time.now()
# create the tool pose
toolCmd = ToolCommand()
toolCmd.pose = tfx.pose([0,0,0]).msg.Pose()
toolCmd.pose_option = ToolCommand.POSE_RELATIVE
toolCmd.grasp_option = ToolCommand.GRASP_OFF
# create the arm command
armCmd = ArmCommand()
armCmd.tool_command = toolCmd
armCmd.active = True
# create the raven command
ravenPauseCmd = RavenCommand()
ravenPauseCmd.arm_names.append(self.arms)
ravenPauseCmd.arms.append(armCmd)
ravenPauseCmd.pedal_down = True
ravenPauseCmd.header = header
ravenPauseCmd.controller = Constants.CONTROLLER_CARTESIAN_SPACE
self.ravenPauseCmd = ravenPauseCmd
def callback(data):
error = 0
warn_string = ""
global prev_theta
global prev_theta2
global prev_theta4
rate = rospy.Rate(f)
r14 = float(data.pose.position.x)
r24 = float(data.pose.position.y)
r34 = float(data.pose.position.z)
theta1 = atan(r24 / r14)
alpha = r14 * cos(theta1) + r24 * sin(theta1)
if (-a1**4 + 2 * a1**2 * a2**2 + 2 * a1**2 * alpha**2 + 2 * a1**2 * r34**2
- a2**4 + 2 * a2**2 * alpha**2 + 2 * a2**2 * r34**2 - alpha**4 -
2 * alpha**2 * r34**2 - r34**4) < 0:
theta2 = prev_theta2
error = 2
else:
theta2 = -2 * atan(
(2 * a1 * r34 +
(-a1**4 + 2 * a1**2 * a2**2 + 2 * a1**2 * alpha**2 + 2 * a1**2 *
r34**2 - a2**4 + 2 * a2**2 * alpha**2 + 2 * a2**2 * r34**2 -
alpha**4 - 2 * alpha**2 * r34**2 - r34**4)**(0.5)) /
(a1**2 + 2 * a1 * alpha - a2**2 + alpha**2 + r34**2))
prev_theta2 = theta2 + pi / 2
theta2 = theta2 + pi / 2
if (-a1**2 + 2 * a1 * a2 - a2**2 + alpha**2 +
r34**2) * (a1**2 + 2 * a1 * a2 + a2**2 - alpha**2 - r34**2) < 0:
theta4 = prev_theta4
error = 2
else:
theta4 = -2 * atan(
(2 * a2 * r34 -
((-a1**2 + 2 * a1 * a2 - a2**2 + alpha**2 + r34**2) *
(a1**2 + 2 * a1 * a2 + a2**2 - alpha**2 - r34**2))**(0.5)) /
(-a1**2 + a2**2 + 2 * a2 * alpha + alpha**2 + r34**2))
prev_theta4 = theta4
if error == 2:
warn_string = "Pozycja koncowki niemozliwa do osiagniecia \n"
theta3 = theta4 - theta2
theta_vec = [theta1, theta2, theta3]
i = 0
for joint in restrictions:
if restrictions[i]['backward'] > theta_vec[i]:
theta_vec[i] = restrictions[i]['backward']
warn_string = warn_string + "Przekroczono ograniczenie dolne stawu o numerze: " + str(
i + 1) + "\n"
error = 1
#rospy.logerr(warn_string)
elif restrictions[i]['forward'] < theta_vec[i]:
theta_vec[i] = restrictions[i]['forward']
warn_string = warn_string + "Przekroczono ograniczenie gorne stawu o numerze: " + str(
i + 1) + "\n"
#rospy.logerr(warn_string)
error = 1
i = i + 1
if error == 1:
theta_vec = prev_theta
rospy.logerr(warn_string)
elif error == 2:
rospy.logerr(warn_string)
else:
prev_theta = theta_vec
# create a new JoinState
newJS = JointState()
newJS.header = Header()
newJS.header.stamp = rospy.Time.now()
newJS.header.frame_id = 'base_link'
newJS.name = ['base_to_link1', 'link1_to_link2',
'link2_to_link3'] #wskazanie jointow
newJS.position = theta_vec
pub.publish(newJS)
rate.sleep()
def moveRobot(self):
poruka = FollowJointTrajectoryActionGoal()
h = Header()
h.stamp = rospy.get_rostime()
print 'Start H'
print h
print 'end H'
poruka.header = h
poruka.goal.trajectory.joint_names.append('arm_1_joint')
poruka.goal.trajectory.joint_names.append('arm_2_joint')
poruka.goal.trajectory.joint_names.append('arm_3_joint')
poruka.goal.trajectory.joint_names.append('arm_4_joint')
poruka.goal.trajectory.joint_names.append('arm_5_joint')
poruka.goal.trajectory.joint_names.append('arm_6_joint')
poruka.goal_id.id = 'Move trajectory ' + str(h.stamp.nsecs)
h.stamp = rospy.get_rostime()
poruka.goal.trajectory.header.stamp = h.stamp
poruka.header.stamp = h.stamp
tocka = JointTrajectoryPoint()
'''
tocka.positions.append(0)
tocka.positions.append(-1.58) # INVERTIRATI NA CANOPEN
tocka.positions.append(1.38005) # -1.38005 INVERTIRATI na IPI!!!!
tocka.positions.append(0)
tocka.positions.append(1.36365) # -1.36365 # INVERTIRATI NA CANOPEN
tocka.positions.append(0)
'''
if (1 == 1):
tocka.positions.append(0)
tocka.positions.append(0)
tocka.positions.append(0 * -1.57) # INVERTIRATI!!!!
tocka.positions.append(0)
tocka.positions.append(0)
tocka.positions.append(0)
else:
tocka.positions.append(0)
tocka.positions.append(0.0)
tocka.positions.append(1.77) # INVERTIRATI!!!!
tocka.positions.append(0.0)
tocka.positions.append(-0.2)
tocka.positions.append(0.9)
tocka.velocities.append(0)
tocka.velocities.append(0)
tocka.velocities.append(0)
tocka.velocities.append(0)
tocka.velocities.append(0)
tocka.velocities.append(0)
tocka.accelerations.append(0)
tocka.accelerations.append(0)
tocka.accelerations.append(0)
tocka.accelerations.append(0)
tocka.accelerations.append(0)
tocka.accelerations.append(0)
tocka.time_from_start.nsecs = 0 * pow(10, 9)
tocka.time_from_start.secs = 7
poruka.goal.trajectory.points.append(tocka)
#self.trajPub_red.publish(poruka)
self.trajPub_blue.publish(poruka)
print ""
print poruka
print rospy.get_rostime()
print h.stamp
def robot_scan_received(self, data):
# wait until initialization is complete
if not (self.initialized):
return
# we need to be able to transfrom the laser frame to the base frame
if not (self.tf_listener.canTransform(
self.base_frame, data.header.frame_id, data.header.stamp)):
return
# wait for a little bit for the transform to become avaliable (in case the scan arrives
# a little bit before the odom to base_footprint transform was updated)
self.tf_listener.waitForTransform(self.base_frame,
self.odom_frame, data.header.stamp,
rospy.Duration(0.5))
if not (self.tf_listener.canTransform(
self.base_frame, data.header.frame_id, data.header.stamp)):
return
# calculate the pose of the laser distance sensor
p = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=data.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# determine where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=data.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# we need to be able to compare the current odom pose to the prior odom pose
# if there isn't a prior odom pose, set the odom_pose variable to the current pose
if not self.odom_pose_last_motion_update:
self.odom_pose_last_motion_update = self.odom_pose
return
if self.particle_cloud:
# check to see if we've moved far enough to perform an update
curr_x = self.odom_pose.pose.position.x
old_x = self.odom_pose_last_motion_update.pose.position.x
curr_y = self.odom_pose.pose.position.y
old_y = self.odom_pose_last_motion_update.pose.position.y
curr_yaw = get_yaw_from_pose(self.odom_pose.pose)
old_yaw = get_yaw_from_pose(self.odom_pose_last_motion_update.pose)
if (np.abs(curr_x - old_x) > self.lin_mvmt_threshold
or np.abs(curr_y - old_y) > self.lin_mvmt_threshold
or np.abs(curr_yaw - old_yaw) > self.ang_mvmt_threshold):
# This is where the main logic of the particle filter is carried out
self.update_particles_with_motion_model()
self.update_particle_weights_with_measurement_model(data)
self.normalize_particles()
self.resample_particles()
self.update_estimated_robot_pose()
self.publish_particle_cloud()
self.publish_estimated_robot_pose()
self.odom_pose_last_motion_update = self.odom_pose
def no_ball_poseStamped(self):
return PoseStamped( header = Header(stamp = rospy.Time.now(), frame_id = "odom"), \
pose = Pose(position = Point(x = -1, y = -1, z = -1), orientation = Quaternion(w=1)))
def pcl_callback(self, pcl):
"""Callback for the pcl. Input should be a PointCloud2."""
self.rate_counter += 1
if self.rate_counter % 3 == 0:
print 'Received Pointcloud. ' + str(time.time())
points_as_list = pc2_to_list(pcl) # Convert to List
thresholded = self.thresholding(points_as_list) # Apply Thresholds
# Uniform reduction
uniform_reduction_factor = 1
thresholded = list(
thresholded[i]
for i in xrange(0, len(thresholded), uniform_reduction_factor))
# DEBUG Test publish thresholded points
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)
]
header = Header()
header.frame_id = "map"
cloud = pc2.create_cloud(header, fields, thresholded)
cloud.header.stamp = rospy.Time.now()
self.threshold_publisher.publish(cloud)
# END Test publish
if self.started:
if self.pcl_reference == None:
self.pcl_reference = o3d.geometry.PointCloud() # pylint: disable=no-member
self.pcl_reference.points = o3d.utility.Vector3dVector(
thresholded) # pylint: disable=no-member
self.pcl_reference.estimate_normals(
fast_normal_computation=False)
# Store reference point position
pcd_tree = o3d.geometry.KDTreeFlann(self.pcl_reference) # pylint: disable=no-member
ref_point = None
ref_point_nb = 0
# print len(self.pcl_reference.points)
for i in range(0, len(self.pcl_reference.points)):
nb = pcd_tree.search_radius_vector_3d(
self.pcl_reference.points[i], 0.01)
if nb > ref_point_nb:
ref_point = self.pcl_reference.points[i]
ref_point_nb = nb
# print i
self.position_reference = np.asarray(
[[1.0, 0, 0, ref_point[0]], [0, 1.0, 0, ref_point[1]],
[0, 0, 1.0, ref_point[2]], [0.0, 0.0, 0.0, 1.0]])
# print(self.position_reference)
position_shaped = self.position_reference.reshape((16))
self.position_publisher.publish(position_shaped.tolist())
# self.started = False
else:
calctime = time.time()
# Calculate diff from ICP
head_tf = self.calc_head_transform_icp(thresholded)
#print np.round(head_tf, decimals=4)
# Calculate position
# if not self.head_pos == None: # DEBUG PRINT
# print (np.dot(self.position_reference, np.linalg.inv(head_tf)) - self.head_pos)
#self.head_pos = np.dot(self.position_reference, np.linalg.inv(head_tf))
self.head_pos = np.dot(self.position_reference, head_tf)
#print 'Calculation finished ' + str((time.time() - calctime))
print np.round(self.head_pos, decimals=4)
#print "Reference:"
#print np.round(self.position_reference, decimals=4)
head_tf_shaped = self.head_pos.reshape((16)) # Reshape
self.position_publisher.publish(
head_tf_shaped.tolist()) # Publish
# Eval
self.calculation_times.append((time.time() - calctime))
self.positions.append(self.head_pos)
self.ur5_pos_a.append(self.last_ur5_pos)
def image_callback(self, img: Image):
rospy.loginfo_once("Recieved Message")
t_start = time.time()
if not self.camera.ready() or not self.trajectory_complete:
return
pts = []
self.camera.reset_position(publish_basecamera=True, timestamp=img.header.stamp)
image = CvBridge().imgmsg_to_cv2(img, desired_encoding="rgb8")
hsv = cv2.cvtColor(src=image, code=cv2.COLOR_BGR2HSV)
h = self.camera.calculateHorizonCoverArea()
cv2.rectangle(image, [0, 0], [640, h], [0, 0, 0], cv2.FILLED)
# Field line detection
mask2 = cv2.inRange(hsv, (0, 0, 255 - 65), (255, 65, 255))
out = cv2.bitwise_and(image, image, mask=mask2)
kernel = np.ones((7, 7), np.uint8)
out = cv2.morphologyEx(out, cv2.MORPH_CLOSE, kernel)
cdst = cv2.cvtColor(out, cv2.COLOR_BGR2GRAY)
retval, dst = cv2.threshold(cdst, 127, 255, cv2.THRESH_BINARY)
edges = cv2.Canny(dst, 50, 150)
lines = cv2.HoughLinesP(edges, rho=DetectorFieldline.HOUGH_RHO,
theta=DetectorFieldline.HOUGH_THETA,
threshold=DetectorFieldline.HOUGH_THRESHOLD,
minLineLength=DetectorFieldline.HOUGH_MIN_LINE_LENGTH,
maxLineGap=DetectorFieldline.HOUGH_MAX_LINE_GAP)
ccdst = cv2.cvtColor(cdst, cv2.COLOR_GRAY2RGB)
if lines is None:
return
for l in lines:
x1, y1, x2, y2 = l[0]
# computing magnitude and angle of the line
if x2 == x1:
angle = 0
else:
angle = np.rad2deg(np.arctan((y2 - y1) / (x2 - x1)))
pt1 = (x1, y1)
pt2 = (x2, y2)
if abs(angle) < 10: # Horizontal
cv2.line(ccdst, pt1, pt2, (255, 0, 0), thickness=3, lineType=cv2.LINE_AA)
elif abs(abs(angle) - 90) < 10: # Vertical
cv2.line(ccdst, pt1, pt2, (0, 255, 0), thickness=3, lineType=cv2.LINE_AA)
else:
cv2.line(ccdst, pt1, pt2, (0, 0, 255), thickness=3, lineType=cv2.LINE_AA)
if (pt2[0] - pt1[0]) == 0:
continue
slope = (pt2[1] - pt1[1]) / (pt2[0] - pt1[0])
b = y1 - slope * x1
for i in range(x1, x2, 15):
y = slope * i + b
pt = [i, y]
pts.append(pt)
if self.image_publisher.get_num_connections() > 0:
img_out = CvBridge().cv2_to_imgmsg(ccdst)
img_out.header = img.header
self.image_publisher.publish(img_out)
if self.point_cloud_publisher.get_num_connections() > 0:
points3d = []
for p in pts:
points3d.append(self.camera.findFloorCoordinate(p))
# Publish fieldlines in laserscan format
header = Header()
header.stamp = img.header.stamp
header.frame_id = self.robot_name + "/base_camera"
point_cloud_msg = pcl2.create_cloud_xyz32(header, points3d)
if self.point_cloud_publisher.get_num_connections() > 0:
self.point_cloud_publisher.publish(point_cloud_msg)
t_end = time.time()
rospy.loginfo_throttle(20, "Fieldline detection rate: " + str(t_end - t_start))
#! /usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from std_msgs.msg import Header
from gazebo_msgs.srv import GetModelState, GetModelStateRequest
rospy.init_node('odom_pub')
odom_pub = rospy.Publisher('/my_odom', Odometry)
rospy.wait_for_service('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
odom = Odometry()
header = Header()
header.frame_id = '/odom'
model = GetModelStateRequest()
model.model_name = 'simple_box'
r = rospy.Rate(2)
while not rospy.is_shutdown():
result = get_model_srv(model)
odom.pose.pose = result.pose
odom.twist.twist = result.twist
header.stamp = rospy.Time.now()
odom.header = header
def extract_and_publish_contours(self):
if OPENCV_VERSION == 2:
contours, hierarchy = cv2.findContours(self.threshed,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
elif OPENCV_VERSION == 3:
image, contours, hierarchy = cv2.findContours(self.threshed,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
# http://docs.opencv.org/trunk/doc/py_tutorials/py_imgproc/py_contours/py_contour_features/py_contour_features.html
try:
header = Header(stamp=self.framestamp, frame_id=str(self.framenumber))
except:
header = Header(stamp=None, frame_id=str(self.framenumber))
print 'could not get framestamp, run tracker_nobuffer instead'
contour_info = []
for contour in contours:
# Large objects are approximated by an ellipse
if len(contour) > 5:
x, y, ecc, area, angle = fit_ellipse_to_contour(self, contour)
# if object is too large, split it in two, this helps with colliding objects, but is not 100% correct
if area > self.params['max_expected_area']:
slope = np.tan(angle)
intercept = y - slope * x
c1 = []
c2 = []
for point in contour:
point = point.reshape(2)
if is_point_below_line(point, slope, intercept):
c1.append([point])
else:
c2.append([point])
c1 = np.array(c1)
c2 = np.array(c2)
if len(c1) > 5:
x, y, ecc, area, angle = fit_ellipse_to_contour(
self, np.array(c1))
if area < self.params['max_size'] and area > self.params[
'min_size']:
data = add_data_to_contour_info(
x, y, ecc, area, angle, self.dtCamera, header)
contour_info.append(data)
if len(c2) > 5:
x, y, ecc, area, angle = fit_ellipse_to_contour(
self, np.array(c2))
if area < self.params['max_size'] and area > self.params[
'min_size']:
data = add_data_to_contour_info(
x, y, ecc, area, angle, self.dtCamera, header)
contour_info.append(data)
else:
if area < self.params['max_size'] and area > self.params[
'min_size']:
data = add_data_to_contour_info(x, y, ecc, area, angle,
self.dtCamera, header)
contour_info.append(data)
# Small ones just get a point
else:
area = 0
# publish the contours
self.pubContours.publish(Contourlist(header=header, contours=contour_info))
return
from math import pi, radians
from gazebo_msgs.msg import ModelState, ModelStates
from nav_msgs.msg import Odometry, Path
from tf import TransformBroadcaster
import rospy
import copy
path_pub = rospy.Publisher('/global_path', Path, latch=True, queue_size=50)
puntos = [[-1, 0], [-3.5, 0], [-3.5, 3.5], [1.5, 3.5], [1.5, -1.5],
[3.5, -1.5], [3.5, -8.0], [-2.5, -8.0], [-2.5, -5.5], [1.5, -5.5],
[1.5, -3.5], [-1, -3.5]]
# puntos=[[-3.5,3.5],[1.5,3.5],[1.5,-1.5],[3.5,-1.5],[3.5,-8.0],[-2.5,-8.0],[-2.5,-5.5],[1.5,-5.5],[1.5,-3.5],[-1,-3.5]]
# puntos=[[-3.5,0],[-3.5,3.5]]
rospy.init_node('robot_path', anonymous=True)
rate = rospy.Rate(1) # 10hz
h = Header()
path = Path()
pose_t = PoseStamped()
if __name__ == '__main__':
try:
print("dibujando el path en rviz")
h.frame_id = "odom"
path.header = h
pose_t.header = h
while not rospy.is_shutdown():
# h = Header()
# path=Path()
# pose_t=PoseStamped()
# h.frame_id="odom"
# path.header=h
# pose_t.header=h
def test_vehicle_status(self):
rospy.init_node('test_node', anonymous=True)
msg = rospy.wait_for_message("/carla/ego_vehicle/vehicle_status", CarlaEgoVehicleStatus, timeout=15)
self.assertNotEqual(msg.header, Header()) #todo: check frame-id
self.assertNotEqual(msg.orientation, Quaternion())
def get_pose_stamped(self):
"""Will return a pose stamped object of the pose."""
pose = self.get_pose()
header = Header(stamp=rospy.Time.now(), frame_id='base')
return PoseStamped(header=header, pose=pose)
def right_arm_go_to_pose_goal(self):
# 设置动作对象变量,此处为arm
right_arm = self.right_arm
# 获取当前末端执行器位置姿态
pose_goal = right_arm.get_current_pose().pose
# 限制末端夹具运动
right_joint_const = JointConstraint()
right_joint_const.joint_name = "gripper_r_joint_r"
if Rightfinger > -55 :
right_joint_const.position = 0.024
else:
right_joint_const.position = 0
right_joint_const.weight = 1.0
# 限制1轴转动
right_joint1_const = JointConstraint()
right_joint1_const.joint_name = "yumi_joint_1_r"
right_joint1_const.position = 0
right_joint1_const.tolerance_above = 120
right_joint1_const.tolerance_below = 0
right_joint1_const.weight = 1.0
# 限制2轴转动
right_joint2_const = JointConstraint()
right_joint2_const.joint_name = "yumi_joint_2_r"
right_joint2_const.position = 0
right_joint2_const.tolerance_above = 0
right_joint2_const.tolerance_below = 150
right_joint2_const.weight = 1.0
# 限制3轴转动
right_joint3_const = JointConstraint()
right_joint3_const.joint_name = "yumi_joint_3_r"
right_joint3_const.position = 0
right_joint3_const.tolerance_above = 35
right_joint3_const.tolerance_below = 55
right_joint3_const.weight = 1.0
# 限制4轴转动
right_joint4_const = JointConstraint()
right_joint4_const.joint_name = "yumi_joint_4_r"
right_joint4_const.position = 0
right_joint4_const.tolerance_above = 60
right_joint4_const.tolerance_below = 75
right_joint4_const.weight = 1.0
# 限制5轴转动
right_joint5_const = JointConstraint()
right_joint5_const.joint_name = "yumi_joint_5_r"
right_joint5_const.position = 40
right_joint5_const.tolerance_above = 50
right_joint5_const.tolerance_below = 20
right_joint5_const.weight = 1.0
# 限制6轴转动
right_joint6_const = JointConstraint()
right_joint6_const.joint_name = "yumi_joint_6_r"
right_joint6_const.position = 0
right_joint6_const.tolerance_above = 10
right_joint6_const.tolerance_below = 35
right_joint6_const.weight = 1.0
# 限制7轴转动
right_joint7_const = JointConstraint()
right_joint7_const.joint_name = "yumi_joint_7_r"
right_joint7_const.position = -10
right_joint7_const.tolerance_above = 0
right_joint7_const.tolerance_below = 160
right_joint7_const.weight = 1.0
# 限制末端位移
right_position_const = PositionConstraint()
right_position_const.header = Header()
right_position_const.link_name = "gripper_r_joint_r"
right_position_const.target_point_offset.x = 0.5
right_position_const.target_point_offset.y = -0.5
right_position_const.target_point_offset.z = 1.0
right_position_const.weight = 1.0
# 添加末端姿态约束:
right_orientation_const = OrientationConstraint()
right_orientation_const.header = Header()
right_orientation_const.orientation = pose_goal.orientation
right_orientation_const.link_name = "gripper_r_joint_r"
right_orientation_const.absolute_x_axis_tolerance = 0.50
right_orientation_const.absolute_y_axis_tolerance = 0.25
right_orientation_const.absolute_z_axis_tolerance = 0.50
right_orientation_const.weight = 100
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const]
consts.orientation_constraints = [right_orientation_const]
# consts.position_constraints = [right_position_const]
right_arm.set_path_constraints(consts)
# 设置动作对象目标位置姿态
pose_goal.orientation.x = Right_Qux
pose_goal.orientation.y = Right_Quy
pose_goal.orientation.z = Right_Quz
pose_goal.orientation.w = Right_Quw
pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053
pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12
pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47
right_arm.set_pose_target(pose_goal)
print "End effector pose %s" % pose_goal
# # 设置动作对象目标位置姿态
# pose_goal.orientation.x = 0.1644
# pose_goal.orientation.y = 0.3111
# pose_goal.orientation.z = 0.9086
# pose_goal.orientation.w = 0.2247
# pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053
# pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12
# pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47
# right_arm.set_pose_target(pose_goal)
# print "End effector pose %s" % pose_goal
# # 设置动作对象目标位置姿态
# pose_goal.orientation.x = pose_goal.orientation.x
# pose_goal.orientation.y = pose_goal.orientation.y
# pose_goal.orientation.z = pose_goal.orientation.z
# pose_goal.orientation.w = pose_goal.orientation.w
# pose_goal.position.x = pose_goal.position.x
# pose_goal.position.y = pose_goal.position.y - 0.01
# pose_goal.position.z = pose_goal.position.z
# right_arm.set_pose_target(pose_goal)
# print "End effector pose %s" % pose_goal
# 规划和输出动作
traj = right_arm.plan()
right_arm.execute(traj, wait=False)
# 动作完成后清除目标信息
right_arm.clear_pose_targets()
# 清除路径约束
right_arm.clear_path_constraints()
# 确保没有剩余未完成动作在执行
right_arm.stop()
def left_arm_go_to_pose_goal(self):
# 设置动作对象变量,此处为arm
left_arm = self.left_arm
# 获取当前末端执行器位置姿态
pose_goal = left_arm.get_current_pose().pose
# 限制末端夹具运动
left_joint_const = JointConstraint()
left_joint_const.joint_name = "gripper_l_joint_r"
if Leftfinger > -32 :
left_joint_const.position = 0.024
else:
left_joint_const.position = 0
left_joint_const.weight = 1.0
# 限制末端位移
left_position_const = PositionConstraint()
left_position_const.header = Header()
left_position_const.link_name = "gripper_l_joint_r"
left_position_const.target_point_offset.x = 0.5
left_position_const.target_point_offset.y = -0.5
left_position_const.target_point_offset.z = 1.0
left_position_const.weight = 1.0
# # 限制1轴转动
left_joint1_const = JointConstraint()
left_joint1_const.joint_name = "yumi_joint_1_l"
left_joint1_const.position = 0
left_joint1_const.tolerance_above = 1.76
left_joint1_const.tolerance_below = 0
left_position_const.weight = 1.0
# 限制2轴转动
left_joint2_const = JointConstraint()
left_joint2_const.joint_name = "yumi_joint_2_l"
left_joint2_const.position = 0
left_joint2_const.tolerance_above = 0
left_joint2_const.tolerance_below = 150
left_joint2_const.weight = 1.0
# 限制3轴转动
left_joint3_const = JointConstraint()
left_joint3_const.joint_name = "yumi_joint_3_l"
left_joint3_const.position = 0
left_joint3_const.tolerance_above = 35
left_joint3_const.tolerance_below = 55
left_joint3_const.weight = 1.0
# 限制4轴转动
left_joint4_const = JointConstraint()
left_joint4_const.joint_name = "yumi_joint_4_l"
left_joint4_const.position = 0
left_joint4_const.tolerance_above = 60
left_joint4_const.tolerance_below = 75
left_joint4_const.weight = 1.0
# 限制5轴转动
right_joint5_const = JointConstraint()
right_joint5_const.joint_name = "yumi_joint_5_l"
right_joint5_const.position = 40
right_joint5_const.tolerance_above = 50
right_joint5_const.tolerance_below = 20
right_joint5_const.weight = 1.0
# 限制6轴转动
left_joint6_const = JointConstraint()
left_joint6_const.joint_name = "yumi_joint_6_l"
left_joint6_const.position = 0
left_joint6_const.tolerance_above = 10
left_joint6_const.tolerance_below = 35
left_joint6_const.weight = 1.0
# 限制7轴转动
left_joint7_const = JointConstraint()
left_joint7_const.joint_name = "yumi_joint_7_l"
left_joint7_const.position = -10
left_joint7_const.tolerance_above = 0
left_joint7_const.tolerance_below = 160
left_joint7_const.weight = 1.0
# 限制末端位移
left_position_const = PositionConstraint()
left_position_const.header = Header()
left_position_const.link_name = "gripper_l_joint_r"
left_position_const.target_point_offset.x = 0.5
left_position_const.target_point_offset.y = 0.25
left_position_const.target_point_offset.z = 0.5
left_position_const.weight = 1.0
# 添加末端姿态约束:
left_orientation_const = OrientationConstraint()
left_orientation_const.header = Header()
left_orientation_const.orientation = pose_goal.orientation
left_orientation_const.link_name = "gripper_l_joint_r"
left_orientation_const.absolute_x_axis_tolerance = 0.5
left_orientation_const.absolute_y_axis_tolerance = 0.25
left_orientation_const.absolute_z_axis_tolerance = 0.5
left_orientation_const.weight = 100
# 施加全约束
consts = Constraints()
consts.joint_constraints = [left_joint_const]
consts.orientation_constraints = [left_orientation_const]
# consts.position_constraints = [left_position_const]
left_arm.set_path_constraints(consts)
# 设置动作对象目标位置姿态
pose_goal.orientation.x = Left_Qux
pose_goal.orientation.y = Left_Quy
pose_goal.orientation.z = Left_Quz
pose_goal.orientation.w = Left_Quw
pose_goal.position.x = (Neurondata[11]-0.05)*1.48+0.053
pose_goal.position.y = (Neurondata[9]-0.18)*1.48+0.12
pose_goal.position.z = (Neurondata[10]-0.53)*1.48+0.47
left_arm.set_pose_target(pose_goal)
print "End effector pose %s" % pose_goal
# # 设置动作对象目标位置姿态
# pose_goal.orientation.x = pose_goal.orientation.x
# pose_goal.orientation.y = pose_goal.orientation.y
# pose_goal.orientation.z = pose_goal.orientation.z
# pose_goal.orientation.w = pose_goal.orientation.w
# pose_goal.position.x = pose_goal.position.x
# pose_goal.position.y = pose_goal.position.y - 0.01
# pose_goal.position.z = pose_goal.position.z
# left_arm.set_pose_target(pose_goal)
# print "End effector pose %s" % pose_goal
# 规划和输出动作
traj = left_arm.plan()
left_arm.execute(traj, wait=False)
# 动作完成后清除目标信息
left_arm.clear_pose_targets()
# 清除路径约束
left_arm.clear_path_constraints()
# 确保没有剩余未完成动作在执行
left_arm.stop()
def _plot(self, publisher, model, data, previous_ids=()):
"""Create markers for each object of the model and publish it as MarkerArray (also delete unused previously created markers)"""
from rospy import get_rostime
from std_msgs.msg import Header, ColorRGBA
from geometry_msgs.msg import Point
from visualization_msgs.msg import MarkerArray, Marker
self.seq += 1
header = Header(frame_id='map', seq=self.seq,
stamp=get_rostime()) # Common header for every marker
marker_array = MarkerArray()
for obj in model.geometryObjects:
obj_id = model.getGeometryId(obj.name)
# Prepare marker
marker = Marker()
marker.id = obj_id
marker.header = header
marker.action = Marker.ADD # same as Marker.MODIFY
marker.pose = SE3ToROSPose(data.oMg[obj_id])
marker.color = ColorRGBA(*obj.meshColor)
if obj.meshTexturePath != "":
warnings.warn(
"Textures are not supported in RVizVisualizer (for " +
obj.name + ")",
category=UserWarning,
stacklevel=2)
# Create geometry
geom = obj.geometry
if WITH_HPP_FCL_BINDINGS and isinstance(geom, hppfcl.ShapeBase):
# append a primitive geometry
if isinstance(geom, hppfcl.Cylinder):
d, l = 2 * geom.radius, 2 * geom.halfLength
marker.type = Marker.CYLINDER
marker.scale = Point(d, d, l)
marker_array.markers.append(marker)
elif isinstance(geom, hppfcl.Box):
size = npToTuple(2. * geom.halfSide)
marker.type = Marker.CUBE
marker.scale = Point(*size)
marker_array.markers.append(marker)
elif isinstance(geom, hppfcl.Sphere):
d = 2 * geom.radius
marker.type = Marker.SPHERE
marker.scale = Point(d, d, d)
marker_array.markers.append(marker)
elif isinstance(geom, hppfcl.Capsule):
d, l = 2 * geom.radius, 2 * geom.halfLength
marker_array.markers.extend(
create_capsule_markers(marker, data.oMg[obj_id], d, l))
else:
msg = "Unsupported geometry type for %s (%s)" % (
obj.name, type(geom))
warnings.warn(msg, category=UserWarning, stacklevel=2)
continue
else:
# append a mesh
marker.type = Marker.MESH_RESOURCE # Custom mesh
marker.scale = Point(*npToTuple(obj.meshScale))
marker.mesh_resource = 'file://' + obj.meshPath
marker_array.markers.append(marker)
# Remove unused markers
new_ids = [marker.id for marker in marker_array.markers]
for old_id in previous_ids:
if not old_id in new_ids:
marker_remove = Marker()
marker_remove.header = header
marker_remove.id = old_id
marker_remove.action = Marker.DELETE
marker_array.markers.append(marker_remove)
# Publish markers
publisher.publish(marker_array)
# Return list of markers id
return new_ids
def incredibly_basic(self):
# If there is no background image, grab one, and move on to the next frame
if self.backgroundImage is None:
reset_background(self)
return
if self.reset_background_flag:
reset_background(self)
self.reset_background_flag = False
return
self.absdiff = cv2.absdiff(np.float32(self.imgScaled),
self.backgroundImage)
self.imgproc = copy.copy(self.imgScaled)
retval, self.threshed = cv2.threshold(self.absdiff,
self.params['threshold'], 255, 0)
# convert to gray if necessary
if len(self.threshed.shape) == 3:
self.threshed = np.uint8(
cv2.cvtColor(self.threshed, cv2.COLOR_BGR2GRAY))
# extract and publish contours
# http://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_imgproc/py_contours/py_contour_features/py_contour_features.html
if OPENCV_VERSION == 2:
contours, hierarchy = cv2.findContours(self.threshed,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
elif OPENCV_VERSION == 3:
self.threshed = np.uint8(self.threshed)
image, contours, hierarchy = cv2.findContours(self.threshed,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
try:
header = Header(stamp=self.framestamp, frame_id=str(self.framenumber))
except:
header = Header(stamp=None, frame_id=str(self.framenumber))
print 'could not get framestamp, run tracker_nobuffer instead'
contour_info = []
for contour in contours:
if len(contour) > 5: # Large objects are approximated by an ellipse
ellipse = cv2.fitEllipse(contour)
(x, y), (a, b), angle = ellipse
a /= 2.
b /= 2.
ecc = np.min((a, b)) / np.max((a, b))
area = np.pi * a * b
data = Contourinfo()
data.header = header
data.dt = dtCamera
data.x = x
data.y = y
data.area = area
data.angle = angle
data.ecc = ecc
contour_info.append(data)
else: # Small ones get ignored
pass
# publish the contours
self.pubContours.publish(Contourlist(header=header, contours=contour_info))
def ik_test(limb, dx, dy, dz):
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
left_limb=baxter_interface.Limb('left')
rospy.sleep(0.1)
right_limb=baxter_interface.Limb('right')
rospy.sleep(0.1)
# left_limb.move_to_neutral()
# right_limb.move_to_neutral()
left_end=left_limb.endpoint_pose()
left_pos=left_end['position']
left_or=left_end['orientation']
# right_end=right_limb.endpoint_pose()
# right_pos=right_end['position']
# right_or=right_end['orientation']
xn=left_pos.x
yn=left_pos.y
zn=left_pos.z
poses = {
'left': PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=xn+dx,
y=yn+dy,
z=zn+dz,
),
orientation=Quaternion(
x=left_or.x,
y=left_or.y,
z=left_or.z,
w=left_or.w,
),
),
),
'right': PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=0.656982770038,
y=-0.852598021641,
z=0.0388609422173,
),
orientation=Quaternion(
x=0.367048116303,
y=0.885911751787,
z=-0.108908281936,
w=0.261868353356,
),
),
),
}
ikreq.pose_stamp.append(poses[limb])
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return 1
def convert_numpy_2_pointcloud2_color(points,
stamp=None,
frame_id=None,
maxDistColor=None):
# Clipping input.
dist = np.linalg.norm(points, axis=1)
if (maxDistColor is not None and maxDistColor > 0):
dist = np.clip(dist, 0, maxDistColor)
# Compose color.
cr, cg, cb = color_map(dist, DIST_COLORS, DIST_COLOR_LEVELS)
C = np.zeros((cr.size, 4), dtype=np.uint8) + 255
C[:, 0] = cb.astype(np.uint8)
C[:, 1] = cg.astype(np.uint8)
C[:, 2] = cr.astype(np.uint8)
C = C.view("uint32")
# Structured array.
pointsColor = np.zeros( (points.shape[0], 1), \
dtype={
"names": ( "x", "y", "z", "rgba" ),
"formats": ( "f4", "f4", "f4", "u4" )} )
points = points.astype(np.float32)
pointsColor["x"] = points[:, 0].reshape((-1, 1))
pointsColor["y"] = points[:, 1].reshape((-1, 1))
pointsColor["z"] = points[:, 2].reshape((-1, 1))
pointsColor["rgba"] = C
header = Header()
if stamp is None:
header.stamp = rospy.Time().now()
else:
header.stamp = stamp
if frame_id is None:
header.frame_id = "/map"
else:
header.frame_id = frame_id
msg = PointCloud2()
msg.header = header
if len(points.shape) == 3:
msg.height = points.shape[1]
msg.width = points.shape[0]
else:
msg.height = 1
msg.width = points.shape[0]
msg.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('rgb', 12, PointField.UINT32, 1),
]
msg.is_bigendian = False
msg.point_step = 16
msg.row_step = msg.point_step * points.shape[0]
msg.is_dense = int(np.isfinite(points).all())
msg.data = pointsColor.tostring()
return msg
import matplotlib.pyplot as plt
from os.path import join
import time
import sys
import rosbag
import ros_numpy
import struct
from sensor_msgs.msg import PointCloud2, PointField
import sensor_msgs.point_cloud2 as pc2
from std_msgs.msg import Header
from readpcd import offset_range_img, image_rows_full, image_cols, save_grey_img, create_4dir, progressbar
from readbag import get_topics_types
from numpy2pcd import range2xyz
header = Header()
header.frame_id = "map"
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
] # from point_cloud2.py def create_cloud_xyz32
def save_bag(range_imgs):
bag = rosbag.Bag('test.bag', 'w')
bag_topic = '/object_scan/raw_cloud'
for i in range(0, range_imgs.shape[0]):
clouds = range2xyz(range_imgs[i])
def get_end_effector_pose_from_state(self, state):
header = Header()
fk_link_names = [self._move_group_commander.get_end_effector_link()]
header.frame_id = self._move_group_commander.get_pose_reference_frame()
response = self._forward_k(header, fk_link_names, state)
return response.pose_stamped[0]
def spin_once(self):
def quat_from_orient(orient):
'''Build a quaternion from orientation data.'''
try:
w, x, y, z = orient['quaternion']
return (x, y, z, w)
except KeyError:
pass
try:
return quaternion_from_euler(pi*orient['roll']/180.,
pi*orient['pitch']/180, pi*orient['yaw']/180.)
except KeyError:
pass
try:
m = identity_matrix()
m[:3,:3] = orient['matrix']
return quaternion_from_matrix(m)
except KeyError:
pass
# get data
data = self.mt.read_measurement()
# common header
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
# get data (None if not present)
temp = data.get('Temp') # float
raw_data = data.get('RAW')
imu_data = data.get('Calib')
orient_data = data.get('Orient')
velocity_data = data.get('Vel')
position_data = data.get('Pos')
rawgps_data = data.get('RAWGPS')
status = data.get('Stat') # int
# create messages and default values
imu_msg = Imu()
imu_msg.orientation_covariance = (-1., )*9
imu_msg.angular_velocity_covariance = (-1., )*9
imu_msg.linear_acceleration_covariance = (-1., )*9
pub_imu = False
gps_msg = NavSatFix()
xgps_msg = GPSFix()
pub_gps = False
vel_msg = TwistStamped()
pub_vel = False
mag_msg = Vector3Stamped()
pub_mag = False
temp_msg = Float32()
pub_temp = False
# fill information where it's due
# start by raw information that can be overriden
if raw_data: # TODO warn about data not calibrated
pub_imu = True
pub_vel = True
pub_mag = True
pub_temp = True
# acceleration
imu_msg.linear_acceleration.x = raw_data['accX']
imu_msg.linear_acceleration.y = raw_data['accY']
imu_msg.linear_acceleration.z = raw_data['accZ']
imu_msg.linear_acceleration_covariance = (0., )*9
# gyroscopes
imu_msg.angular_velocity.x = raw_data['gyrX']
imu_msg.angular_velocity.y = raw_data['gyrY']
imu_msg.angular_velocity.z = raw_data['gyrZ']
imu_msg.angular_velocity_covariance = (0., )*9
vel_msg.twist.angular.x = raw_data['gyrX']
vel_msg.twist.angular.y = raw_data['gyrY']
vel_msg.twist.angular.z = raw_data['gyrZ']
# magnetometer
mag_msg.vector.x = raw_data['magX']
mag_msg.vector.y = raw_data['magY']
mag_msg.vector.z = raw_data['magZ']
# temperature
# 2-complement decoding and 1/256 resolution
x = raw_data['temp']
if x&0x8000:
temp_msg.data = (x - 1<<16)/256.
else:
temp_msg.data = x/256.
if rawgps_data:
if rawgps_data['bGPS']<self.old_bGPS:
pub_gps = True
# LLA
xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7
xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7
xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3
# NED vel # TODO?
# Accuracy
# 2 is there to go from std_dev to 95% interval
xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3
xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3
self.old_bGPS = rawgps_data['bGPS']
if temp is not None:
pub_temp = True
temp_msg.data = temp
if imu_data:
try:
x = imu_data['gyrX']
y = imu_data['gyrY']
z = imu_data['gyrZ']
v = numpy.array([x, y, z])
v = v.dot(self.R)
imu_msg.angular_velocity.x = v[0]
imu_msg.angular_velocity.y = v[1]
imu_msg.angular_velocity.z = v[2]
imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0.,
radians(0.025), 0., 0., 0., radians(0.025))
pub_imu = True
vel_msg.twist.angular.x = v[0]
vel_msg.twist.angular.y = v[1]
vel_msg.twist.angular.z = v[2]
pub_vel = True
except KeyError:
pass
try:
x = imu_data['accX']
y = imu_data['accY']
z = imu_data['accZ']
v = numpy.array([x, y, z])
v = v.dot(self.R)
imu_msg.linear_acceleration.x = v[0]
imu_msg.linear_acceleration.y = v[1]
imu_msg.linear_acceleration.z = v[2]
imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0., 0.0004)
pub_imu = True
except KeyError:
pass
try:
x = imu_data['magX']
y = imu_data['magY']
z = imu_data['magZ']
v = numpy.array([x, y, z])
v = v.dot(self.R)
mag_msg.vector.x = v[0]
mag_msg.vector.y = v[1]
mag_msg.vector.z = v[2]
pub_mag = True
except KeyError:
pass
if velocity_data:
pub_vel = True
vel_msg.twist.linear.x = velocity_data['Vel_X']
vel_msg.twist.linear.y = velocity_data['Vel_Y']
vel_msg.twist.linear.z = velocity_data['Vel_Z']
if orient_data:
pub_imu = True
orient_quat = quat_from_orient(orient_data)
imu_msg.orientation.x = orient_quat[0]
imu_msg.orientation.y = orient_quat[1]
imu_msg.orientation.z = orient_quat[2]
imu_msg.orientation.w = orient_quat[3]
imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0., radians(9.))
if position_data:
pub_gps = True
xgps_msg.latitude = gps_msg.latitude = position_data['Lat']
xgps_msg.longitude = gps_msg.longitude = position_data['Lon']
xgps_msg.altitude = gps_msg.altitude = position_data['Alt']
if status is not None:
if status & 0b0001:
self.stest_stat.level = DiagnosticStatus.OK
self.stest_stat.message = "Ok"
else:
self.stest_stat.level = DiagnosticStatus.ERROR
self.stest_stat.message = "Failed"
if status & 0b0010:
self.xkf_stat.level = DiagnosticStatus.OK
self.xkf_stat.message = "Valid"
else:
self.xkf_stat.level = DiagnosticStatus.WARN
self.xkf_stat.message = "Invalid"
if status & 0b0100:
self.gps_stat.level = DiagnosticStatus.OK
self.gps_stat.message = "Ok"
else:
self.gps_stat.level = DiagnosticStatus.WARN
self.gps_stat.message = "No fix"
self.diag_msg.header = h
self.diag_pub.publish(self.diag_msg)
if pub_gps:
if status & 0b0100:
gps_msg.status.status = NavSatStatus.STATUS_FIX
xgps_msg.status.status = GPSStatus.STATUS_FIX
gps_msg.status.service = NavSatStatus.SERVICE_GPS
xgps_msg.status.position_source = 0b01101001
xgps_msg.status.motion_source = 0b01101010
xgps_msg.status.orientation_source = 0b01101010
else:
gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
xgps_msg.status.status = GPSStatus.STATUS_NO_FIX
gps_msg.status.service = 0
xgps_msg.status.position_source = 0b01101000
xgps_msg.status.motion_source = 0b01101000
xgps_msg.status.orientation_source = 0b01101000
# publish available information
if pub_imu:
imu_msg.header = h
self.imu_pub.publish(imu_msg)
if pub_gps:
xgps_msg.header = gps_msg.header = h
self.gps_pub.publish(gps_msg)
self.xgps_pub.publish(xgps_msg)
if pub_vel:
vel_msg.header = h
self.vel_pub.publish(vel_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
if pub_temp:
self.temp_pub.publish(temp_msg)
#!/usr/bin/python
import rospy
import moveit_python
from moveit_msgs.msg import CollisionObject
from geometry_msgs.msg import Pose, Point, Quaternion
from shape_msgs.msg import SolidPrimitive
from std_msgs.msg import Header
rospy.init_node("moveit_table_scene")
scene = moveit_python.PlanningSceneInterface("base_link")
#pub = rospy.Publisher("/move_group/monitored_planning_scene", CollisionObject, queue_size=1)
table = CollisionObject()
# Header
h = Header()
h.stamp = rospy.Time.now()
# Shape
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = [0.808, 1.616, 2.424]
# Pose
position = Point(*[1.44, 0.0, 0.03])
orient = Quaternion(*[-0.510232, 0.49503, 0.515101, 0.478832])
# Create Collision Object
scene.addSolidPrimitive("table", box, Pose(*[position, orient]))
timestampFormat = "{:0>4d}-{:0>2d}-{:0>2d} {:0>2d}:{:0>2d}:{:0>2d}".format(
years, mouth, day, hour, minutes, second)
# print("timestamp format",timestampFormat)
timeArray = time.strptime(timestampFormat, "%Y-%m-%d %H:%M:%S")
timeStamp = int(time.mktime(timeArray))
# timeStamp = timeStamp*1000 + mils
# print("timeStamp:",timeStamp)
data_array = data[20:]
image_size = image_height * image_width * 2
if len(data_array) != image_size:
print("error size:", len(data_array))
continue
# print("ok size:",len(data_array))
#publish temp data
image_temp = Image()
header = Header()
header.stamp.secs = timeStamp
header.stamp.nsecs = mils * 1000000
image_temp.header = header
image_temp.height = image_height
image_temp.width = image_width
image_temp.encoding = 'rgb8'
image_temp.data = str(data_array)
temp_pub.publish(image_temp)
else:
no_response = no_response + 1
if no_response > 600:
print(["no_response:", no_response])
no_response = 0
break
rate.sleep()
def crane_solver(self, data):
P1 = [0,0,0]
P2 = [0,0,0]
c = 1.484 # speed of sound in 20 C water per uSec
#c = 0.343 #speed of sound in air
hydro = rospy.ServiceProxy('hydrophones/hydrophone_position', Hydrophone_locations_service)
holder = hydro()
self.hydro0 = holder.hydro0_xyz
self.hydro1 = holder.hydro1_xyz
self.hydro2 = holder.hydro2_xyz
self.hydro3 = holder.hydro3_xyz
#position of hydrophones
x1 = self.hydro1[0] #+ 0.75 #np.random.uniform(-1, 1) #in mm
x2 = self.hydro2[0] #- 0.75 #np.random.uniform(-1, 1)
y2 = self.hydro2[1] #+ 0.75 #np.random.uniform(-1, 1)
x3 = self.hydro3[0] #- 0.75 #np.random.uniform(-1, 1)
y3 = self.hydro3[1] #+ 0.75 #np.random.uniform(-1, 1)
z3 = self.hydro3[2]
#print "x1: %f" % x1
calc_service = rospy.ServiceProxy('hydrophones/calculated_time_stamps', Calculated_time_stamps_service)
#calc_service = rospy.ServiceProxy('hydrophones/actual_time_stamps', Actual_time_stamps_service)
tstampts = calc_service()
'''calc_service = rospy.ServiceProxy('hydrophones/actual_time_stamps', Actual_time_stamps_service)
tstampts = calc_service()
del1 = (tstampts.actual_time_stamps[1])*c #mm/uSec
del2 = (tstampts.actual_time_stamps[2])*c #mm/uSec
del3 = (tstampts.actual_time_stamps[3])*c #mm/uSec '''
#print tstampts.calculated_time_stamps[1]
#print tstampts.calculated_time_stamps[2]
#print tstampts.calculated_time_stamps[3]
#print tstampts.actual_time_stamps[1]
#print tstampts.actual_time_stamps[2]
#print tstampts.actual_time_stamps[3]
del1 = (tstampts.calculated_time_stamps[1])*c #mm/uSec
del2 = (tstampts.calculated_time_stamps[2])*c #mm/uSec
del3 = (tstampts.calculated_time_stamps[3])*c #mm/uSec
#del1 = (tstampts.actual_time_stamps[1])*c #mm/uSec
#del2 = (tstampts.actual_time_stamps[2])*c #mm/uSec
#del3 = (tstampts.actual_time_stamps[3])*c #mm/uSec
'''#Experiment
x1 = 8.500000
x2 = 5.650000
y2 = 4.750000
x3 = -1.750000
y3 = 3.500000
z3 = 1.750000
del1 = 2.053896
del2 = -1.349004
del3 = -2.817121 '''
#print del1
#print del2
#print del3
print "********"
#print "\n"
#print "del1: %f del2: %f del3: %f" % (del1, del2, del3)
left = x1/del1 if del1 != 0 else 0
right = x2/del2 if del2 != 0 else 0
#print "left = %f B1 = %f right = %f" % (left, B1, right)
A1 = left - right # eqn (12)
B1 = -y2/del2 if del2 != 0 else 0
#print "A1: %f" % A1
catch = (x1*x1-del1*del1)/(2.0*del1) if del1 != 0 else 0
zero = (x2*x2 + y2*y2-del2*del2)/(2.0*del2) if del2 != 0 else 0
D1 = zero - catch
#print "D1: %f" % D1
Q1 = -z3/del3 if del3 != 0 else 0
Q2aleft = (A1*y3)/(B1*del3) if (B1*del3) !=0 else 0
Q2amiddle = x3/del3 if del3 != 0 else 0
Q2aright = x1/del1 if del1 != 0 else 0
Q2a = Q2aleft - Q2amiddle + Q2aright
Q2bleft = (del1*del1-x1*x1)/(2.0*del1) if (2.0*del1) != 0 else 0
Q2bmiddle = (D1*y3)/(B1*del3) if (B1*del3) != 0 else 0
Q2bright = (del3*del3 - x3*x3 - y3*y3 - z3*z3)/(2*del3) if (2*del3) != 0 else 0
Q2b = Q2bleft + Q2bmiddle - Q2bright
R1aleft = (A1*A1)/(B1*B1) if (B1*B1) != 0 else 0
R1aright = (x1*x1)/(del1*del1) if (del1*del1) != 0 else 0
R1a = R1aleft + 1 - R1aright
R1bleft = (x1*x1*x1)/(del1*del1) if (del1*del1) != 0 else 0
R1bright = (2.0*A1*D1)/(B1*B1) if (B1*B1) != 0 else 0
R1b = R1bleft - x1 + R1bright
R1cleft = (D1*D1)/(B1*B1) if (B1*B1) != 0 else 0
R1cright = (x1*x1*x1*x1)/(4.0*del1*del1) if (4.0*del1*del1) != 0 else 0
R1c = (x1*x1)/2 - (del1*del1)/4 + R1cleft - R1cright
AA = Q1*Q1*R1a + Q2a*Q2a
BB = Q1*Q1*R1b + 2.0*Q2a*Q2b
CC = Q1*Q1*R1c + Q2b*Q2b
discr = BB*BB - 4.0*AA*CC ;
if (discr < 0):
#no real solution was found; set garbage values for P1 and P2 and return 0
P1[0] = P1[1] = P1[2] = 0.0
P2[0] = P2[1] = P2[2] = 0.0
else:
P1[0] = (-BB+math.sqrt(discr))/(2.0*AA) if (2.0*AA) != 0 else 0
P2[0] = (-BB-math.sqrt(discr))/(2.0*AA) if (2.0*AA) != 0 else 0
# get corresponding value for y coordinate ; eqn (11)
P1[1] = -(A1*P1[0]+D1)/B1 if B1 != 0 else 0
P2[1] = -(A1*P2[0]+D1)/B1 if B1 != 0 else 0
# get correspoinding value for z coordinate ; eqn (16)
P1[2] = -(Q2a*P1[0]+Q2b)/Q1 if Q1 != 0 else 0
P2[2] = -(Q2a*P2[0]+Q2b)/Q1 if Q1 != 0 else 0
#print "x: %f, y: %f, z: %f" % (x,y,z)
P1sum = abs(P1[0]) + abs(P1[1]) + abs(P1[2])
P2sum = abs(P2[0]) + abs(P2[1]) + abs(P2[2])
if P1sum > P2sum:
x = P1[0]
y = P1[1]
z = P1[2]
rospy.loginfo("x1: %f" % (P1[0]))
rospy.loginfo("y1: %f" % (P1[1]))
rospy.loginfo("z1: %f" % (P1[2]))
else:
x = P2[0]
y = P2[1]
z = P2[2]
rospy.loginfo("x2: %f" % (P2[0]))
rospy.loginfo("y2: %f" % (P2[1]))
rospy.loginfo("z2: %f" % (P2[2]))
self.crane_pub = rospy.Publisher('hydrophones/crane_pos', Crane_pos, queue_size = 1)
self.crane_pub.publish(Crane_pos(
header=Header(stamp=rospy.Time.now(),
frame_id='Crane_pos_calc'),
x_pos=x,
y_pos=y,
z_pos=z))
return Crane_pos_serviceResponse(x, y, z)
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not (self.initialized):
# wait for initialization to complete
return
if not (self.tf_listener.canTransform(
self.base_frame, msg.header.frame_id, msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame,
msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
p = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not (self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (len(self.current_odom_xy_theta) < 3
or math.fabs(new_odom_xy_theta[0] -
self.current_odom_xy_theta[0]) > self.d_thresh
or math.fabs(new_odom_xy_theta[1] -
self.current_odom_xy_theta[1]) > self.d_thresh
or math.fabs(new_odom_xy_theta[2] -
self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
if self.last_projected_stable_scan:
last_projected_scan_timeshift = deepcopy(
self.last_projected_stable_scan)
last_projected_scan_timeshift.header.stamp = msg.header.stamp
self.scan_in_base_link = self.tf_listener.transformPointCloud(
"base_link", last_projected_scan_timeshift)
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose() # update robot's pose
self.resample_particles(
) # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(
msg
) # update map to odom transform now that we have new particles
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
positions.append(0.0)
joints.append('head_j2')
positions.append(0.0)
joints.append('head_j3')
positions.append(0.0)
joints.append('head_j4')
positions.append(0.0)
joints.append('head_j5')
positions.append(0.0)
joints.append('head_j6')
positions.append(0.0)
header = Header(0,rospy.Time.now(),'0')
pub.publish(JointState(header, joints, positions, [0]*len(positions), [0]*len(positions)))
try:
while not rospy.is_shutdown():
time.sleep(1.0)
header = Header(0,rospy.Time.now(),'0')
pub.publish(JointState(header, joints, positions, [0]*len(positions), [0]*len(positions)))
except (KeyboardInterrupt,EOFError,rospy.ROSInterruptException):
pass
def Stamped(obj, stamp, frame_id):
obj.header = Header(frame_id=frame_id, stamp=stamp)
return obj
def _update_viz_core(self):
"""Updates visualization after a change."""
# Create a new IM control.
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
# Multiplex marker types added based on action step type.
if self.action_step.type == ActionStep.ARM_TARGET:
# Handle "normal" steps (saved poses).
menu_control = self._make_gripper_marker(menu_control,
self._is_hand_open())
elif self.action_step.type == ActionStep.ARM_TRAJECTORY:
# Handle trajectories.
# First, get all trajectory positions.
point_list = []
for j in range(len(self.action_step.armTrajectory.timing)):
point_list.append(self._get_traj_pose(j).position)
# Add a main maker for all points in the trajectory (sphere
# list).
# NOTE(jstn): This will not work anymore, trajectories are
# unsupported for now.
menu_control.markers.append(
Marker(type=Marker.SPHERE_LIST,
id=self.get_uid(),
lifetime=TRAJ_MARKER_LIFETIME,
scale=SCALE_TRAJ_STEP_SPHERES,
header=Header(frame_id=frame_id),
color=COLOR_TRAJ_STEP_SPHERES,
points=point_list))
# Add a marker for the first step in the trajectory.
menu_control.markers.append(
ActionStepMarker._make_sphere_marker(
self.get_uid() + ID_OFFSET_TRAJ_FIRST,
self._get_traj_pose(0), frame_id, TRAJ_ENDPOINT_SCALE))
# Add a marker for the last step in the trajectory.
last_index = len(self.action_step.armTrajectory.timing) - 1
menu_control.markers.append(
ActionStepMarker._make_sphere_marker(
self.get_uid() + ID_OFFSET_TRAJ_LAST,
self._get_traj_pose(last_index), frame_id,
TRAJ_ENDPOINT_SCALE))
else:
# Neither "normal" pose nor trajectory; error...
rospy.logerr('Non-handled action step type ' +
str(self.action_step.type))
# Add an arrow to the relative object, if there is one.
base_pose = world.get_absolute_pose(self.get_target())
# base_pose refers to the wrist link, which looks weird in
# visualizations, so offset_pose is shifted forward to be in the
# "middle" of the gripper
offset_pose = ActionStepMarker._offset_pose(base_pose, 1)
ref_frame = world.get_ref_from_name(self._get_ref_name())
if ref_frame == ArmState.OBJECT:
menu_control.markers.append(
Marker(type=Marker.ARROW,
id=(ID_OFFSET_REF_ARROW + self.get_uid()),
scale=SCALE_OBJ_REF_ARROW,
header=Header(frame_id=BASE_LINK),
color=COLOR_OBJ_REF_ARROW,
points=[
offset_pose.position,
self.get_target().refFrameLandmark.pose.position
]))
# Make and add the text for this step ('Step X').
text_pos = Point()
text_pos.x = offset_pose.position.x
text_pos.y = offset_pose.position.y
text_pos.z = offset_pose.position.z + TEXT_Z_OFFSET
menu_control.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=self.get_uid(),
scale=SCALE_STEP_TEXT,
text='Step ' + str(self.step_number),
color=COLOR_STEP_TEXT,
header=Header(frame_id=BASE_LINK),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
# Make and add interactive marker.
int_marker = InteractiveMarker()
int_marker.name = self._get_name()
int_marker.header.frame_id = BASE_LINK
int_marker.pose = offset_pose
int_marker.scale = INT_MARKER_SCALE
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(menu_control)
ActionStepMarker._im_server.insert(int_marker, self.marker_feedback_cb)
def both_arms_go_to_pose_goal(self):
# 设置动作对象变量,此处为both_arms
both_arms = self.both_arms
# 获取当前各轴转角
axis_angle = both_arms.get_current_joint_values()
# print axis_angle
# 获取当前末端执行器位置姿态
right_pose_goal = both_arms.get_current_pose(end_effector_link="gripper_r_finger_r")
left_pose_goal = both_arms.get_current_pose(end_effector_link="gripper_l_finger_r")
print right_pose_goal
# 限制末端夹具运动
right_joint_const = JointConstraint()
right_joint_const.joint_name = "gripper_r_joint_r"
if Rightfinger > -55 :
right_joint_const.position = 0.024
else:
right_joint_const.position = 0
left_joint_const = JointConstraint()
left_joint_const.joint_name = "gripper_l_joint_r"
if Leftfinger > -32 :
left_joint_const.position = 0.024
else:
left_joint_const.position = 0
# 添加末端姿态约束:
right_orientation_const = OrientationConstraint()
right_orientation_const.header = Header()
right_orientation_const.orientation = right_pose_goal.pose.orientation
right_orientation_const.link_name = "gripper_r_joint_r"
right_orientation_const.absolute_x_axis_tolerance = 0.6
right_orientation_const.absolute_y_axis_tolerance = 0.6
right_orientation_const.absolute_z_axis_tolerance = 0.6
right_orientation_const.weight = 1
left_orientation_const = OrientationConstraint()
left_orientation_const.header = Header()
left_orientation_const.orientation = left_pose_goal.pose.orientation
left_orientation_const.link_name = "gripper_l_joint_r"
left_orientation_const.absolute_x_axis_tolerance = 0.6
left_orientation_const.absolute_y_axis_tolerance = 0.6
left_orientation_const.absolute_z_axis_tolerance = 0.6
left_orientation_const.weight = 1
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const, left_joint_const]
# consts.orientation_constraints = [right_orientation_const, left_orientation_const]
both_arms.set_path_constraints(consts)
# # 设置动作对象目标位置姿态
# # 右臂
# right_pose_goal.pose.orientation.x = Right_Qux
# right_pose_goal.pose.orientation.y = Right_Quy
# right_pose_goal.pose.orientation.z = Right_Quz
# right_pose_goal.pose.orientation.w = Right_Quw
# right_pose_goal.pose.position.x = Neurondata[5]
# right_pose_goal.pose.position.y = Neurondata[3]
# right_pose_goal.pose.position.z = Neurondata[4]
# # 左臂
# left_pose_goal.pose.orientation.x = Left_Qux
# left_pose_goal.pose.orientation.y = Left_Quy
# left_pose_goal.pose.orientation.z = Left_Quz
# left_pose_goal.pose.orientation.w = Left_Quw
# left_pose_goal.pose.position.x = Neurondata[11]
# left_pose_goal.pose.position.y = Neurondata[9]
# left_pose_goal.pose.position.z = Neurondata[10]
# # 右臂
# right_pose_goal.pose.orientation.x = Right_Qux
# right_pose_goal.pose.orientation.y = Right_Quy
# right_pose_goal.pose.orientation.z = Right_Quz
# right_pose_goal.pose.orientation.w = Right_Quw
# right_pose_goal.pose.position.x = (1266/740)*(Neurondata[5]+0.28)-0.495
# right_pose_goal.pose.position.y = (1295/780)*(Neurondata[3]+0.56)-0.754
# right_pose_goal.pose.position.z = (1355/776)*(Neurondata[4]-0.054)-0.184
# # 左臂
# left_pose_goal.pose.orientation.x = Left_Qux
# left_pose_goal.pose.orientation.y = Left_Quy
# left_pose_goal.pose.orientation.z = Left_Quz
# left_pose_goal.pose.orientation.w = Left_Quw
# left_pose_goal.pose.position.x = (1266/850)*(Neurondata[11]+0.33)-0.495
# left_pose_goal.pose.position.y = (1295/720)*(Neurondata[9]+0.19)-0.541
# left_pose_goal.pose.position.z = (1355/745)*(Neurondata[10]-0.055)-0.184
# 右臂
right_pose_goal.pose.orientation.x = Right_Qux
right_pose_goal.pose.orientation.y = Right_Quy
right_pose_goal.pose.orientation.z = Right_Quz
right_pose_goal.pose.orientation.w = Right_Quw
right_pose_goal.pose.position.x = (Neurondata[5]-0.05)*1.48+0.053
right_pose_goal.pose.position.y = (Neurondata[3]+0.18)*1.48-0.12
right_pose_goal.pose.position.z = (Neurondata[4]-0.53)*1.48+0.47
# 左臂
left_pose_goal.pose.orientation.x = Left_Qux
left_pose_goal.pose.orientation.y = Left_Quy
left_pose_goal.pose.orientation.z = Left_Quz
left_pose_goal.pose.orientation.w = Left_Quw
left_pose_goal.pose.position.x = (Neurondata[11]-0.05)*1.48+0.053
left_pose_goal.pose.position.y = (Neurondata[9]-0.18)*1.48+0.12
left_pose_goal.pose.position.z = (Neurondata[10]-0.53)*1.48+0.47
# # 右臂
# right_pose_goal.pose.orientation.x = right_pose_goal.pose.orientation.x
# right_pose_goal.pose.orientation.y = right_pose_goal.pose.orientation.y
# right_pose_goal.pose.orientation.z = right_pose_goal.pose.orientation.z
# right_pose_goal.pose.orientation.w = right_pose_goal.pose.orientation.w
# right_pose_goal.pose.position.x = right_pose_goal.pose.position.x
# right_pose_goal.pose.position.y = right_pose_goal.pose.position.y
# right_pose_goal.pose.position.z = right_pose_goal.pose.position.z
# # 左臂
# left_pose_goal.pose.orientation.x = left_pose_goal.pose.orientation.x
# left_pose_goal.pose.orientation.y = left_pose_goal.pose.orientation.y
# left_pose_goal.pose.orientation.z = left_pose_goal.pose.orientation.z
# left_pose_goal.pose.orientation.w = left_pose_goal.pose.orientation.w
# left_pose_goal.pose.position.x = left_pose_goal.pose.position.x
# left_pose_goal.pose.position.y = left_pose_goal.pose.position.y
# left_pose_goal.pose.position.z = left_pose_goal.pose.position.z
# 设置动作组的两个目标点
both_arms.set_pose_target(right_pose_goal, end_effector_link="gripper_r_finger_r")
both_arms.set_pose_target(left_pose_goal, end_effector_link="gripper_l_finger_r")
# 规划和输出动作
traj = both_arms.plan()
both_arms.execute(traj, wait=False)
# # 清除路径约束
both_arms.clear_path_constraints()
# 确保输出停止
both_arms.stop()
def create_point_cloud_message(self, pts):
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = '/world'
cloud_message = pcl2.create_cloud_xyz32(header, pts)
return cloud_message
def run():
global vx_
global vy_
global target_
global poses_
global createkeyboard_
global destroykeyboard_
global exist_
rx = 0
ry = 0
pressed = pygame.key.get_pressed()
for event in pygame.event.get():
# determin if X was clicked, or Ctrl+W or Alt+F4 was used
if event.type == pygame.QUIT:
sys.exit(0)
elif event.type == pygame.KEYDOWN:
# switch human control
if event.key == pygame.K_f:
target_ += 1
if target_ == len(exist_):
target_ = 0
elif event.key == pygame.K_j:
target_ -= 1
if target_ == -1:
target_ = len(exist_) - 1
# look control
if event.key == pygame.K_e:
exist_[target_] = not exist_[target_]
# face direction control
if event.key == pygame.K_w:
vy_ = 0.05
ry += 1
pass
if event.key == pygame.K_a:
vx_ = -0.05
rx -= 1
pass
if event.key == pygame.K_s:
vy_ = -0.05
ry -= 1
pass
if event.key == pygame.K_d:
vx_ = 0.05
rx += 1
pass
elif event.type == pygame.KEYUP:
if event.key == pygame.K_w:
vy_ = 0.0
pass
if event.key == pygame.K_a:
vx_ = 0.0
pass
if event.key == pygame.K_s:
vy_ = 0.0
pass
if event.key == pygame.K_d:
vx_ = 0.0
pass
# speech menu control
if createkeyboard_:
createKeyboard()
createkeyboard_ = False
if destroykeyboard_:
cv2.destroyWindow(__keyboard__)
destroykeyboard_ = False
# update
poses_[target_].position.x += vx_
poses_[target_].position.y += vy_
if rx != 0 or ry != 0:
if rx > 0:
if ry == 0:
rx = 1
ry = 0
else:
if ry > 0: # w + d
rx = 0.923879
ry = 0.382683
else: # s + d
rx = -0.923879
ry = 0.382683
elif rx == 0:
rx = 0.707107
if ry > 0:
ry = 0.707107
else:
ry = -0.707107
else:
if ry == 0:
rx = 0
ry = 1
else:
if ry > 0: # w + a
rx = 0.382683
ry = 0.923879
else: # s + a
rx = 0.382683
ry = -0.923879
poses_[target_].orientation = Quaternion(rx, 0, 0, ry)
for i, pose in enumerate(poses_):
if exist_[i]:
pub_detected_person.publish(header=Header(
stamp=rospy.get_rostime(), frame_id='map'),
id=str(i),
position3d_map=pose.position,
position3d_camera=Point(0, 0, 0),
pose3d_camera=Quaternion(1, 0, 0, 0),
map_to_camera=pose)
def _cloud_cb(self, cloud):
points = np.array(list(read_points(cloud)))
if points.shape[0] == 0:
return
pos = points[:,0:3]
cor = np.reshape(points[:,-1], (points.shape[0], 1))
# Get 4x4 matrix which transforms point cloud co-ords to odometry frame
try:
points_to_map = self.tf.asMatrix('/lasths', cloud.header)
except tf.ExtrapolationException:
return
transformed_points = points_to_map.dot(np.vstack((pos.T, np.ones((1, pos.shape[0])))))
transformed_points = transformed_points[:3,:].T
self.seq += 1
header = Header()
header.seq = self.seq
header.stamp = rospy.Time.now()
header.frame_id = '/lasths'
self.cloud = np.vstack((self.cloud, np.hstack((transformed_points, cor))))
if self.seq % 30 == 0:
print "plup!"
self.cloud = np.zeros((0, 4))
output_cloud = create_cloud(header, cloud.fields, self.cloud)
self.cloud_pub.publish(output_cloud)
