def __init__(self):
self.sim_mode = rospy.get_param('simulation_mode', False)
self.publish_joint_angles = rospy.get_param(
'publish_joint_angles', True) # if self.sim_mode else False
self.axis_for_right = float(rospy.get_param(
'~axis_for_right',
0)) # if right calibrates first, this should be 0, else 1
self.wheel_track = float(rospy.get_param(
'~wheel_track', 0.285)) # m, distance between wheel centres
self.tyre_circumference = float(
rospy.get_param('~tyre_circumference', 0.341)
) # used to translate velocity commands in m/s into motor rpm
self.connect_on_startup = rospy.get_param('~connect_on_startup', False)
#self.calibrate_on_startup = rospy.get_param('~calibrate_on_startup', False)
#self.engage_on_startup = rospy.get_param('~engage_on_startup', False)
self.has_preroll = rospy.get_param('~use_preroll', True)
self.publish_current = rospy.get_param('~publish_current', True)
self.publish_raw_odom = rospy.get_param('~publish_raw_odom', True)
self.publish_odom = rospy.get_param('~publish_odom', True)
self.publish_tf = rospy.get_param('~publish_odom_tf', False)
self.odom_topic = rospy.get_param('~odom_topic', "odom")
self.odom_frame = rospy.get_param('~odom_frame', "odom")
self.base_frame = rospy.get_param('~base_frame', "base_link")
self.odom_calc_hz = rospy.get_param('~odom_calc_hz', 25)
rospy.on_shutdown(self.terminate)
rospy.Service('connect_driver', std_srvs.srv.Trigger,
self.connect_driver)
rospy.Service('disconnect_driver', std_srvs.srv.Trigger,
self.disconnect_driver)
rospy.Service('calibrate_motors', std_srvs.srv.Trigger,
self.calibrate_motor)
rospy.Service('calibrate_motors_reverse', std_srvs.srv.Trigger,
self.calibrate_motor_reverse)
rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor)
rospy.Service('release_motors', std_srvs.srv.Trigger,
self.release_motor)
self.command_queue = Queue.Queue(maxsize=5)
self.vel_subscribe = rospy.Subscriber("/cmd_vel",
Twist,
self.cmd_vel_callback,
queue_size=2)
if self.publish_current:
self.current_loop_count = 0
self.left_current_accumulator = 0.0
self.right_current_accumulator = 0.0
self.current_publisher_left = rospy.Publisher(
'odrive/left_current', Float64, queue_size=2)
self.current_publisher_right = rospy.Publisher(
'odrive/right_current', Float64, queue_size=2)
rospy.logdebug("ODrive will publish motor currents.")
self.last_cmd_vel_time = rospy.Time.now()
if self.publish_raw_odom:
self.raw_odom_publisher_encoder_left = rospy.Publisher(
'odrive/raw_odom/encoder_left', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_encoder_right = rospy.Publisher(
'odrive/raw_odom/encoder_right', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_vel_left = rospy.Publisher(
'odrive/raw_odom/velocity_left', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_vel_right = rospy.Publisher(
'odrive/raw_odom/velocity_right', Int32,
queue_size=2) if self.publish_raw_odom else None
if self.publish_odom:
rospy.Service('reset_odometry', std_srvs.srv.Trigger,
self.reset_odometry)
self.old_pos_l = 0
self.old_pos_r = 0
self.odom_publisher = rospy.Publisher(self.odom_topic,
Odometry,
tcp_nodelay=True,
queue_size=2)
# setup message
self.odom_msg = Odometry()
#print(dir(self.odom_msg))
self.odom_msg.header.frame_id = self.odom_frame
self.odom_msg.child_frame_id = self.base_frame
self.odom_msg.pose.pose.position.x = 0.0
self.odom_msg.pose.pose.position.y = 0.0
self.odom_msg.pose.pose.position.z = 0.0 # always on the ground, we hope
self.odom_msg.pose.pose.orientation.x = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.y = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.z = 0.0
self.odom_msg.pose.pose.orientation.w = 1.0
self.odom_msg.twist.twist.linear.x = 0.0
self.odom_msg.twist.twist.linear.y = 0.0 # no sideways
self.odom_msg.twist.twist.linear.z = 0.0 # or upwards... only forward
self.odom_msg.twist.twist.angular.x = 0.0 # or roll
self.odom_msg.twist.twist.angular.y = 0.0 # or pitch... only yaw
self.odom_msg.twist.twist.angular.z = 0.0
# store current location to be updated.
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# setup transform
self.tf_publisher = tf2_ros.TransformBroadcaster()
self.tf_msg = TransformStamped()
self.tf_msg.header.frame_id = self.odom_frame
self.tf_msg.child_frame_id = self.base_frame
self.tf_msg.transform.translation.x = 0.0
self.tf_msg.transform.translation.y = 0.0
self.tf_msg.transform.translation.z = 0.0
self.tf_msg.transform.rotation.x = 0.0
self.tf_msg.transform.rotation.y = 0.0
self.tf_msg.transform.rotation.w = 0.0
self.tf_msg.transform.rotation.z = 1.0
if self.publish_joint_angles:
self.joint_state_publisher = rospy.Publisher(
'/odrive/joint_states', JointState, queue_size=2)
jsm = JointState()
self.joint_state_msg = jsm
#jsm.name.resize(2)
#jsm.position.resize(2)
jsm.name = ['joint_left_wheel', 'joint_right_wheel']
jsm.position = [0.0, 0.0]
def image_detection_results (self, args):
assert args.visp_model is None
from agimus_vision.msg import ImageDetectionResult
self.broadcaster = tf2_ros.TransformBroadcaster()
rospy.Subscriber ("/agimus/vision/detection", ImageDetectionResult,
self.handle_detection_results)
def __init__(self):
self.br = tf2_ros.TransformBroadcaster()
self.mat44_map_odom = tft.identity_matrix()
t_ = threading.Thread(target=self.tf_publish)
t_.start()
#!/usr/bin/env python
import rospy
import tf2_ros as tf2
from geometry_msgs.msg import TransformStamped
from iarc7_msgs.msg import OdometryArray
from nav_msgs.msg import Odometry
if __name__ == '__main__':
rospy.init_node('stupid_roomba')
tf2_broadcaster = tf2.TransformBroadcaster()
pub = rospy.Publisher('/roombas', OdometryArray, queue_size=10)
vx = 0.33
roomba_msg = OdometryArray()
roomba = Odometry()
roomba.header.frame_id = 'map'
roomba.child_frame_id = 'roomba0/base_link'
roomba.pose.pose.orientation.w = 1.0
roomba.twist.twist.linear.x = vx
roomba_msg.data.append(roomba)
tf_msg = TransformStamped()
tf_msg.header.frame_id = 'map'
tf_msg.child_frame_id = 'roomba0/base_link'
tf_msg.transform.rotation.w = 1.0
def __init__(self, dictionnary):
threading.Thread.__init__(self)
self.pose_dict = dictionnary
self.br = tf2_ros.TransformBroadcaster()
self.second_broadcaster = rospy.Publisher(
"/agent_poses", geometry_msgs.msg.TransformStamped, queue_size=1)
def __init__(self):
self.client = rospy.ServiceProxy(name="get_pose", service_class=pose)
self.client.wait_for_service()
self.br = tf2_ros.TransformBroadcaster()
def handle_goal_base_pose(self, msg):
# msg.transforms # Type: array of fiducial_msgs.msg.FiducialTransform
br = tf2_ros.TransformBroadcaster()
t = geometry_msgs.msg.TransformStamped()
fix_tag = None
base_tag = None
for detected_transform in msg.transforms:
t.header.stamp = rospy.Time.now()
t.header.frame_id = "kin1_rgb_optical_frame"
id1 = detected_transform.fiducial_id
if id1 == 35:
self.base_pos = np.array([
detected_transform.transform.translation.x,
detected_transform.transform.translation.y
])
if id1 == 15:
# print detected_transform.translation.x
self.fix_pos = np.array([
detected_transform.transform.translation.x,
detected_transform.transform.translation.y
])
self.distance = self.base_pos - self.fix_pos
# move the base if needed
if not self.moving:
return
# Control Law Gains
v_p_gain = 0.364425 * 0.75 # Speed is max when error is larger than 1meter
v_d_gain = 0.0
w_p_gain = 0.848960 * 0.5 # Angular speed is when error is larger than 1 radian
w_d_gain = 0.0
# marker id's
base_id = 35
goal1_id = 15
goal2_id = 15
P_bg = self.base_goal - self.distance
# Control Law
P_bg = [allowence(n, 0.005) for n in P_bg] # 5cm
# E_bg = [allowence(n, 0.015) for n in E_bg] # 5 degrees = 0.09 radians
V_x = P_bg[0] * v_p_gain
V_y = P_bg[1] * v_p_gain
# no rotation now
# W_z = E_bg[2] * w_p_gain
W_z = 0
# Create Velocity Message to move end effector to Goal
publishControl(V_x, -V_y, W_z)
# add by Ruijie
# rospy.sleep(1)
print(V_x, V_y, W_z)
rospy.sleep(0.3)
publishControl(0, 0, 0)
# Check whether the goal position is reached
if (abs(V_x) <= 0.01 and abs(V_y) <= 0.01 and abs(W_z) <= 0.03):
self.moving = False
def __init__(self):
super(PickAndDropProgram, self).__init__()
# First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('pick_and_drop_program', anonymous=True)
filepath = rospy.get_param('~goal_joint_states')
goal_joint_states = joint_state_filereader.read(filepath)
self.goal_names = ["home", "near_box", "near_drop", "drop"]
for name in self.goal_names:
assert name in goal_joint_states, \
"Joint state name '{}' is not found".format(name)
# Declare suctionpad topics
self.pub_to = rospy.Publisher('toArduino', String, queue_size=100)
self.pub_from = rospy.Publisher('fromArduino', String, queue_size=100)
# Vision config
config = YamlConfig(rospy.get_param('~config'))
# Instantiate a `RobotCommander`_ object. This object is the outer-level interface to
# the robot:
robot = moveit_commander.RobotCommander()
# Instantiate a `PlanningSceneInterface`_ object. This object is an interface
# to the world surrounding the robot:
scene = moveit_commander.PlanningSceneInterface()
# Instantiate a `MoveGroupCommander`_ object. This object is an interface
# to one group of joints. In this case the group is the joints in the UR5
# arm so we set ``group_name = manipulator``. If you are using a different robot,
# you should change this value to the name of your robot arm planning group.
group_name = "manipulator" # See .srdf file to get available group names
group = moveit_commander.MoveGroupCommander(group_name)
# We create a `DisplayTrajectory`_ publisher which is used later to publish
# trajectories for RViz to visualize:
# display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
# moveit_msgs.msg.DisplayTrajectory,
# queue_size=20)
# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
print("============ Reference frame: %s" % planning_frame)
# We can also print(the name of the end-effector link for this group:
eef_link = group.get_end_effector_link()
print("============ End effector: %s" % eef_link)
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print("============ Robot Groups:", robot.get_group_names())
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print("============ Printing robot state")
print(robot.get_current_state())
print("")
# Setup vision sensor
print("============ Setup vision sensor")
# create rgbd sensor
rospy.loginfo('Creating RGBD Sensor')
sensor_cfg = config['sensor_cfg']
sensor_type = sensor_cfg['type']
self.sensor = RgbdSensorFactory.sensor(sensor_type, sensor_cfg)
self.sensor.start()
rospy.loginfo('Sensor Running')
rospy.loginfo('Loading T_camera_world')
tf_camera_world = RigidTransform.load(
rospy.get_param('~world_camera_tf'))
rospy.loginfo('tf_camera_world: {}'.format(tf_camera_world))
# Setup client for grasp pose service
rospy.loginfo('Setup client for grasp pose service')
rospy.wait_for_service('grasping_policy')
self.plan_grasp_client = rospy.ServiceProxy('grasping_policy',
GQCNNGraspPlanner)
self.transform_broadcaster = tf2_ros.TransformBroadcaster()
# Misc variables
self.robot = robot
self.scene = scene
self.group = group
self.group_names = group_names
self.config = config
self.tf_camera_world = tf_camera_world
self.goal_joint_states = goal_joint_states
def __init__(self):
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("image_mono", Image, self.callback)
br = tf2_ros.TransformBroadcaster()
t = geometry_msgs.msg.TransformStamped()
def handle_detector_result(self, msg):
#print("Detector result!!!")
print(msg)
if self.start_detect:
if self.clear_buffer_count > 0:
print("Clearing buffer ...")
self.clear_buffer_count -= 1
return
br = tf2_ros.TransformBroadcaster()
t = geometry_msgs.msg.TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = "camera_rgb_optical_frame"
t.child_frame_id = "object_coordinate"
t.transform.rotation.x = 0
t.transform.rotation.y = 0
t.transform.rotation.z = 0
t.transform.rotation.w = 1
t.transform.translation.x = self.detected_pose[0]
t.transform.translation.y = self.detected_pose[1]
t.transform.translation.z = self.detected_pose[2]
if not self.publishing:
if not self.start_detect:
br.sendTransform(t)
return
if msg.data != self.goal:
br.sendTransform(t)
return
print("Detection succeed!!")
print("Result with (camera coordinate)")
print(msg)
self.result_frame_id = msg.data
self.detected_pose[0] = msg.pose.pose.position.x
self.detected_pose[1] = msg.pose.pose.position.y
self.detected_pose[2] = msg.pose.pose.position.z
self.publishing = True
else:
br.sendTransform(t)
# Publish at least 10 times.
if self.publish_count > 0:
print("Publishing")
self.publish_count = self.publish_count - 1
# rospy.sleep(1)
return
else:
print("Publish done")
self.publish_count = 10
self.publishing = False
self.detect_done = True
self.start_detect = False
self.clear_buffer_count = 5
# self.detected_object.point.x = msg.pose.pose.position.x
# self.detected_object.point.y = msg.pose.pose.position.y
# self.detected_object.point.z = msg.pose.pose.position.z
self.detected_object.point.x = 0
self.detected_object.point.y = 0
self.detected_object.point.z = 0
self.result_orientation = np.array([
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])
def __init__(self):
self.axis_for_right = float(rospy.get_param('~axis_for_right', 0)) # if right calibrates first, this should be 0, else 1
self.wheel_track = float(rospy.get_param('~wheel_track', 0.285)) # m, distance between wheel centres
self.tyre_circumference = float(rospy.get_param('~tyre_circumference', 0.341)) # used to translate velocity commands in m/s into motor rpm
self.doStuffTrue = rospy.get_param('~motor_initialization',True)
self.axis_eng = Bool()
self.axis_eng.data = False
self.prev_axis = False
self.prev_axis_topic = rospy.get_param('~previous_axis', "motor_engage")
self.doStuff = False
self.connect_on_startup = rospy.get_param('~connect_on_startup', True) # Edit by GGC on June 14: Does not automatically connect
self.calibrate_on_startup = rospy.get_param('~calibrate_on_startup', False)####################
self.engage_on_startup = rospy.get_param('~engage_on_startup', False)###################
self.has_preroll = rospy.get_param('~use_preroll', False) # GGC on July 11: PREROLL IS NOT WORKING
# Specifically, when axis1 is put in Encoder Index Search, it throws the error: ERROR_INVALID_STATE
self.publish_current = rospy.get_param('~publish_current', True)
self.publish_raw_odom =rospy.get_param('~publish_raw_odom', True)
self.publish_odom = rospy.get_param('~publish_odom', True)
self.publish_tf = rospy.get_param('~publish_odom_tf', False)
self.odom_topic = rospy.get_param('~odom_topic', "odom")
self.odom_frame = rospy.get_param('~odom_frame', "odom")
self.base_frame = rospy.get_param('~base_frame', "base_link")
self.odom_calc_hz = rospy.get_param('~odom_calc_hz', 100) # Edit by GGC on June 20
self.pos_cmd_topic_name = rospy.get_param('~pos_cmd_topic',"/cmd_pos")
self.hip_cmd_topic1_name = rospy.get_param('~hip_cmd_topic1',"/hip_pos1") #############
self.hip_cmd_topic2_name = rospy.get_param('~hip_cmd_topic2',"/hip_pos2") ################
self.mode = rospy.get_param('~control_mode', "position")
self.lim1low_topic = rospy.get_param('~lim1low_topic', "odrive/odrive1_low_tib")
self.lim1high_topic = rospy.get_param('~lim1high_topic', "odrive/odrive1_high_tib")
self.lim2low_topic = rospy.get_param('~lim2low_topic', "odrive/odrive1_low_fem")
self.lim2high_topic = rospy.get_param('~lim2high_topic', "odrive/odrive1_high_fem")
self.serial_number = rospy.get_param('~odrive_serial', "3363314C3536")
# self.port_nunber = rospy.get_param('~odrive_port', "/dev/ttyACM0")
print(self.mode)
# rospy.on_shutdown(self.terminate)
rospy.Service('connect_driver', std_srvs.srv.Trigger, self.connect_driver)
rospy.Service('disconnect_driver', std_srvs.srv.Trigger, self.disconnect_driver)
rospy.Service('stop_motor', std_srvs.srv.Trigger, self.stop_motor)
rospy.Service('home_encoder', std_srvs.srv.Trigger, self.home_encoder)
rospy.Service('calibrate_motors', std_srvs.srv.Trigger, self.calibrate_motor)
rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor)
rospy.Service('release_motors', std_srvs.srv.Trigger, self.release_motor)
rospy.Service('clear_errors', std_srvs.srv.Trigger, self.clear_errors)
self.command_queue = Queue.Queue(maxsize=5)
# Edit by GGC on June 28: Determine subscribed topic based on control mode
# Edit by GGC on July 4: Changing "is" to "==" allows the if-else block to work properly
if self.mode == "position":
self.pos_subscribe = rospy.Subscriber(self.pos_cmd_topic_name, Pose, self.cmd_pos_callback, queue_size=2)
self.hip1_subscribe = rospy.Subscriber(self.hip_cmd_topic1_name, Pose, self.hip_pos1_callback, queue_size=2)
self.hip2_subscribe = rospy.Subscriber(self.hip_cmd_topic2_name, Pose, self.hip_pos2_callback, queue_size=2)
print("Subscribed to " +str(self.pos_cmd_topic_name))
elif self.mode == "velocity":
self.vel_subscribe = rospy.Subscriber("/cmd_vel", Twist, self.cmd_vel_callback, queue_size=2)
print("Subscribed to /cmd_vel")
else:
# Edit by GGC on July 4:
# Debugging line to see if something went wrong with self.mode assignment...
# ...or the if statement syntax
print("Can't understand you, launch file")
self.lim1low_sub = rospy.Subscriber(self.lim1low_topic ,Bool,self.lim1lowcallback)
self.lim1high_sub = rospy.Subscriber(self.lim1high_topic ,Bool,self.lim1highcallback)
self.lim2low_sub = rospy.Subscriber(self.lim2low_topic ,Bool,self.lim2lowcallback)
self.lim2high_sub = rospy.Subscriber(self.lim2high_topic ,Bool,self.lim2highcallback)
self.prev_axis_sub = rospy.Subscriber(self.prev_axis_topic ,Bool,self.prev_axis_callback)
self.axis_eng_pub = rospy.Publisher('~motor_engage', Bool, queue_size=2)
if self.publish_current:
self.current_loop_count = 0
self.left_current_accumulator = 0.0
self.right_current_accumulator = 0.0
self.current_publisher_left = rospy.Publisher('odrive/left_current', Float64, queue_size=2)
self.current_publisher_right = rospy.Publisher('odrive/right_current', Float64, queue_size=2)
rospy.loginfo("ODrive will publish motor currents.")
self.last_cmd_vel_time = rospy.Time.now()
if self.publish_raw_odom:
self.raw_odom_publisher_encoder_left = rospy.Publisher('odrive/raw_odom/encoder_left', Int32, queue_size=2) if self.publish_raw_odom else None
# Temporary Edit by GGC on June 25: commented this so I could test pos_control with rostopic pub
# REMEMBER TO UNCOMMENT THIS WHEN WE USE THE MOTOR!
self.raw_odom_publisher_encoder_right = rospy.Publisher('odrive/raw_odom/encoder_right', Int32, queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_vel_left = rospy.Publisher('odrive/raw_odom/velocity_left', Int32, queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_vel_right = rospy.Publisher('odrive/raw_odom/velocity_right', Int32, queue_size=2) if self.publish_raw_odom else None
if self.publish_odom:
rospy.Service('reset_odometry', std_srvs.srv.Trigger, self.reset_odometry)
self.old_pos_l = 0
self.old_pos_r = 0
self.odom_publisher = rospy.Publisher(self.odom_topic, Odometry, tcp_nodelay=True, queue_size=2)
# setup message
self.odom_msg = Odometry()
#print(dir(self.odom_msg))
self.odom_msg.header.frame_id = self.odom_frame
self.odom_msg.child_frame_id = self.base_frame
self.odom_msg.pose.pose.position.x = 0.0
self.odom_msg.pose.pose.position.y = 0.0
self.odom_msg.pose.pose.position.z = 0.0 # always on the ground, we hope
self.odom_msg.pose.pose.orientation.x = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.y = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.z = 0.0
self.odom_msg.pose.pose.orientation.w = 1.0 # Edit by GGC on June 14: What is w???
self.odom_msg.twist.twist.linear.x = 0.0
self.odom_msg.twist.twist.linear.y = 0.0 # no sideways
self.odom_msg.twist.twist.linear.z = 0.0 # or upwards... only forward
self.odom_msg.twist.twist.angular.x = 0.0 # or roll
self.odom_msg.twist.twist.angular.y = 0.0 # or pitch... only yaw
self.odom_msg.twist.twist.angular.z = 0.0
self.rollover_l = 0
self.rollover_r = 0
self.real_encoder_l = 0
self.real_encoder_r = 0
# store current location to be updated.
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# setup transform
self.tf_publisher = tf2_ros.TransformBroadcaster()
self.tf_msg = TransformStamped()
self.tf_msg.header.frame_id = self.odom_frame
# self.tf_msg.child_frame_id = self.base_frames
self.tf_msg.transform.translation.x = 0.0
self.tf_msg.transform.translation.y = 0.0
self.tf_msg.transform.translation.z = 0.0
self.tf_msg.transform.rotation.x = 0.0
self.tf_msg.transform.rotation.y = 0.0
self.tf_msg.transform.rotation.w = 0.0
self.tf_msg.transform.rotation.z = 1.0
#Added Nov.5.2019 by SL:
#set up vars to hold limit switch states
self.lim1low = False
self.lim1high = False
self.lim2low = False
self.lim2high = False
self.lim1low_old = False
self.lim1high_old = False
self.lim2low_old = False
self.lim2high_old = False
def __init__(self):
self.axis_for_right = float(rospy.get_param(
'~axis_for_right',
0)) # if right calibrates first, this should be 0, else 1
self.wheel_track = float(rospy.get_param(
'~wheel_track', 0.285)) # m, distance between wheel centres
self.tyre_circumference = float(
rospy.get_param('~tyre_circumference', 0.341)
) # used to translate velocity commands in m/s into motor rpm
self.connect_on_startup = rospy.get_param('~connect_on_startup', False)
self.calibrate_on_startup = rospy.get_param('~calibrate_on_startup',
False)
self.engage_on_startup = rospy.get_param('~engage_on_startup', False)
self.max_speed = rospy.get_param('~max_speed', 0.5)
self.max_angular = rospy.get_param('~max_angular', 1.0)
self.publish_current = rospy.get_param('~publish_current', True)
self.has_preroll = rospy.get_param('~use_preroll', False)
self.publish_current = rospy.get_param('~publish_current', True)
self.publish_odom = rospy.get_param('~publish_odom', True)
self.publish_tf = rospy.get_param('~publish_odom_tf', True)
self.odom_topic = rospy.get_param('~odom_topic', "odom")
self.odom_frame = rospy.get_param('~odom_frame', "odom")
self.base_frame = rospy.get_param('~base_frame', "base_link")
self.odom_calc_hz = rospy.get_param('~odom_calc_hz', 20)
self.publish_joint_state = rospy.get_param('~publish_joint_state',
True)
self.joint_state_topic = rospy.get_param('~joint_state_topic',
"joint_state")
self.Calibrate_Axis = rospy.get_param('~Calibrate_Axis', 1)
self.motor_id = rospy.get_param('~motor_id', "0")
rospy.on_shutdown(self.terminate)
rospy.Service('connect_driver', std_srvs.srv.Trigger,
self.connect_driver)
rospy.Service('disconnect_driver', std_srvs.srv.Trigger,
self.disconnect_driver)
rospy.Service('calibrate_motors', std_srvs.srv.Trigger,
self.calibrate_motor)
rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor)
rospy.Service('release_motors', std_srvs.srv.Trigger,
self.release_motor)
self.vel_subscribe = rospy.Subscriber("/cmd_vel",
Twist,
self.cmd_vel_callback,
queue_size=2)
self.timer = rospy.Timer(
rospy.Duration(0.1),
self.timer_check) # stop motors if no cmd_vel received > 1second
if self.publish_current:
self.current_loop_count = 0
self.left_current_accumulator = 0.0
self.right_current_accumulator = 0.0
self.current_timer = rospy.Timer(
rospy.Duration(0.05), self.timer_current
) # publish motor currents at 10Hz, read at 50Hz
self.current_publisher_left = rospy.Publisher(
'odrive/left_current', Float64, queue_size=2)
self.current_publisher_right = rospy.Publisher(
'odrive/right_current', Float64, queue_size=2)
rospy.loginfo("ODrive will publish motor currents.")
if self.publish_odom:
rospy.Service('reset_odometry', std_srvs.srv.Trigger,
self.reset_odometry)
self.odom_publisher = rospy.Publisher(self.odom_topic,
Odometry,
queue_size=2)
# setup message
self.odom_msg = Odometry()
#print(dir(self.odom_msg))
self.odom_msg.header.frame_id = self.odom_frame
self.odom_msg.child_frame_id = self.base_frame
self.odom_msg.pose.pose.position.z = 0.0 # always on the ground, we hope
self.odom_msg.pose.pose.orientation.x = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.y = 0.0 # always vertical
self.odom_msg.twist.twist.linear.y = 0.0 # no sideways
self.odom_msg.twist.twist.linear.z = 0.0 # or upwards... only forward
self.odom_msg.twist.twist.angular.x = 0.0 # or roll
self.odom_msg.twist.twist.angular.y = 0.0 # or pitch... only yaw
# store current location to be updated.
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# setup transform
self.tf_publisher = tf2_ros.TransformBroadcaster()
self.tf_msg = TransformStamped()
self.tf_msg.header.frame_id = self.odom_frame
self.tf_msg.child_frame_id = self.base_frame
self.tf_msg.transform.translation.z = 0.0
self.tf_msg.transform.rotation.x = 0.0
self.tf_msg.transform.rotation.y = 0.0
self.odom_timer = rospy.Timer(
rospy.Duration(1 / float(self.odom_calc_hz)),
self.timer_odometry)
if self.publish_joint_state:
self.joint_state_publisher = rospy.Publisher(
self.joint_state_topic, JointState, queue_size=2)
# setup message
self.joint_state_msg = JointState()
self.joint_state_msg.name = self.motor_id
self.state_subscriber = rospy.Subscriber(
'motor_states/%s' % self.motor_id, MotorState,
self.state_callback)
# setup message
self.state = MotorState()
self.RADIANS_PER_ENCODER_TICK = 2.0 * 3.1415926 / 4000.0
self.ENCODER_TICKS_PER_RADIAN = 1.0 / self.RADIANS_PER_ENCODER_TICK
#print(dir(self.odom_msg))
# self.joint_state_msg.header.frame_id = self.odom_frame
# self.joint_state_msg.child_frame_id = self.base_frame
if not self.connect_on_startup:
rospy.loginfo("ODrive node started, but not connected.")
return
if not self.connect_driver(None)[0]:
return # Failed to connect
if not self.calibrate_on_startup:
rospy.loginfo("ODrive node started and connected. Not calibrated.")
return
if not self.calibrate_motor(None)[0]:
return
if not self.engage_on_startup:
rospy.loginfo("ODrive connected and configured. Engage to drive.")
return
if not self.engage_motor(None)[0]:
return
rospy.loginfo("ODrive connected and configured. Ready to drive.")
def __init__(self, dictionnary):
threading.Thread.__init__(self)
self.pose_dict = dictionnary
self.br = tf2_ros.TransformBroadcaster()
def ballDetectorCallback(self, msg: BoundingBoxes):
if not self.camera.ready:
return
self.camera.reset_position(timestamp=msg.header.stamp)
detected_robots = 0
def box_union(box1: BoundingBox, box2: BoundingBox) -> BoundingBox:
box_final = box1
box_final.ymin = min(box_final.ymin, box2.ymin)
box_final.xmin = min(box_final.xmin, box2.xmin)
box_final.ymax = max(box_final.ymax, box2.ymax)
box_final.xmax = max(box_final.xmax, box2.xmax)
return box_final
# Ball
final_box = None
for box in msg.bounding_boxes:
if box.Class == "ball":
if final_box == None:
final_box = box
else:
final_box = box_union(final_box, box)
if final_box is not None:
boundingBoxes = [[final_box.xmin, final_box.ymin],
[final_box.xmax, final_box.ymax]]
position = self.camera.calculateBallFromBoundingBoxes(
0.07, boundingBoxes)
if position.get_position()[0] > 0.0:
br = tf2_ros.TransformBroadcaster()
ball_pose = TransformStamped()
ball_pose.header.frame_id = self.robot_name + "/base_camera"
ball_pose.child_frame_id = self.robot_name + "/ball"
ball_pose.header.stamp = msg.header.stamp
ball_pose.header.seq = msg.header.seq
ball_pose.transform.translation.x = position.get_position()[0]
ball_pose.transform.translation.y = position.get_position()[1]
ball_pose.transform.translation.z = 0
ball_pose.transform.rotation.x = 0
ball_pose.transform.rotation.y = 0
ball_pose.transform.rotation.z = 0
ball_pose.transform.rotation.w = 1
br.sendTransform(ball_pose)
# Robots
for box in msg.bounding_boxes:
if box.Class == "robot":
if self.head_motor_1_angle > 0.6:
continue # probably looking at own hands
x_avg = (box.xmin + box.xmax) / 2.
y_avg = (box.ymax + box.ymin) / 2.
y_close = box.ymax
robot_length = abs(box.xmax - box.xmin)
robot_width = abs(box.ymax - box.ymin)
if robot_length <= 0 or robot_width <= 0:
continue
length_to_width_ratio_in_full_view = 68 / 22.
foot_ratio_of_length = 0.2
foot_pixel = box.ymax
robot_standing_up = True
if robot_standing_up:
# Side obstructed
if box.xmax == 640:
# right side obstructed
robot_width = robot_length / length_to_width_ratio_in_full_view
box.xmax = box.xmin + robot_length
elif box.xmin == 0:
# left side obstructed
robot_width = robot_length / length_to_width_ratio_in_full_view
box.xmin = box.xmax - robot_length
# If entire robot in field of view
if box.ymax == 480 and box.ymin == 0:
# Top and bottom both obstructed
# TODO dont know what to do at this moment, assume 50 50 top bottom
desired_robot_length = length_to_width_ratio_in_full_view * robot_width
additional_robot_length = desired_robot_length - robot_length
box.ymax = box.ymax + additional_robot_length / 2
box.ymin = box.ymin - additional_robot_length / 2
elif box.ymax == 480:
# bottom obstructed
robot_length = length_to_width_ratio_in_full_view * robot_width
box.ymax = box.ymin + robot_length
elif box.ymin == 0:
# top obstructed
robot_length = length_to_width_ratio_in_full_view * robot_width
box.ymin = box.ymax - robot_length
foot_pixel = box.ymax - robot_length * foot_ratio_of_length
[floor_center_x, floor_center_y,
_] = self.camera.findFloorCoordinate([x_avg, foot_pixel])
[floor_close_x, floor_close_y,
_] = self.camera.findFloorCoordinate([x_avg, y_close])
camera_pose = self.camera.pose
distance = (
(floor_center_x - camera_pose.get_position()[0])**2 +
(floor_center_y - camera_pose.get_position()[1])**2)**0.5
theta = math.atan2(distance, camera_pose.get_position()[2])
ratio = math.tan(theta)**2
ratio2 = 1 / (1 + ratio)
if 1 < ratio2 < 0:
continue
floor_x = floor_close_x * (1 -
ratio2) + floor_center_x * ratio2
floor_y = floor_close_y * (1 -
ratio2) + floor_center_y * ratio2
if floor_x > 0.0:
br = tf2_ros.TransformBroadcaster()
robot_pose = TransformStamped()
robot_pose.header.frame_id = self.robot_name + "/base_camera"
robot_pose.child_frame_id = self.robot_name + "/detected_robot_" + str(
detected_robots)
robot_pose.header.stamp = msg.header.stamp
robot_pose.header.seq = msg.header.seq
robot_pose.transform.translation.x = floor_x
robot_pose.transform.translation.y = floor_y
robot_pose.transform.translation.z = 0
robot_pose.transform.rotation.x = 0
robot_pose.transform.rotation.y = 0
robot_pose.transform.rotation.z = 0
robot_pose.transform.rotation.w = 1
br.sendTransform(robot_pose)
detected_robots += 1
def simulate():
global dislocation_srv, thrust_newtons, roll, pitch, yaw
rospy.init_node('modrotor_simulator', anonymous=True)
robot_id = rospy.get_param('~robot_id', 'modquad01')
init_x = rospy.get_param('~init_x', 1.)
init_y = rospy.get_param('~init_y', 0.)
init_z = rospy.get_param('~init_z', 0.)
demo_trajectory = rospy.get_param('~demo_trajectory', False)
odom_topic = rospy.get_param('~odom_topic', '/odom') # '/odom2'
# cmd_vel_topic = rospy.get_param('~cmd_vel_topic', '/cmd_vel') # '/cmd_vel2'
# service for dislocate the robot
rospy.Service('dislocate_robot', Dislocation, dislocate)
# TODO read structure and create a service to change it.
structure4 = Structure(ids=['modquad01', 'modquad02'],
xx=[0, params.cage_width, 0, params.cage_width],
yy=[0, 0, params.cage_width, params.cage_width],
motor_failure=[])
structure4fail = Structure(ids=['modquad01', 'modquad02'],
xx=[0, params.cage_width, 0, params.cage_width],
yy=[0, 0, params.cage_width, params.cage_width],
motor_failure=[(1, 0)])
structure1 = Structure(ids=[robot_id], xx=[0], yy=[0])
structure = structure1
# Subscribe to control input
rospy.Subscriber('/' + robot_id + '/cmd_vel', Twist,
control_input_listener)
# Odom publisher
odom_publishers = {
id_robot: rospy.Publisher('/' + id_robot + odom_topic,
Odometry,
queue_size=0)
for id_robot in structure.ids
}
# TF publisher
tf_broadcaster = tf2_ros.TransformBroadcaster()
########### Simulator ##############
loc = [init_x, init_y, init_z]
state_vector = init_state(loc, 0)
freq = 100 # 100hz
rate = rospy.Rate(freq)
t = 0
waypts = np.array([[0, 0, 0], [0, 0.5, 0], [0.5, 0.5, 0], [0.5, 0, 0],
[0, 0, 0]])
traj_vars = min_snap_trajectory(0, 10, None, waypts)
while not rospy.is_shutdown():
rate.sleep()
t += 1. / freq
## Dislocate based on request
state_vector[0] += dislocation_srv[0]
state_vector[1] += dislocation_srv[1]
dislocation_srv = (0., 0.)
# Publish odometry
publish_structure_odometry(structure, state_vector, odom_publishers,
tf_broadcaster)
if demo_trajectory:
# F, M = control_output( state_vector,
# min_snap_trajectory(t % 10, 30, traj_vars), control_handle)
# F, M = control_output( state_vector,
# simple_waypt_trajectory(waypts, t % 10, 30), control_handle)
# F, M = control_output( state_vector,
# circular_trajectory(t % 10, 10), control_handle)
# Overwrite the control input with the demo trajectory
[thrust_newtons, roll, pitch,
yaw] = position_controller(state_vector,
circular_trajectory(t % 10, 10))
# Control output based on crazyflie input
F_single, M_single = attitude_controller(
(thrust_newtons, roll, pitch, yaw), state_vector)
# Control of Moments and thrust
F_structure, M_structure, rotor_forces = modquad_torque_control(
F_single, M_single, structure, motor_sat=False)
# Simulate
state_vector = simulation_step(structure, state_vector, F_structure,
M_structure, 1. / freq)
msg_tf.header.stamp = rospy.Time.now()
msg_tf.transform.translation.x = 0
msg_tf.transform.translation.y = 0
msg_tf.transform.translation.z = 0
quaternion = tf.transformations.quaternion_from_euler(
msg_in.RPY.x / 180.0 * 3.1415, -msg_in.RPY.y / 180.0 * 3.1415,
-msg_in.RPY.z / 180.0 * 3.1415)
msg_tf.transform.rotation.w = quaternion[3]
msg_tf.transform.rotation.x = quaternion[0]
msg_tf.transform.rotation.y = quaternion[1]
msg_tf.transform.rotation.z = quaternion[2]
pub_tf.sendTransform(msg_tf)
if __name__ == '__main__':
global pub_tf
global msg_tf
rospy.init_node('vn_orient')
msg_tf = TransformStamped()
msg_tf.header.frame_id = "world"
msg_tf.child_frame_id = "laser"
pub_tf = tf2_ros.TransformBroadcaster()
rospy.Subscriber("vectornav/ins", ins, subCB)
rospy.spin()
def __init__(self, stateDim, inputDim, dt):
self.namespace = ""
'''
states : [x, y, yaw, vx]
'''
'''
Initial conditions
'''
self.x0 = 0.
self.y0 = 0.
self.yaw0 = 0.
self.vx0 = 0.
'''
Parameters (RC platform)
'''
self.dt = dt
self.max_steer_anlge = 17.75 * np.pi / 180 # rad
self.max_vx = 30.0 / 3.6 # m/s
self.length = 0.257
'''
Variable initialization
'''
self.stateDim = stateDim
self.inputDim = inputDim
self.dynamicsDim = 4
self.states_init = [self.x0, self.y0, self.yaw0, self.vx0]
self.states = self.states_init
self.states_dot = np.zeros(self.stateDim)
self.state_hist = np.zeros([1, (self.stateDim)])
self.state_dot_hist = np.zeros([1, (self.stateDim)])
self.data = np.zeros([1, 2 * self.dynamicsDim + self.inputDim])
self.inputs = np.zeros(self.inputDim)
self.toggle_switch = False
'''
Vehicle dynamics object
'''
self.dynamics = dynamics.Dynamics(stateDim, inputDim, dt)
# self.dynamics.modelType = 1
self.dynamics.max_steer = self.max_steer_anlge
'''
ROS publishers
'''
self.pose_pub = rospy.Publisher('simulation/pose',
PoseStamped,
queue_size=1)
self.bodyOdom_pub = rospy.Publisher(
'simulation/bodyOdom', Odometry,
queue_size=1) # velocities in the body frame.
self.poseOdom_pub = rospy.Publisher(
'simulation/poseOdom', Odometry,
queue_size=1) # velocities in the map frame.
self.input_pub = rospy.Publisher('simulation/inputs',
Joy,
queue_size=1)
self.accel_pub = rospy.Publisher('simulation/acceleration',
Accel,
queue_size=1)
self.br = tf2_ros.TransformBroadcaster()
self.ego_model_marker_pub = rospy.Publisher('simulation/car_marker',
Marker,
queue_size=1)
'''
Variable definitions for autonomous driving
'''
self.auto_mode = True
self.control_sub = rospy.Subscriber('control', AckermannDriveStamped,
self.controlSubCallback)
self.steering = 0
self.throttle = 0
self.steer_angle_to_norm = 1 / self.max_steer_anlge #1/0.25 #30/180*np.pi
self.throttle_to_norm = 1
def broadcast_transform(self):
bf = tf2_ros.TransformBroadcaster()
self.transform.header.stamp = rospy.Time.now()
self.transform.header.frame_id = 'pegasus_map'
self.transform.child_frame_id = 'control_station_validator'
bf.sendTransform(self.transform)
def __init__(self, robot_ip):
self.vel = 0.15
self.acc = 0.5
self.stop_acc = 0.3
self.cmd_velocity_vector = []
self.move_vel = False
self.item_height = 0.11
self.robot_c = rtde_control.RTDEControlInterface(
robot_ip) #urx.Robot(robot_ip, True)
self.robot_r = rtde_receive.RTDEReceiveInterface(
robot_ip) #urx.Robot(robot_ip, True)
self.robot_io = rtde_io.RTDEIOInterface(
robot_ip) #urx.Robot(robot_ip, True)
self.my_tcp = m3d.Transform() # create a matrix for our tool tcp
self.current_TCP = 'TCP'
self.set_TCP('pressure_ft')
# self.robot.set_payload(4.25)
# self.robot.set_gravity([0, 0, 0.15])
time.sleep(0.2)
self.ros_node = rospy.init_node('ur10_node', anonymous=True)
self.pose_publisher = rospy.Publisher('tcp_pose',
PoseStamped,
queue_size=1)
self.velocity_publisher = rospy.Publisher('/dvs/vel',
Twist,
queue_size=1)
self.speed_publisher = rospy.Publisher('/dvs/spd',
Float64,
queue_size=1)
self.cam_pose_publisher = rospy.Publisher('/dvs/pose',
PoseStamped,
queue_size=1)
self.cmd_vel_subs = rospy.Subscriber("ur_cmd_vel",
Twist,
self.move_robot_callback,
queue_size=1)
self.cmd_pose_subs = rospy.Subscriber("ur_cmd_pose", Pose,
self.move_pose_callback)
self.cmd_adjust_pose_subs = rospy.Subscriber("ur_cmd_adjust_pose",
Pose,
self.adjust_pose_callback)
self.rotate_ee_cmd = rospy.Subscriber("ur_rotate_ee", Float64,
self.angle_callback)
self.rotate_ee_cmd = rospy.Subscriber("ur_rotate_ee_x", Float64,
self.angle_callback_x)
self.pressure_movement_subs = rospy.Subscriber("move_pressure_to_cam",
Bool,
self.move_PF_to_cam)
self.pickup_service = rospy.Service("ur_pickup", Empty, self.pick_item)
self.set_tcp_service = rospy.Service("set_TCP", desiredTCP,
self.set_TCP_cb)
self.move_service = rospy.Service('move_ur', moveUR, self.moveUR_cb)
self.move_service = rospy.Service('fire_drill', fireDrill,
self.fire_drill_cb)
self.rate = rospy.Rate(100)
#TF
self.tfBuffer = tf2_ros.Buffer()
self.tl = tf2_ros.TransformListener(self.tfBuffer)
self.br = tf2_ros.TransformBroadcaster()
self.brs = tf2_ros.StaticTransformBroadcaster()
self.robot_pose = PoseStamped()
self.camera_pose = PoseStamped()
self.prev_camera_pose = PoseStamped()
self.pressure_ft_pose = PoseStamped()
self.cam_vel = Twist()
self.cam_speed = Float64()
self.seq = 1
self.pose = []
self.initial_pose = []
self.center_pose = []
self.static_transform_list = []
self.setup_tf()
def __init__(self):
self.__transformBroadcaster = tf.TransformBroadcaster()
self.__trandformDict = {}
self.__trandformTTL = {}
def __init__(self):
self.rate = rospy.get_param("~rate", 20.0)
self.period = 1.0 / self.rate
self.tf_buffer = tf2_ros.Buffer()
self.tf = tf2_ros.TransformListener(self.tf_buffer)
self.br = tf2_ros.TransformBroadcaster()
# get a dict of joints and their link locations
# TODO(lucasw) need to know per wheel radius to compute velocity
# correctly, for now assume all wheels are same radius
self.joints = rospy.get_param("~steered_joints", [{
'link':
'front_left_steer',
'steer_joint':
'front_left_steer_joint',
'steer_topic':
'/carbot/front_left/steer_position_controller/command',
'wheel_joint':
'wheel_front_left_axle',
'wheel_topic':
'/carbot/front_left/wheel_position_controller/command'
}, {
'link':
'front_right_steer',
'steer_joint':
'front_right_steer_joint',
'steer_topic':
'/carbot/front_right/steer_position_controller/command',
'wheel_joint':
'wheel_front_right_axle',
'wheel_topic':
'/carbot/front_right/wheel_position_controller/command'
}, {
'link':
'back_left',
'steer_joint':
None,
'steer_topic':
None,
'wheel_joint':
'wheel_back_left_axle',
'wheel_topic':
'/carbot/back_left/wheel_position_controller/command'
}, {
'link':
'back_right',
'steer_joint':
None,
'steer_topic':
None,
'wheel_joint':
'wheel_back_right_axle',
'wheel_topic':
'/carbot/back_right/wheel_position_controller/command'
}])
self.wheel_radius = rospy.get_param("~wheel_radius", 0.15)
# gazebo joint controller commands
self.command_pub = {}
# use this to store all the positions persistently
self.wheel_joint_states = JointState()
for wheel in self.joints:
steer_topic = wheel['steer_topic']
if steer_topic is not None:
self.command_pub[wheel['steer_joint']] = rospy.Publisher(
steer_topic, Float64, queue_size=4)
wheel_topic = wheel['wheel_topic']
if wheel_topic is not None:
self.command_pub[wheel['wheel_joint']] = rospy.Publisher(
wheel_topic, Float64, queue_size=4)
# TODO(lucasw) read the current angle to initialize
self.wheel_joint_states.name.append(wheel['wheel_joint'])
self.wheel_joint_states.position.append(0.0)
self.wheel_joint_states.velocity.append(0.0)
# TODO(lucasw) initialize the current position from
# another source
self.ts = TransformStamped()
self.ts.header.frame_id = "map"
self.ts.child_frame_id = "base_link"
self.ts.transform.rotation.w = 1.0
# the angle of the base_link
self.angle = 0
# the fixed back axle- all the fixed wheels rotate around the y-axis
# of the back axle
self.back_axle_link = rospy.get_param("~back_axle_link", "back_axle")
# TODO(lucasw) for now assume all the links have no rotation
# with respect to each other, later do this robustly with tf lookups
self.marker = Marker()
self.marker.id = 0
self.marker.type = Marker.LINE_STRIP
self.marker.frame_locked = True
self.marker.action = Marker.ADD
self.marker.header.frame_id = self.back_axle_link
self.marker.scale.x = 0.07
self.marker.scale.y = 0.07
self.marker.scale.z = 0.07
self.marker.color.r = 0.5
self.marker.color.g = 0.5
self.marker.color.b = 0.5
self.marker.color.a = 0.5
self.marker.pose.orientation.w = 1.0
self.marker_pub = rospy.Publisher("marker",
Marker,
queue_size=len(self.joints) * 2)
self.point_pub = rospy.Publisher("spin_center",
PointStamped,
queue_size=1)
# TODO(lucasw) would like there to be a gazebo plugin that takes
# desired joint position and velocity, but I believe there is only
# the simple command inputs that pid to a position or velocity, but not both.
self.joint_pub = rospy.Publisher("steered_joint_states",
JointState,
queue_size=3)
self.lead_steer = rospy.get_param(
"~steer", {
'link': 'lead_steer',
'joint': 'lead_steer_joint',
'wheel_joint': 'wheel_lead_axle'
})
self.twist_pub = rospy.Publisher("odom_cmd_vel", Twist, queue_size=3)
# TODO(lucasw) circular publisher here- the steer command is on
# joint_states, then published onto steered_joint_states, which updates
# joint_states- but the joints are different.
self.joint_sub = rospy.Subscriber("joint_states",
JointState,
self.lead_steer_callback,
queue_size=4)
self.timer = rospy.Timer(rospy.Duration(self.period), self.update)
def test_force_transformer_node(self):
"""
Verifies the node's interface is correct.
Note: this is not a functionality test.
"""
frame_1 = 'f1'
frame_2 = 'f2'
# Note: this should match the one specified in the .test file.
reference_frame = 'object_frame'
force_array_1 = udom_common_msgs.msg.ForceArray()
force_array_1.header.frame_id = frame_1
force_array_1.positions = [geometry_msgs.msg.Point(1.0, 1.0, 1.0)]
force_array_1.wrenches = [
geometry_msgs.msg.Wrench(
force=geometry_msgs.msg.Vector3(1.0, 1.0, 1.0))
]
force_array_2 = udom_common_msgs.msg.ForceArray()
force_array_2.header.frame_id = frame_2
force_array_2.positions = [geometry_msgs.msg.Point(2.0, 2.0, 2.0)]
force_array_2.wrenches = [
geometry_msgs.msg.Wrench(
force=geometry_msgs.msg.Vector3(2.0, 2.0, 2.0))
]
force_in = udom_common_msgs.msg.ForceMultiArray()
force_in.forces = [force_array_1, force_array_2]
# Desired values.
point_1 = geometry_msgs.msg.Point(2.0, 1.0, 1.0)
point_2 = geometry_msgs.msg.Point(4.0, 2.0, 2.0)
desired_positions = [point_1, point_2]
force_1 = geometry_msgs.msg.Vector3(1.0, 1.0, 1.0)
torque_1 = geometry_msgs.msg.Vector3(0.0, -1.0, 1.0)
force_2 = geometry_msgs.msg.Vector3(2.0, 2.0, 2.0)
torque_2 = geometry_msgs.msg.Vector3(0.0, -4.0, 4.0)
wrench_1 = geometry_msgs.msg.Wrench(force_1, torque_1)
wrench_2 = geometry_msgs.msg.Wrench(force_2, torque_2)
desired_wrenches = [wrench_1, wrench_2]
# Set transforms.
t_1 = geometry_msgs.msg.TransformStamped()
t_1.header.frame_id = reference_frame
t_1.child_frame_id = frame_1
t_1.transform.translation.x = 1.0
t_1.transform.translation.y = 0.0
t_1.transform.translation.z = 0.0
t_1.transform.rotation.x = 0.0
t_1.transform.rotation.y = 0.0
t_1.transform.rotation.z = 0.0
t_1.transform.rotation.w = 1.0
t_2 = geometry_msgs.msg.TransformStamped()
t_2.header.frame_id = reference_frame
t_2.child_frame_id = frame_2
t_2.transform.translation.x = 2.0
t_2.transform.translation.y = 0.0
t_2.transform.translation.z = 0.0
t_2.transform.rotation.x = 0.0
t_2.transform.rotation.y = 0.0
t_2.transform.rotation.z = 0.0
t_2.transform.rotation.w = 1.0
broadcaster_1 = tf2_ros.TransformBroadcaster()
broadcaster_2 = tf2_ros.TransformBroadcaster()
while not self.wait_for_result:
t_1.header.stamp = rospy.Time.now()
t_2.header.stamp = rospy.Time.now()
broadcaster_1.sendTransform(t_1)
broadcaster_2.sendTransform(t_2)
self.force_in.publish(force_in)
self.event_out.publish('e_start')
self.assertIsInstance(self.result, udom_common_msgs.msg.ForceArray)
self.assertEqual(self.result.header.frame_id, reference_frame)
self.assertEqual(self.result.positions, desired_positions)
self.assertEqual(self.result.wrenches, desired_wrenches)
def __init__(self):
# Initialize ROS node
rospy.init_node('turtlebot_supervisor', anonymous=True)
self.params = SupervisorParams(verbose=True)
# Food delivery queue
self.orderQueue = Queue.Queue()
# PICUP mode timer
self.pickupTimer = 0
# Current state - todo: woodoo testing
self.x = 3.15
self.y = 1.6
self.theta = 0
# Food object names
self.valid_food_names = {"hot_dog": None, "apple": None, "donut": None}
# For testing phase 2, delete when doing demo
#self.valid_food_names = {"hot_dog": (2.2851, -0.0211), "apple": (0.0804, 0.0722), "donut": (2.061, 2.1991)}
# Explore waypoints list
self.explore_waypoints = [(3.39, 2.78, 1.62), (0.66, 2.77, -3.12), (0.32, 2.22, -2.08), (0.29, 1.65, -2.08),
(0.31, 0.37, -0.06), (2.27, 0.33, -3.0), (2.30, 1.62, 0), (2.27, 0.4, -2.0),
(3.35, 0.30, 1.63), (3.09, 1.38, -1.56)]
self.next_waypoint_index = 0
# Goal state
self.x_g, self.y_g, self.theta_g = self.explore_waypoints[self.next_waypoint_index]
# Current mode
self.mode = Mode.EXPLORE
self.prev_mode = None # For printing purposes
self.tfBroadcaster = tf2_ros.TransformBroadcaster()
########## PUBLISHERS ##########
# Command pose for controller
self.pose_goal_publisher = rospy.Publisher('/cmd_nav', Pose2D, queue_size=10)
# Command vel (used for idling)
self.cmd_vel_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
# Food object marker
self.food_viz_pub = rospy.Publisher("/viz/food_marker", Marker, queue_size=10)
########## SUBSCRIBERS ##########
# Listen to order
rospy.Subscriber('/order', String , self.order_callback)
# Object detector
rospy.Subscriber('/detector/objects', DetectedObjectList, self.object_detected_callback)
# High-level navigation pose
rospy.Subscriber('/nav_pose', Pose2D, self.nav_pose_callback)
# If using gazebo, we have access to perfect state
if self.params.use_gazebo:
rospy.Subscriber('/gazebo/model_states', ModelStates, self.gazebo_callback)
self.trans_listener = tf.TransformListener()
'''
def __init__(self):
rospy.init_node('real_robot_pose', anonymous=True)
self.tf_broadcaster = tf2_ros.TransformBroadcaster()
rospy.Subscriber("/base_pose_ground_truth", Odometry, self.handleBPGT)
def __init__(self, core, position_p_gain):
self.core = core # Reference to the InterbotixRobotXSCore object
self.position_p_gain = position_p_gain # Desired Proportional gain for all servos
self.inc_prev = 0 # Latest increment during the gait cycle
self.period_cntr = 0 # Used to count a period (self.num_steps/2.0) during the wave or ripple gait cycles
self.num_steps = 20.0 # Number of steps in one wave of the first sinusoid cycle
self.step_cntr = 1 # Counts the number of steps during a single gait cycle
self.gait_types = ["tripod", "ripple", "wave"
] # Three supported gaits that can be selected
self.gait_factors = {
"tripod": 2.0,
"ripple": 3.0,
"wave": 6.0
} # Gait factors that modify the first sinusoid function mentioned above based on the selected gait
self.wave_legs = [
"right_front", "left_front", "right_middle", "left_middle",
"right_back", "left_back"
] # Leg 'Queue' when doing the wave gait; after every period, the first element is taken out and appended to the end of the list
self.wave_incs = {
leg: 0
for leg in self.wave_legs
} # Dictionary to keep track of where each leg's foot is during the wave gait
self.ripple_legs = {
"first": ["left_middle", "right_front"],
"second": ["left_back", "right_middle"],
"third": ["left_front", "right_back"]
} # Dictionary to keep track of which two legs move together during the ripple gait
self.ripple_leg_pairs = [
"first", "second", "third"
] # Leg pair 'Queue' when doing the ripple gait; after every period, the first element is taken out and appended to the end of the list
self.ripple_incs = {
pair: 0
for pair in self.ripple_leg_pairs
} # Dictionary to keep track of where each leg pair's feet are during the ripple gait
self.leg_list = [
"left_back", "left_middle", "left_front", "right_front",
"right_middle", "right_back"
] # List of all legs in the hexapod
self.leg_time_map = {
leg: {
"move": 0,
"accel": 0
}
for leg in self.leg_list
} # Keeps track of the moving & accel times for each joint group
self.leg_time_map["all"] = {"move": 0, "accel": 0}
self.leg_mode_on = False # Boolean dictating whether or no 'individual leg control' is on or not
self.foot_points = {
} # Dictionary that contains the current feet positions for each leg
self.home_foot_points = {
} # Dictionary that contains the 'home' feet positions for each leg before starting a gait cycle
self.sleep_foot_points = {
} # Dictionary that contains the 'sleep' feet positions for each leg
self.home_height = 0 # The 'z' component of self.T_fb specifying the height of the 'base_link' frame relative to the 'base_footprint' frame
self.sleep_height = 0 # The 'z' component of self.T_fb specifying the height of the 'base_link' frame relative to the 'base_footprint' frame when sleeping
self.bottom_height = 0 # Height difference between the 'base_link' frame and the 'base_bottom_link' frame
self.T_sf = np.identity(
4
) # Odometry transform specifying the 'base_footprint' frame relative to the 'odom' frame
self.T_fb = np.identity(
4
) # Body transform specifying the 'base_link' frame relative to the 'base_footprint' frame
self.T_bc = {
} # Dictionary containing the static transforms of all six 'coxa_link' frames relative to the 'base_link' frame
self.coxa_length = None # Length [meters] of the coxa_link
self.femur_length = None # Length [meters] of the femur_link
self.tibia_length = None # Length [meters] of the tibia_link
self.femur_offset_angle = None # Offset angle [rad] that makes the tibia_link frame coincident with a line shooting out of the coxa_link frame that's parallel to the ground
self.tibia_offset_angle = None # Offset angle [rad] that makes the foot_link frame coincident with a line shooting out of the coxa_link frame that's parallel to the ground
self.get_urdf_info()
self.pose = PoseStamped(
) # ROS PoseStamped message to publish self.T_sf to its own topic
self.t_sf = TransformStamped(
) # ROS Transform that holds self.T_sf and is published to the /tf topic
self.t_fb = TransformStamped(
) # ROS Transform that holds self.T_fb and is published to the /tf topic
self.br = tf2_ros.TransformBroadcaster()
self.initialize_transforms()
self.info = self.core.srv_get_info("group", "all")
self.info_index_map = dict(
zip(self.info.joint_names, range(len(self.info.joint_names)))
) # Map joint names to their positions in the upper/lower and sleep position arrays
self.hexapod_command = JointGroupCommand(
name="all", cmd=[0] * self.info.num_joints
) # ROS Message to command all 18 joints in the hexapod simultaneously
self.initialize_start_pose()
self.pub_pose = rospy.Publisher(
"/" + self.core.robot_name + "/pose", PoseStamped, queue_size=1
) # ROS Publisher to publish self.T_sf as a PoseStamped message
tmr_transforms = rospy.Timer(
rospy.Duration(0.04), self.publish_states
) # ROS Timer to publish transforms to the /tf and /odom topics at a fixed rate
print("Initialized InterbotixHexapodXSInterface!\n")
def init():
#pub = rospy.Publisher('ar_pose', geometry_msgs, queue_size=10)
rospy.init_node('ar_localization', anonymous=True)
global tfBuffer
global t
global dataT
global indexT
global dataR
global indexR
global maxT
global maxR
maxT = 1
maxR = 1
initialT = [rospy.get_param("~initial_x", 0), rospy.get_param("~initial_y", 0), 0]
dataT = []
for i in range(maxT):
dataT.append(initialT)
indexT = 0
dataR = []
for i in range(maxR):
dataR.append([0,0,0])
indexR = 0
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
t = geometry_msgs.msg.TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = "map"
t.child_frame_id = "odom"
t.transform.translation.x = rospy.get_param("~initial_x", 0)
t.transform.translation.y = rospy.get_param("~initial_y", 0)
t.transform.translation.z = 0
(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w) = (0,0,0,1)
br = tf2_ros.TransformBroadcaster()
try:
tfBuffer.lookup_transform("tagposition_0", "map", rospy.Time(0), rospy.Duration(10))
except:
rospy.logwarn("Unable to find position of Tag 0, no marker transforms published?")
try:
tfBuffer.lookup_transform("base_link", "odom", rospy.Time(0), rospy.Duration(10))
except:
rospy.logwarn("Unable to find odom to base_link, no odometry transforms published?")
rospy.Subscriber("tag_detections", AprilTagDetectionArray, tag_callback)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
t.header.stamp = rospy.Time.now()
tAvg = [0,0,0]
for i in dataT:
tAvg[0] = tAvg[0] + i[0]
tAvg[1] = tAvg[1] + i[1]
tAvg[2] = tAvg[2] + i[2]
tAvg[0] = tAvg[0]/len(dataT)
tAvg[1] = tAvg[1]/len(dataT)
tAvg[2] = tAvg[2]/len(dataT)
t.transform.translation.x = tAvg[0]
t.transform.translation.y = tAvg[1]
t.transform.translation.z = tAvg[2]
rAvg = [0,0,0]
for i in dataR:
#rAvg[0] = rAvg[0] + i[0]
#rAvg[1] = rAvg[1] + i[1]
rAvg[2] = rAvg[2] + i[2]
rAvg[0] = rAvg[0]/len(dataR)
rAvg[1] = rAvg[1]/len(dataR)
rAvg[2] = rAvg[2]/len(dataR)
quat = tf.transformations.quaternion_from_euler(rAvg[0], rAvg[1], rAvg[2])
t.transform.translation.x = tAvg[0]
t.transform.translation.y = tAvg[1]
t.transform.translation.z = tAvg[2]
(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w) = (quat[0],quat[1],quat[2],quat[3])
br.sendTransform(t);
rate.sleep()
def simulate():
global dislocation_srv
rospy.init_node('modrotor_simulator', anonymous=True)
robot_id = rospy.get_param('~robot_id', 'crazy01')
init_x = rospy.get_param('~init_x', 0.)
init_y = rospy.get_param('~init_y', 0.)
init_z = rospy.get_param('~init_z', 0.)
odom_topic = rospy.get_param('~odom_topic', '/odom') # '/odom2'
# cmd_vel_topic = rospy.get_param('~cmd_vel_topic', '/cmd_vel') # '/cmd_vel2'
# viewer = rospy.get_param('~viewer', False)
# service for dislocate the robot
rospy.Service('dislocate_robot', Dislocation, dislocate)
# Subscribe to control input
rospy.Subscriber('/' + robot_id + '/cmd_vel', Twist,
control_input_listenener)
# Odom publisher
odom_pub = rospy.Publisher('/' + robot_id + odom_topic,
Odometry,
queue_size=0)
# TF publisher
tf_broadcaster = tf2_ros.TransformBroadcaster()
########### Simulator ##############
loc = [init_x, init_y, init_z]
x = init_state(loc, 0)
freq = 100 # 100hz
rate = rospy.Rate(freq)
while not rospy.is_shutdown():
rate.sleep()
# Dislocate based on request
x[0] += dislocation_srv[0]
x[1] += dislocation_srv[1]
dislocation_srv = (0., 0.)
# Publish odometry
publish_odom(x, odom_pub)
publish_transform_stamped(robot_id, x, tf_broadcaster)
# Control output
F, M = attitude_controller((thrust_pwm, roll, pitch, yaw), x)
# Derivative of the robot dynamics
f_dot = lambda t1, s: state_derivative(s, F, M)
# Solve the differential equation of motion
r = ode(f_dot).set_integrator('dopri5')
r.set_initial_value(x, 0)
r.integrate(r.t + 1. / freq, step=True)
if not r.successful():
return
x = r.y
# Simulate floor
if x[2] < 0:
x[2] = 0.
# Velocity towards the floor
if x[5] < 0:
x[5] = 0.
def __init__(self):
print("init")
self.model_states_sub = rospy.Subscriber("/gazebo/model_states",
ModelStates,
self.model_states_cb)
self.transform_broadcaster_ = tf2_ros.TransformBroadcaster()
listener = tf2_ros.TransformListener(tfBuffer)
ROBOT_NAME = rospy.get_param(param_name="~robot_name")
tf_request_list = [
(ROBOT_NAME + "/raw/base_link", ROBOT_NAME + "/raw/shelf_car1"),
(ROBOT_NAME + "/raw/base_link", ROBOT_NAME + "/raw/shelf_car2"),
(ROBOT_NAME + "/raw/base_link", ROBOT_NAME + "/raw/home"),
(ROBOT_NAME + "/raw/base_link", ROBOT_NAME + "/raw/A_site"),
(ROBOT_NAME + "/raw/base_link", ROBOT_NAME + "/raw/B_site")
]
#(ROBOT_NAME+"/raw/base_link", ROBOT_NAME+"/raw/marker1"),
#(ROBOT_NAME+"/raw/base_link", ROBOT_NAME+"/raw/marker2"),
#(ROBOT_NAME+"/raw/base_link", ROBOT_NAME+"/raw/marker3")]
br = tf2_ros.TransformBroadcaster()
rate = rospy.Rate(10.0)
trans_last_list = [9999.0, 9999.0, 9999.0, 9999.0, 9999.0]
while not rospy.is_shutdown():
for i in range(len(tf_request_list)):
try:
trans = tfBuffer.lookup_transform(tf_request_list[i][0],
tf_request_list[i][1],
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
else:
# Check this is really a different tf
if abs(trans_last_list[i] -
trans.transform.translation.x) < 0.00000001:
def __init__(self,
cf_id,
radio_uri,
data_only=False,
motion='None',
config='None',
warmup=True):
self._id = cf_id
self._uri = radio_uri
# represents any extra hardware configuration details
# important for capturing weight / config details
if len([a for a in cfg_options if re.compile(a).match(config)]) == 0:
print("[WARNING] Config option not recognized: %s" % config)
self._config = config
self.stop_sig = False
# self.data_imu_freq_timer = FreqTimer('DATA')
# self.data_gyro_freq_timer = FreqTimer('GYRODATA')
self.data_stab_freq_timer = FreqTimer('STABDATA')
self.data_kalman_freq_timer = FreqTimer('KALMDATA')
signal.signal(signal.SIGINT, self.signal_handler)
self.cf_active = False
self.accept_commands = False
self.data_only = data_only
self.motion = prebuilt_motions[motion]
#only first time
self.first_takeoff = True
self.takeoff_warmup = warmup
self.data = None
self.alt = 0
self.to_publish = None
# self._rec_data_imu = False
# self._rec_data_gyro = False
self._rec_data_stab = False
self._rec_data_kalman = False
# self._rec_data_pos = False
# self._rec_data_mag = False
self.cb_lock = threading.Lock()
self.cmd_lock = threading.Lock()
self.cmd_in_use = False
# self.bridge = CvBridge()
cflib.crtp.init_drivers(enable_debug_driver=False)
# try:
# with SyncCrazyflie(self._uri) as scf:
self.cf = CF(rw_cache="./cache")
self.cf.connected.add_callback(self.connected)
self.cf.disconnected.add_callback(self.disconnected)
self.cf.connection_failed.add_callback(self.connection_lost)
self.cf.connection_lost.add_callback(self.connection_failed)
print('Connecting to %s' % radio_uri)
self.cf.open_link(radio_uri)
# self.cf.param.set_value('kalman.resetEstimation', '1')
# time.sleep(0.1)
# self.cf.param.set_value('kalman.resetEstimation', '0')
# time.sleep(1.5)
# except Exception as e:
# print(type(e))
# print("Unable to connect to CF %d at URI %s" % (self._id, self._uri))
# self.scf = None
# self.cf = None
# STATIC PUBLISHERS
self.stat_tf_br = tf.StaticTransformBroadcaster()
sts = TransformStamped()
sts.header.stamp = rospy.Time.now()
sts.header.frame_id = "world"
sts.child_frame_id = "map"
sts.transform.translation.x = 0
sts.transform.translation.y = 0
sts.transform.translation.z = 0
sts.transform.rotation.x = 0
sts.transform.rotation.y = 0
sts.transform.rotation.z = 0
sts.transform.rotation.w = 1
self.stat_tf_br.sendTransform(sts)
# NON STATIC PUBLISHERS
self.tf_br = tf.TransformBroadcaster()
self.data_pub = rospy.Publisher('cf/%d/data' % self._id,
CFData,
queue_size=10)
self.imu_pub = rospy.Publisher('cf/%d/imu' % self._id,
Imu,
queue_size=10)
self.pose_pub = rospy.Publisher('cf/%d/pose' % self._id,
PoseStamped,
queue_size=10)
self.twist_pub = rospy.Publisher('cf/%d/twist' % self._id,
TwistStamped,
queue_size=10)
# self.image_pub = rospy.Publisher('cf/%d/image'%self._id, Image, queue_size=10)
if not self.data_only:
self.cmd_sub = rospy.Subscriber('cf/%d/command' % self._id,
CFCommand, self.command_cb)
self.motion_sub = rospy.Subscriber('cf/%d/motion' % self._id,
CFMotion, self.motion_cb)
