def add_invisible_collision_object(self):
co_box = CollisionObject()
co_box.header.frame_id = 'base_footprint'
co_box.id = 'invisible_box'
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box_height = 0.76
box.dimensions.append(0.80)
box.dimensions.append(1.60)
box.dimensions.append(box_height)
co_box.primitives.append(box)
box_pose = Pose()
box_pose.position.x = 0.80
box_pose.position.y = 0.0
box_pose.position.z = box_height / 2.0
box_pose.orientation.w = 1.0
co_box.primitive_poses.append(box_pose)
co_box.operation = CollisionObject.ADD
color = ObjectColor()
color.id = 'invisible_box'
color.color.g = 1.0
color.color.a = 0.15
ps = PlanningScene()
ps.world.collision_objects.append(co_box)
ps.object_colors.append(color)
ps.is_diff = True
try:
self.planning_scene_service(ps)
except rospy.ServiceException, e:
print("Service call failed: %s" % e)
def setColor(self,name,r,g,b,a=0.9): color=ObjectColor() color.id=name color.color.r=r color.color.g=g color.color.b=b color.color.a=a self.colors[name]=color
def _make_color(self, name, rgba):
color = ObjectColor()
color.id = name
color.color.r = rgba[0]
color.color.g = rgba[1]
color.color.b = rgba[2]
color.color.a = rgba[3] if len(rgba) > 3 else 1.0
return color
def setColor(self, name, r, g, b, a = 0.9):
color = ObjectColor()
color.id = name
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
self.colors[name] = color
def set_color(self, name, rgba):
color = ObjectColor()
color.id = name
color.color.r = rgba[0]
color.color.g = rgba[1]
color.color.b = rgba[2]
color.color.a = rgba[3]
self.colors[name] = color
def setColor(name, r, g, b, a):
# Create our color
color = ObjectColor()
color.id = name
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
_colors[name] = color
def setColor(name, color_dict, rgb, a):
# Create our color
color = ObjectColor()
color.id = name
color.color.r = rgb[0]
color.color.g = rgb[1]
color.color.b = rgb[2]
color.color.a = a
color_dict[name] = color
def setColor(self, name, r, g, b, a=0.9):
'''
Set the color of the specified object
'''
color = ObjectColor()
color.id = name
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
self._colors[name] = color
def setColor(self, name, r, g, b, a=0.9):
# 初始化moveit颜色对象
color = ObjectColor()
# 设置颜色值
color.id = name
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# 更新颜色字典
self.colors[name] = color
def setColor(self, name, r, g, b, a=0.9):
# Initialize MoveIt color object
color = ObjectColor()
# Set color value
color.id = name
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update dictionary
self.colors[name] = color
def setColor(self, name, r, g, b, a=0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
def set_object_color(self,id,r,g,b): color = ObjectColor() color.id = str(id) color.color.r = float(r) color.color.g = float(g) color.color.b = float(b) color.color.a = 1.0 self._colors[str(id)] = color self._planningscene.is_diff = True for color in self._colors.values(): self._planningscene.object_colors.append(color) self.scene_pub.publish(self._planningscene)
def set_object_color(self, id, r, g, b):
#defines the color of an object in MoveIt
color = ObjectColor()
color.id = str(id)
color.color.r = float(r)
color.color.g = float(g)
color.color.b = float(b)
color.color.a = 1.0
self._colors[str(id)] = color
self._planningscene.is_diff = True
for color in self._colors.values():
self._planningscene.object_colors.append(color)
self.scene_pub.publish(self._planningscene)
def add_object_mesh(self, object_name, pose, color, frame, file_name):
"""
add object in rviz
:param object_name: name of object
:param pose: pose of object. (xyz) in mm,(abc) in degree
:param color: color of object.(RGBA)
:param frame: reference_frame
:param file_name: mesh file name
:return: None
"""
# Add object
object_pose = PoseStamped()
object_pose.header.frame_id = frame
object_pose.pose.position.x = pose.X / 1000.0
object_pose.pose.position.y = pose.Y / 1000.0
object_pose.pose.position.z = pose.Z / 1000.0
quaternion = quaternion_from_euler(np.deg2rad(pose.A),
np.deg2rad(pose.B),
np.deg2rad(pose.C))
object_pose.pose.orientation.x = quaternion[0]
object_pose.pose.orientation.y = quaternion[1]
object_pose.pose.orientation.z = quaternion[2]
object_pose.pose.orientation.w = quaternion[3]
self.scene.add_mesh(name=object_name,
pose=object_pose,
filename=file_name,
size=(0.001, 0.001, 0.001))
# Add object color
object_color = ObjectColor()
object_color.id = object_name
object_color.color.r = color.R / 255.00
object_color.color.g = color.G / 255.00
object_color.color.b = color.B / 255.00
object_color.color.a = color.A / 255.00
p = PlanningScene()
p.is_diff = True
p.object_colors.append(object_color)
self.scene_pub.publish(p)
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
# Set the color of an object
<<<<<<< HEAD
def setColor(self, name, r, g, b, a=0.9):
=======
def setColor(self, name, r, g, b, a = 0.9):
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
