#!/usr/bin/env python3 # stdlib import sys import argparse # 3rd-party import rospkg import rospy from colorama import Fore, Style from std_msgs.msg import Header, ColorRGBA from geometry_msgs.msg import Point from gazebo_msgs.msg import ModelState, ModelStates from geometry_msgs.msg import Point, Pose, Vector3, Quaternion from interactive_markers.interactive_marker_server import * from interactive_markers.menu_handler import * from visualization_msgs.msg import * from atom_core.ros_utils import filterLaunchArguments from gazebo_msgs.srv import SetModelState, GetModelState, GetModelStateRequest, SetModelStateRequest from tf2_ros import TransformBroadcaster br = TransformBroadcaster() current_pose = Pose() current_pose.orientation.w = 1 rospy.init_node("interactive_pattern") set_state_service = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) pub = rospy.Publisher('~pattern', MarkerArray, queue_size=0, latch=True) # local packages model_name = 'charuco_800x600' server = None menu_handler = MenuHandler() def makeBox( msg, pose): marker = Marker(header=Header(frame_id="world", stamp=rospy.Time.now()), ns=model_name, id=0, frame_locked=False, type=Marker.MESH_RESOURCE, action=Marker.ADD, lifetime=rospy.Duration(0), pose=pose, scale=Vector3(x=1.0, y=1.0, z=1.0), color=ColorRGBA(r=1, g=1, b=1, a=1)) marker.mesh_resource = 'package://atlascar2_gazebo/models/charuco_800x600/charuco_800x600.dae' marker.mesh_use_embedded_materials = True return marker def makeBoxControl( msg, pose ): control = InteractiveMarkerControl() control.always_visible = True control.markers.append( makeBox(msg, pose) ) msg.controls.append( control ) return control def make6DofMarker(fixed, interaction_mode, pose, show_6dof=False): int_marker = InteractiveMarker() int_marker.header.frame_id = "world" int_marker.pose = pose int_marker.scale = 0.3 int_marker.name = "simple_6dof" int_marker.description = "Simple 6-DOF Control" # insert a box makeBoxControl(int_marker, pose) int_marker.controls[0].interaction_mode = interaction_mode if fixed: int_marker.name += "_fixed" int_marker.description += "\n(fixed orientation)" if interaction_mode != InteractiveMarkerControl.NONE: control_modes_dict = { InteractiveMarkerControl.MOVE_3D: "MOVE_3D", InteractiveMarkerControl.ROTATE_3D: "ROTATE_3D", InteractiveMarkerControl.MOVE_ROTATE_3D: "MOVE_ROTATE_3D"} int_marker.name += "_" + control_modes_dict[interaction_mode] int_marker.description = "3D Control" if show_6dof: int_marker.description += " + 6-DOF controls" int_marker.description += "\n" + control_modes_dict[interaction_mode] if show_6dof: control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) server.insert(int_marker, processFeedback) menu_handler.apply(server, int_marker.name) def processFeedback( feedback ): s = "feedback from marker '" + feedback.marker_name s += "' / control '" + feedback.control_name + "'" if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: rospy.loginfo( s + ": pose changed") print('new pose = ' + str(feedback.pose)) print('feedback = \n' + str(feedback)) global pose pose.position.x = feedback.pose.position.x pose.position.y = feedback.pose.position.y pose.position.z = feedback.pose.position.z pose.orientation.x = feedback.pose.orientation.x pose.orientation.y = feedback.pose.orientation.y pose.orientation.z = feedback.pose.orientation.z pose.orientation.w = feedback.pose.orientation.w req = SetModelStateRequest() # Create an object of type SetModelStateRequest req.model_state.model_name = model_name req.model_state.pose.position.x = pose.position.x req.model_state.pose.position.y = pose.position.y req.model_state.pose.position.z = pose.position.z req.model_state.pose.orientation.x = pose.orientation.x req.model_state.pose.orientation.y = pose.orientation.y req.model_state.pose.orientation.z = pose.orientation.z req.model_state.pose.orientation.w = pose.orientation.w req.model_state.reference_frame = 'world' global set_state_service result = set_state_service(req.model_state) print(result) # state_msg = ModelState() # state_msg.model_name = model_name # state_msg.pose.position.x = pose.position.x # state_msg.pose.position.y = pose.position.y # state_msg.pose.position.z = pose.position.z # state_msg.pose.orientation.x = pose.orientation.x # state_msg.pose.orientation.y = pose.orientation.y # state_msg.pose.orientation.z = pose.orientation.z # state_msg.pose.orientation.w = pose.orientation.w # state_msg.reference_frame = 'world' # print('set_model_state to:\n' + str(state_msg)) # try: # resp = set_state(state_msg) # print(resp) # except rospy.ServiceException: # print("Service call failed: ") # markers = MarkerArray() # m = Marker(header=Header(frame_id="world", stamp=rospy.Time.now()), # ns=model_name, id=0, frame_locked=False, # type=Marker.MESH_RESOURCE, action=Marker.ADD, lifetime=rospy.Duration(0), # pose=Pose(position=Point(x=pose.position.x, y=pose.position.y, z=pose.position.z), # orientation=Quaternion(x=pose.orientation.x, y=pose.orientation.y, z=pose.orientation.z, # w=pose.orientation.w)), # scale=Vector3(x=1.0, y=1.0, z=1.0), # color=ColorRGBA(r=1, g=1, b=1, a=0.5)) # # m.mesh_resource = 'package://danielabot_gazebo/models/charuco_800x600/charuco_800x600.dae' # m.mesh_use_embedded_materials = True # markers.markers.append(m) # pub.publish(markers) # # # from tf.transformations import euler_from_quaternion, quaternion_from_euler # # quat_list = [pose.orientation.x, # pose.orientation.y, # pose.orientation.z, # pose.orientation.w] # (roll, pitch, yaw) = euler_from_quaternion(quat_list) # print('rpy= ' + str(roll) + ' ; ' + str(pitch) + ' ; ' + str(yaw)) # # t = geometry_msgs.msg.TransformStamped() # t.header.stamp = rospy.Time.now() # t.header.frame_id = "world" # t.child_frame_id = frame_id # # t.transform.translation.x = pose.position.x # t.transform.translation.y = pose.position.y # t.transform.translation.z = pose.position.z # t.transform.rotation.x = pose.orientation.x # t.transform.rotation.y = pose.orientation.y # t.transform.rotation.z = pose.orientation.z # t.transform.rotation.w = pose.orientation.w # # br.sendTransform(t) server.applyChanges() def callbackTimer(event): print('Timer called at ' + str(event.current_real)) global pose if __name__ == "__main__": # Parse command line arguments ap = argparse.ArgumentParser(description='Interactive manipulation of pattern pattern in rviz.') # ap.add_argument("-c", "--calibration_file", type=str, required=True, help='full path to calibration file.') args = vars(ap.parse_args(args=filterLaunchArguments(sys.argv))) # create a timer to update the published transforms # rospy.Timer(rospy.Duration(0.01), frameCallback) server = InteractiveMarkerServer("interactive_pattern") rospy.wait_for_service('/gazebo/set_model_state') # Get current charuco gazebo model pose at set the marker's pose accordingly rospy.wait_for_service('/gazebo/get_model_state') get_model_state_service = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) pose_gazebo = get_model_state_service(model_name, 'world') # pose = Pose(position=Point(x=2, y=2, z=1), orientation=Quaternion(x=0, y=0, z=0, w=1)) import copy pose = copy.deepcopy(pose_gazebo.pose) make6DofMarker(True, InteractiveMarkerControl.MOVE_3D, pose_gazebo.pose, True) server.applyChanges() rospy.spin()