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):