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 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 _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(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(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(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):
'''
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):
# 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)
def __init__(self, topic, jvalue):
rospy.init_node("robot_state_visualization1")
self.robot_state_pub = rospy.Publisher(topic,
DisplayRobotState,
queue_size=1)
self.robot_state = DisplayRobotState()
#arm_joints = ["lbr4_j0", "lbr4_j1", "lbr4_j2", "lbr4_j3", "lbr4_j4", "lbr4_j5", "lbr4_j6"]
hand_joints = [
"index_joint_0", "index_joint_1", "index_joint_2", "index_joint_3",
"middle_joint_0", "middle_joint_1", "middle_joint_2",
"middle_joint_3", "ring_joint_0", "ring_joint_1", "ring_joint_2",
"ring_joint_3", "thumb_joint_0", "thumb_joint_1", "thumb_joint_2",
"thumb_joint_3"
]
#all_joints = arm_joints + hand_joints
all_joints = hand_joints
links = [
"index_link_0", "index_link_1", "index_link_2", "index_link_3",
"middle_link_0", "middle_link_1", "middle_link_2", "middle_link_3",
"ring_link_0", "ring_link_1", "ring_link_2", "ring_link_3",
"thumb_link_0", "thumb_link_1", "thumb_link_2", "thumb_link_3"
]
self.robot_state.state.joint_state.name = all_joints
self.robot_state.state.joint_state.position = [
jvalue for i in range(len(all_joints))
]
# not in collision
self.robot_state.state.joint_state.position = [
0.571000, 1.058667, 1.809000, 0.354667, -0.594000, -0.296000,
-0.274000, -0.327000, -0.594000, -0.296000, -0.274000, -0.327000,
0.363000, -0.205000, -0.289000, -0.262000
]
# in collision
self.robot_state.state.joint_state.position = [
0.571000, 1.058667, 1.809000, 1.036333, -0.594000, -0.296000,
-0.274000, -0.327000, -0.594000, -0.296000, -0.274000, -0.327000,
0.363000, -0.205000, -0.289000, -0.262000
]
color = get_color('green')
self.robot_state.highlight_links = [
ObjectColor(id=l, color=color) for l in links
]
update_state_service = rospy.Service("update_robot_state",
UpdateRobotState,
self.update_robot_state)
def __init__(self, topic, jvalue):
rospy.init_node("robot_state_visualization")
self.robot_state_pub = rospy.Publisher(topic, DisplayRobotState, queue_size=1)
self.robot_state = DisplayRobotState()
arm_joints = ["lbr4_j0", "lbr4_j1", "lbr4_j2", "lbr4_j3", "lbr4_j4", "lbr4_j5", "lbr4_j6"]
hand_joints = ["index_joint_0", "index_joint_1", "index_joint_2", "index_joint_3",
"middle_joint_0", "middle_joint_1", "middle_joint_2", "middle_joint_3",
"ring_joint_0", "ring_joint_1", "ring_joint_2", "ring_joint_3",
"thumb_joint_0", "thumb_joint_1", "thumb_joint_2", "thumb_joint_3"]
all_joints = arm_joints + hand_joints
#all_joints = hand_joints
links = ["index_link_0", "index_link_1", "index_link_2", "index_link_3",
"middle_link_0", "middle_link_1", "middle_link_2", "middle_link_3",
"ring_link_0", "ring_link_1", "ring_link_2", "ring_link_3",
"thumb_link_0", "thumb_link_1", "thumb_link_2", "thumb_link_3"]
self.robot_state.state.joint_state.name = all_joints
self.robot_state.state.joint_state.position = [jvalue for i in range(len(all_joints))]
color = get_color('palegreen')
self.robot_state.highlight_links = [ObjectColor(id=l, color=color) for l in links]
update_state_service = rospy.Service("update_robot_state", UpdateRobotState, self.update_robot_state)
print("Loading msg_dict...")
loaded_params = msgdict.yaml2msgdict(path_load)
for key in loaded_params.keys():
print(loaded_params[key].__class__)
print("Saving msg_dict...")
msgdict.msgdict2yaml(loaded_params, path_save)
saved_params = msgdict.yaml2msgdict(path_save)
if cmp(loaded_params, saved_params) == 0:
print("Params loaded/saved from yaml are identical")
# Loading/saving Complex messages, responses and requests are supported too
print("Loading/Saving complex messages, responses and requests")
complex_dict = dict()
complex_dict["My_fav_robot_traj"] = RobotTrajectory()
complex_dict["DA_best_colour"] = ObjectColor()
complex_dict["I_always_forget_this_request"] = GetPositionIKRequest()
complex_dict["But I never forget this response"] = ListRobotStatesInWarehouseResponse()
msgdict.msgdict2yaml(complex_dict, path_save_complex)
path_save_complex_params = msgdict.yaml2msgdict(path_save_complex)
if cmp(path_save_complex_params, complex_dict) == 0:
print("Complex dicts are identical")
# Loading saveing msg_dict to param server
print("Saving msg_dict to param server...")
rospy.init_node("test")
msgdict.msgdict2params(complex_dict, "~namespace")
#
print("Loading msg_dict to param server...")
read_params = msgdict.params2msgdict("~namespace")
arm.go()
# Exit MoveIt cleanly
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):