def get_odom_angle(self):
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return "succeeded"
return self.quat_to_angle(Quaternion(*rot))
def __init__(self, name) :
rospy.on_shutdown(self._on_node_shutdown)
self.current_node = "Unknown"
self.nogo_pre_active = False
self.nogo_active = False
self.nogos=[]
self.stop_services=["/enable_motors", "/emergency_stop", "/task_executor/set_execution_status"]
self.activate_services=["/enable_motors", "/reset_motorstop", "/task_executor/set_execution_status"]
#Waiting for Topological Map
self._map_received=False
rospy.Subscriber('/topological_map', TopologicalMap, self.MapCallback)
rospy.loginfo("Waiting for Topological map ...")
while not self._map_received :
pass
rospy.loginfo(" ...done")
self.update_service_list()
print self.av_stop_services, self.av_activate_services
#Subscribing to Localisation Topics
rospy.loginfo("Subscribing to Localisation Topics")
rospy.Subscriber('/current_node', String, self.currentNodeCallback)
rospy.loginfo(" ...done")
self.nogos = self.get_no_go_nodes()
print self.nogos
self._killall_timers=False
t = Timer(1.0, self.timer_callback)
t.start()
rospy.loginfo("All Done ...")
rospy.spin()
def BQObs_thread(self,topic1,topic2,sleeptime,*args):
while True:
t1 = 0
t2 = 0
pubcode, statusMessage, topicList = self.m.getPublishedTopics(self.caller_id, "")
for item in topicList:
if item[0] == topic1:
t1 = 1
if item[0] == topic2:
t2 = 1
if t1 == 0:
t1 = 1
self.pub.publish(Observations(time.time(),['~matched('+topic1[1:len(topic1)]+','+topic2[1:len(topic2)]+')']))
if t2 == 0:
t2 = 1
self.pub.publish(Observations(time.time(),['~matched('+topic1[1:len(topic1)]+','+topic2[1:len(topic2)]+')']))
print('Num1='+str(self.num1)+',Num2'+str(self.num2))
if (self.num1 != 0) & (self.num2 != 0):
data1 = self.sum1/self.num1
data2 = self.sum2/self.num2
self.Trend1 = self.regression1.find(data1,self.curr_t1,self.r11)
self.Trend2 = self.regression2.find(data2,self.curr_t2,self.r12)
rospy.loginfo(self.topic1+'_avg_data='+ str(data1) +','+self.topic1+'Trend='+ str(self.Trend1) +','+self.topic2+'Trend='+str(self.Trend2)+','+self.topic2+'_avg_data='+str(data2))
self.num1 = 0
self.num2 = 0
self.sum1 = 0.0
self.sum2 = 0.0
self.publish_output()
time.sleep(sleeptime) #sleep for a specified amount of time.
def run_test(self):
rate = rospy.Rate(1) #hz
while not rospy.is_shutdown():
rospy.loginfo("Publishing...")
self.point_cloud_generator.generate_b_blocks()
print self.point_cloud_generator.b_tree_diag
rate.sleep()
def __init__(self):
rospy.init_node('actuators_handler')
rospy.loginfo(rospy.get_caller_id() + 'Initializing actuators_handler node')
self.timelast = 0
#Get all parameters from config (rosparam)
name = 'engine'
engine_output_pin = int(rospy.get_param('actuators/' + name + '/output_pin', 1))
engine_board_pin = int(rospy.get_param('actuators/' + name + '/board_pin', 60))
engine_period_us = int(1e6 / float(rospy.get_param('actuators/' + name + '/frequency', 60)))
name = 'steering'
steering_output_pin = int(rospy.get_param('actuators/' + name + '/output_pin', 1))
steering_board_pin = int(rospy.get_param('actuators/' + name + '/board_pin', 62))
steering_period_us = int(1e6 / float(rospy.get_param('actuators/' + name + '/frequency', 60)))
#Initialize PWM
self.dev1 = mraa.Pwm(engine_board_pin)
self.dev1.period_us(engine_period_us)
self.dev1.enable(True)
self.dev1.pulsewidth_us(1500)
self.dev2 = mraa.Pwm(steering_board_pin)
self.dev2.period_us(steering_period_us)
self.dev2.enable(True)
self.dev2.pulsewidth_us(1500)
def load_config_from_param():
# first, make sure parameter server is even loaded
while not rospy.search_param("/joints"):
rospy.loginfo("waiting for parameter server to load with joint definitions")
rospy.sleep(1)
rospy.sleep(1)
joints = rospy.get_param('/joints')
for name in joints:
rospy.loginfo( "found: " + name )
s = Servo()
key = '/joints/' + name + '/'
s.bus = rospy.get_param(key + 'bus')
s.servoPin = rospy.get_param(key + 'servoPin')
s.minPulse = rospy.get_param(key + 'minPulse')
s.maxPulse = rospy.get_param(key + 'maxPulse')
s.minGoal = rospy.get_param(key + 'minGoal')
s.maxGoal = rospy.get_param(key + 'maxGoal')
s.rest = rospy.get_param(key + 'rest')
s.maxSpeed = rospy.get_param(key + 'maxSpeed')
s.smoothing = rospy.get_param(key + 'smoothing')
s.sensorpin = rospy.get_param(key + 'sensorPin')
s.minSensor = rospy.get_param(key + 'minSensor')
s.maxSensor = rospy.get_param(key + 'maxSensor')
servos[name] = s
return servos
def __init__(self, node_name):
ROS2OpenCV2.__init__(self, node_name)
self.match_threshold = rospy.get_param("~match_threshold", 0.7)
self.find_multiple_targets = rospy.get_param("~find_multiple_targets", False)
self.n_pyr = rospy.get_param("~n_pyr", 2)
self.min_template_size = rospy.get_param("~min_template_size", 25)
self.scale_factor = rospy.get_param("~scale_factor", 1.2)
self.scale_and_rotate = rospy.get_param("~scale_and_rotate", False)
self.use_depth_for_detection = rospy.get_param("~use_depth_for_detection", False)
self.fov_width = rospy.get_param("~fov_width", 1.094)
self.fov_height = rospy.get_param("~fov_height", 1.094)
self.max_object_size = rospy.get_param("~max_object_size", 0.28)
# Intialize the detection box
self.detect_box = None
self.detector_loaded = False
rospy.loginfo("Waiting for video topics to become available...")
# Wait until the image topics are ready before starting
rospy.wait_for_message("input_rgb_image", Image)
if self.use_depth_for_detection:
rospy.wait_for_message("input_depth_image", Image)
rospy.loginfo("Ready.")
def __init__(self):
rospy.init_node('Gocrazy', anonymous=False)
self.distance = 0
self.angle = 0
rospy.loginfo("To stop TurtleBot CTRL + C")
# What function to call when you ctrl + c
rospy.on_shutdown(self.shutdown)
# Create a publisher which can "talk" to TurtleBot and tell it to move
# Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if
# you're not using TurtleBot2
self.cmd_vel = rospy.Publisher(
'cmd_vel_mux/input/navi', Twist, queue_size=10)
# TurtleBot will stop if we don't keep telling it to move. How often
# should we tell it to move? 10 HZ
r = rospy.Rate(10)
move_cmd = Twist()
move_cmd.linear.x = self.distance
move_cmd.angular.y = self.angle
while not rospy.is_shutdown():
# publish the velocity
self.cmd_vel.publish(move_cmd)
# wait for 0.1 seconds (10 HZ) and publish again
r.sleep()
def __init__(self, node_name_override = 'imu_node'):
rospy.init_node(node_name_override)
self.nodename = rospy.get_name()
rospy.loginfo("imu_node starting with name %s", self.nodename)
self.rate = rospy.get_param("param_global_rate", 10)
self.init_time = rospy.get_time()
self.roll = 0
self.pitch = 0
self.yaw = 0
self.grad2rad = 3.141592/180.0
self.imuPub = rospy.Publisher('imu_topic', Imu)
self.imuMsg = Imu()
self.imuMsg.orientation_covariance = [999999 , 0 , 0, 0, 9999999, 0, 0, 0, 999999]
self.imuMsg.angular_velocity_covariance = [9999, 0 , 0, 0 , 99999, 0, 0 , 0 , 0.02]
self.imuMsg.linear_acceleration_covariance = [0.2 , 0 , 0, 0 , 0.2, 0, 0 , 0 , 0.2]
self.imuSub = rospy.Subscriber("imu_data", float32_12, self.imuDataCb)
self.yaw = 0.001
self.pitch = 0.001
self.roll = 0.001
self.q = tf.transformations.quaternion_from_euler(self.roll,self.pitch,self.yaw)
self.imuMsg.orientation.x = self.q[0]
self.imuMsg.orientation.y = self.q[1]
self.imuMsg.orientation.z = self.q[2]
self.imuMsg.orientation.w = self.q[3]
self.imuMsg.header.stamp= rospy.Time.now()
self.imuMsg.header.frame_id = 'base_link'
self.imuPub.publish(self.imuMsg)
def delta_move_joint_list(self, value, index=[], interpolate=True):
"""Incremental index move in joint space.
:param value: the incremental amount in which you want to move index by, this is a list
:param index: the joint you want to move, this is a list
:param interpolate: see :ref:`interpolate <interpolate>`"""
rospy.loginfo(rospy.get_caller_id() + ' -> starting delta move joint index')
# check if value is a list
if(self.__check_input_type(value, [list,float])):
initial_joint_position = self.__position_joint_desired
delta_joint = []
delta_joint[:] = initial_joint_position
# give index is not given and the size of the value is 7
if (index == []):
if(self.__check_list_length(value, len(self.__position_joint_desired))):
index = range(len(self.__position_joint_desired))
# is there both an index and a value
else:
# check the length of the delta move
if(self.__check_input_type(index, [list,int]) and len(index) == len(value)):
# make sure it does not exceed the legal joint amount
if(len(index) <= len(initial_joint_position)):
for j in range(len(index)):
if(index[j] < len(initial_joint_position)):
for i in range (len(initial_joint_position)):
if i == index[j]:
delta_joint[i] = initial_joint_position[i] + value[j]
# move accordingly
self.__move_joint(delta_joint, interpolate)
else:
return
def move_joint_list(self, value, index = [], interpolate=True):
"""Absolute index move in joint space.
:param value: the incremental amount in which you want to move index by, this is a list
:param index: the incremental joint you want to move, this is a list
:param interpolate: see :ref:`interpolate <interpolate>`"""
rospy.loginfo(rospy.get_caller_id() + ' -> starting abs move joint index')
# check if value is a list
if(self.__check_input_type(value, [list,float])):
initial_joint_position = self.__position_joint_desired
abs_joint = []
abs_joint[:] = initial_joint_position
# give index is not given and the size of the value is 7
if (index == []):
if(self.__check_list_length(value, len(self.__position_joint_desired))):
index = range(len(self.__position_joint_desired))
# is there both an index and a value
if(self.__check_input_type(index, [list,int]) and len(index) == len(value)):
# if the joint specified exists
if(len(index) <= len(initial_joint_position)):
for j in range(len(index)):
if(index[j] < len(initial_joint_position)):
for i in range (len(initial_joint_position)):
if i == index[j]:
abs_joint[i] = value[j]
self.__move_joint(abs_joint, interpolate)
def __robot_state_callback(self, data):
"""Callback for robot state.
:param data: the current robot state"""
rospy.loginfo(rospy.get_caller_id() + " -> current state is %s", data.data)
self.__robot_state = data.data
self.__robot_state_event.set()
def __goal_reached_callback(self, data):
"""Callback for the goal reached.
:param data: the goal reached"""
rospy.loginfo(rospy.get_caller_id() + " -> goal reached is %s", data.data)
self.__goal_reached = data.data
self.__goal_reached_event.set()
def reconnect(self):
self.socket = socket.socket()
try:
self.socket.connect((self.host, self.port))
except Exception as e:
rospy.loginfo("host: %s port %i error: "%(self.host, self.port) + e.message + str(e))
self.socket = []
def __init__(self, robot_name, ros_namespace = '/dvrk/'):
"""Constructor. This initializes a few data members.It
requires a robot name, this will be used to find the ROS
topics for the robot being controlled. For example if the
user wants `PSM1`, the ROS topics will be from the namespace
`/dvrk/PSM1`"""
# data members, event based
self.__robot_name = robot_name
self.__ros_namespace = ros_namespace
self.__robot_state = 'uninitialized'
self.__robot_state_event = threading.Event()
self.__goal_reached = False
self.__goal_reached_event = threading.Event()
# continuous publish from dvrk_bridge
self.__position_joint_desired = []
self.__effort_joint_desired = []
self.__position_cartesian_desired = Frame()
self.__position_joint_current = []
self.__velocity_joint_current = []
self.__effort_joint_current = []
self.__position_cartesian_current = Frame()
# publishers
frame = Frame()
full_ros_namespace = self.__ros_namespace + self.__robot_name
self.set_robot_state_publisher = rospy.Publisher(full_ros_namespace + '/set_robot_state',
String, latch=True, queue_size = 1)
self.set_position_joint_publisher = rospy.Publisher(full_ros_namespace + '/set_position_joint',
JointState, latch=True, queue_size = 1)
self.set_position_goal_joint_publisher = rospy.Publisher(full_ros_namespace + '/set_position_goal_joint',
JointState, latch=True, queue_size = 1)
self.set_position_cartesian_publisher = rospy.Publisher(full_ros_namespace + '/set_position_cartesian',
Pose, latch=True, queue_size = 1)
self.set_position_goal_cartesian_publisher = rospy.Publisher(full_ros_namespace + '/set_position_goal_cartesian',
Pose, latch=True, queue_size = 1)
self.set_jaw_position_publisher = rospy.Publisher(full_ros_namespace + '/set_jaw_position',
Float32, latch=True, queue_size = 1)
self.set_wrench_body_publisher = rospy.Publisher(full_ros_namespace + '/set_wrench_body',
Wrench, latch=True, queue_size = 1)
self.set_wrench_spatial_publisher = rospy.Publisher(full_ros_namespace + '/set_wrench_spatial',
Wrench, latch=True, queue_size = 1)
# subscribers
rospy.Subscriber(full_ros_namespace + '/robot_state',
String, self.__robot_state_callback)
rospy.Subscriber(full_ros_namespace + '/goal_reached',
Bool, self.__goal_reached_callback)
rospy.Subscriber(full_ros_namespace + '/state_joint_desired',
JointState, self.__state_joint_desired_callback)
rospy.Subscriber(full_ros_namespace + '/position_cartesian_desired',
Pose, self.__position_cartesian_desired_callback)
rospy.Subscriber(full_ros_namespace + '/state_joint_current',
JointState, self.__state_joint_current_callback)
rospy.Subscriber(full_ros_namespace + '/position_cartesian_current',
Pose, self.__position_cartesian_current_callback)
# create node
# rospy.init_node('robot_api', anonymous = True)
rospy.init_node('robot_api',anonymous = True, log_level = rospy.WARN)
rospy.loginfo(rospy.get_caller_id() + ' -> started robot: ' + self.__robot_name)
def listen(self):
self.serversocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
#bind the socket to a public host, and a well-known port
self.serversocket.bind(("", self.tcp_portnum)) #become a server socket
self.serversocket.listen(1)
while True:
#accept connections
print "waiting for socket connection"
(clientsocket, address) = self.serversocket.accept()
#now do something with the clientsocket
rospy.loginfo("Established a socket connection from %s on port %s" % (address))
self.socket = clientsocket
self.isConnected = True
if (self.fork_server == True): # if configured to launch server in a separate process
rospy.loginfo("Forking a socket server process")
process = multiprocessing.Process(target=self.startSocketServer, args=(address))
process.daemon = True
process.start()
rospy.loginfo("launched startSocketServer")
else:
rospy.loginfo("calling startSerialClient")
self.startSerialClient()
rospy.loginfo("startSerialClient() exited")
def command_move(self,goal,block_program=False):
pose=list()
pose.append(float(goal.x))
pose.append(float(goal.y))
pose.append(float(goal.theta))
handle_base = sss.move("base", pose,blocking=block_program)
rospy.loginfo("Commanding move to current goal")
def GetWayPoints(self,Data):
# dir = os.path.dirname(__file__)
# filename = dir+'/locationPoint.txt'
filename = Data.data
rospy.loginfo(str(filename))
rospy.loginfo(self.locations)
self.locations.clear()
rospy.loginfo(self.locations)
f = open(filename,'r')
for i in f.readlines():
j = i.split(",")
current_wp_name = str(j[0])
rospy.loginfo(current_wp_name)
current_point = Point(float(j[1]),float(j[2]),float(j[3]))
current_quaternion = Quaternion(float(j[4]),float(j[5]),float(j[6]),float(j[7]))
self.locations[current_wp_name] = Pose(current_point,current_quaternion)
f.close()
rospy.loginfo(self.locations)
#Rviz Marker
self.init_markers()
self.IsWPListOK = True
def talker(data):
pub = rospy.Publisher('chatter', Int64, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
rospy.loginfo(data)
pub.publish(data)
rate.sleep()
def dispatch_action(self, action):
if not action.name == "move":
return
self.current_action = action
# TODO: delete (for test)
# temp = KeyValue()
# temp.key = "curr_loc"
# temp.value = "room4"
# self.current_action.parameters.append(temp)
#
# from glue pddl->plp parameter glue
for pair in action.parameters:
if pair.key == "loc":
query_result = self.message_store.query_named(pair.value, String._type, False)
# if not saved special value in DB, use the name (check if list returned is empty)
if not query_result:
self.target_location = pair.value
else:
self.target_location = query_result[0][0]
# get values from action outputs history
if not self.target_location:
self.target_location = self.message_store.query_named("output_target_location", String._type, False)
rospy.loginfo("loc value: %s",self.target_location)
# dispatch action if got everything
if self.check_can_dispatch():
self.action_publisher.publish(self.target_location)
else:
rospy.loginfo("Failed at running action: %s. Conditions not met for dispatch", action.name)
def callback(self, data):
bridge=CvBridge()
try:
cv_image=bridge.img_to_cv(data, "bgr8")
except CvBridgeError, e:
#print won't work, need to use rospy.log_ format
rospy.loginfo("Media failed to convert to OpenCV format")
def execute(self,userdata):
global locomotionGoal
#global movement_client
#rospy.loginfo('Executing state VISION_PROCESSING')
self.client = actionlib.SimpleActionClient('LocomotionServer', bbauv_msgs.msg.ControllerAction)
self.client.wait_for_server()
self.client.cancel_all_goals()
while(not rospy.is_shutdown()):
if(len(gate.centroidx) == 2):
target = (gate.centroidx[0] + gate.centroidx[1])/2 - 640*0.5
if(len(gate.centroidx)==2):
centroid_dist = fabs(gate.centroidx[0] - gate.centroidx[1])
if(target>0 and fabs(target) >10):
self.sidemove = centroid_dist*self.Kp
elif(target<0 and fabs(target) >10):
self.sidemove = -centroid_dist*self.Kp
else:
self.sidemove = 0
self.client.cancel_all_goals()
locomotionGoal.sidemove_setpoint = 0
userdata.complete_output = True
try:
resp = mission_srv_request(False,True,locomotionGoal)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
return 'task_complete'
rospy.logdebug("target:" + str(target) + "sidemove:" + str(self.sidemove))
if(not self.isCorrection):
rospy.loginfo("vision action issued!")
goal = bbauv_msgs.msg.ControllerGoal(forward_setpoint=2,heading_setpoint=locomotionGoal.heading_setpoint,depth_setpoint=locomotionGoal.depth_setpoint,sidemove_setpoint=self.sidemove)
self.client.send_goal(goal)
self.client.wait_for_result(rospy.Duration(2))
self.isCorrection = True
def __init__(self, whicharm, tf_listener = None, wait_for_services = 1): #whicharm is 'right' or 'left'
#gets the robot_prefix from the parameter server. Default is pr2
robot_prefix = rospy.get_param('~robot_prefix', '')
self.srvroot = '/'+whicharm+'_arm_kinematics/'
#If collision_aware_ik is set to 0, then collision-aware IK is disabled
self.perception_running = rospy.get_param('~collision_aware_ik', 1)
self._ik_service = rospy.ServiceProxy(self.srvroot+'get_ik', GetPositionIK, True)
if self.perception_running:
self._ik_service_with_collision = rospy.ServiceProxy(self.srvroot+'get_constraint_aware_ik', GetConstraintAwarePositionIK, True)
self._fk_service = rospy.ServiceProxy(self.srvroot+'get_fk', GetPositionFK, True)
self._query_service = rospy.ServiceProxy(self.srvroot+'get_ik_solver_info', GetKinematicSolverInfo, True)
self._check_state_validity_service = rospy.ServiceProxy('/planning_scene_validity_server/get_state_validity', GetStateValidity, True)
#wait for IK/FK/query services and get the joint names and limits
if wait_for_services:
self.check_services_and_get_ik_info()
if tf_listener == None:
rospy.loginfo("ik_utilities: starting up tf_listener")
self.tf_listener = tf.TransformListener()
else:
self.tf_listener = tf_listener
self.marker_pub = rospy.Publisher('interpolation_markers', Marker)
#dictionary for the possible kinematics error codes
self.error_code_dict = {} #codes are things like SUCCESS, NO_IK_SOLUTION
for element in dir(ArmNavigationErrorCodes):
if element[0].isupper():
self.error_code_dict[eval('ArmNavigationErrorCodes.'+element)] = element
#reads the start angles from the parameter server
start_angles_list = rospy.get_param('~ik_start_angles', [])
#good additional start angles to try for IK for the PR2, used
#if no start angles were provided
if start_angles_list == []:
self.start_angles_list = [[-0.697, 1.002, 0.021, -0.574, 0.286, -0.095, 1.699],
[-1.027, 0.996, 0.034, -0.333, -3.541, -0.892, 1.694],
[0.031, -0.124, -2.105, -1.145, -1.227, -1.191, 2.690],
[0.410, 0.319, -2.231, -0.839, -2.751, -1.763, 5.494],
[0.045, 0.859, 0.059, -0.781, -1.579, -0.891, 7.707],
[0.420, 0.759, 0.014, -1.099, -3.204, -1.907, 8.753],
[-0.504, 1.297, -1.857, -1.553, -4.453, -1.308, 9.572]]
else:
self.start_angles_list = start_angles_list
if whicharm == 'left':
for i in range(len(self.start_angles_list)):
for joint_ind in [0, 2, 4]:
self.start_angles_list[i][joint_ind] *= -1.
#changes the set of ids used to show the arrows every other call
self.pose_id_set = 0
rospy.loginfo("ik_utilities: done init")
def exectute_joint_traj(self, joint_trajectory, timing):
'''Moves the arm through the joint sequence'''
# First, do filtering on the trajectory to fix the velocities
trajectory = JointTrajectory()
# Initialize the server
# When to start the trajectory: 0.1 seconds from now
trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
trajectory.joint_names = self.joint_names
## Add all frames of the trajectory as way points
for i in range(len(timing)):
positions = joint_trajectory[i].joint_pose
velocities = [0] * len(positions)
# Add frames to the trajectory
trajectory.points.append(JointTrajectoryPoint(positions=positions,
velocities=velocities,
time_from_start=timing[i]))
output = self.filter_service(trajectory=trajectory,
allowed_time=rospy.Duration.from_sec(20))
rospy.loginfo('Trajectory for arm ' + str(self.arm_index) +
' has been filtered.')
traj_goal = JointTrajectoryGoal()
# TO-DO: check output.error_code
traj_goal.trajectory = output.trajectory
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.joint_names = self.joint_names
# Sends the goal to the trajectory server
self.traj_action_client.send_goal(traj_goal)
def _setup_ik(self):
'''Sets up services for inverse kinematics'''
ik_info_srv_name = ('pr2_' + self._side() +
'_arm_kinematics_simple/get_ik_solver_info')
ik_srv_name = 'pr2_' + self._side() + '_arm_kinematics_simple/get_ik'
rospy.wait_for_service(ik_info_srv_name)
ik_info_srv = rospy.ServiceProxy(ik_info_srv_name,
GetKinematicSolverInfo)
solver_info = ik_info_srv()
rospy.loginfo('IK info service has responded for '
+ self._side() + ' arm.')
rospy.wait_for_service(ik_srv_name)
self.ik_srv = rospy.ServiceProxy(ik_srv_name,
GetPositionIK, persistent=True)
rospy.loginfo('IK service has responded for ' + self._side() + ' arm.')
# Set up common parts of an IK request
self.ik_request = GetPositionIKRequest()
self.ik_request.timeout = rospy.Duration(4.0)
self.ik_joints = solver_info.kinematic_solver_info.joint_names
self.ik_limits = solver_info.kinematic_solver_info.limits
ik_links = solver_info.kinematic_solver_info.link_names
request = self.ik_request.ik_request
request.ik_link_name = ik_links[0]
request.pose_stamped.header.frame_id = 'base_link'
request.ik_seed_state.joint_state.name = self.ik_joints
request.ik_seed_state.joint_state.position = [0] * len(self.ik_joints)
def __init__(self):
self.bridge = CvBridge()
self.pixToMeters = 0.05/(26*2)
self.imageSub = rospy.Subscriber("nao_robot/camera/top/camera/image_raw",Image,self.callback)
self.pointPub = rospy.Publisher("point", Point, queue_size=1 )
self.walkPub = rospy.Publisher("/cmd_vel", Twist, queue_size=1 )
rospy.loginfo("Initialised tomato detector")
def callback(self,data):
rospy.loginfo("Received data")
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
point = self.searchForTomato(cv_image)
# Create twist
twist = Twist()
if point is not None:
twist = self.walkToTomato(point, twist)
else:
rospy.loginfo("No tomato, turn right and search again")
# twist.linear.x = 0
# twist.linear.y = 0
twist.angular.z = -0.5
# findTomato()
# publish twist
rate = rospy.Rate(1) # 10hz
# while not rospy.is_shutdown():
# rospy.loginfo(point)
self.walkPub.publish(twist)
rate.sleep()
def speechCb(self, msg):
rospy.loginfo(msg.data)
self.lastUtterance = msg.data
# a command keyword is found -- update the goalid
# 1. come here
# 2. grab this
# 3. stop!
# self.lastUtterance = 'right'
# self.boardToPersonRotation = random.rand() #0.0
# self.grips.grips.grasps[0].point.x = 0.1*random.rand()
if msg.data.find("come here") > -1:
self.msg.id = 'come_here'
elif msg.data.find("grab") > -1:
self.msg.id = 'grab'
elif msg.data.find("stop") > -1:
self.msg.id = 'stop'
# a locative keyword is found -- update the probabilities/weights
elif any([self.lastUtterance.find(self.legalUtterances[i]) > -1 for i in range(len(self.legalUtterances))]):
if self.axisAlignedBox:
print "board rotation relative to user: "******"I thought I heard a location keyword, but I did not understand it.")
raise RuntimeWarning("I thought I heard a location keyword, but I did not understand it.")
self.newGrips = self.grips
def __init__(self):
# ROS initialization:
rospy.init_node('pose_manager')
self.poseLibrary = dict()
self.readInPoses()
self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction,
execute_cb=self.executeBodyPose,
auto_start=False)
self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction)
if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)):
try:
rospy.wait_for_service("stop_walk_srv", timeout=2.0)
self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty)
except:
rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. "
+"This is normal if there is no nao_walker running.")
self.stopWalkSrv = None
self.poseServer.start()
rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys());
else:
rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?");
rospy.signal_shutdown("Required component missing");
def __check_for_leak(self,data):
if data.data > 1:
self.leak_detected = 1
self.leak_callback()
else:
rospy.loginfo("sensors.py: clear, no leak")
self.leak = 0
message = [wheel_vel[1], wheel_vel[0], wheel_vel[3], wheel_vel[2]]
JointState_obj1.stamp = rospy.get_time()
JointState_obj1.velocity = message
##############################################################
# Service Callbacks
##############################################################
# Main Program Code
# This is run once when the node is brought up (roslaunch or rosrun)
if __name__ == '__main__':
print "Hello world"
# get the node started first so that logging works from the get-go
rospy.init_node("base_controller")
rospy.loginfo("Started template python node: base_controller.")
# tf_listener = tf.TransformListener()
# print tf_listener.lookupTransform("front_right_wheel_joint",
# "base_link", rospy.Time.now())
##############################################################
# Service Advertisers
##############################################################
# Message Subscribers
Twist_sub1 = rospy.Subscriber("cmd_vel", Twist, cmd_vel_cb)
##############################################################
# Message Publishers
JointState_pub1 = rospy.Publisher("motor_cmds",
JointState,
queue_size=1000)
def mono_hand_loop(acq, outSize, config, track=False, paused=False, with_renderer=False):
print("Initialize WACV18 3D Pose estimator (IK)...")
pose_estimator = factory.HandPoseEstimator(config)
if with_renderer:
print("Initialize Hand Visualizer...")
hand_visualizer = pipeline.HandVisualizer(factory.mmanager, outSize)
print("Initialize openpose. Net output size",outSize,"...")
op = OP.OpenPose((160, 160), config["handnet_dims"], tuple(outSize), "COCO", config["OPENPOSE_ROOT"] + os.sep + "models" + os.sep, 0,
False, OP.OpenPose.ScaleMode.ZeroToOne, False, True)
left_hand_model = config["model_left"]
started = False
delay = {True: 0, False: 1}
ik_ms = est_ms = 0
p2d = bbox = None
count = 0
mbv_viz = opviz = None
smoothing = config.get("smoothing", 0)
boxsize = config["handnet_dims"][0]
stride = 8
peaks_thre = config["peaks_thre"]
print("Entering main Loop.")
try:
pub = rospy.Publisher('chatter', float_arr, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(100) # 10hz
while True:
try:
imgs, clbs = acq.grab()
if imgs is None or len(imgs)==0:
break
except Exception as e:
print("Failed to grab", e)
break
st = time.time()
bgr = imgs[0]
clb = clbs[0]
# compute kp using model initial pose
points2d = pose_estimator.ba.decodeAndProject(pose_estimator.model.init_pose, clb)
oldKp = np.array(points2d).reshape(-1, 2)
if bbox is None:
bbox = config["default_bbox"]
score = -1
result_pose = None
crop_viz = None
# STEP2 detect 2D joints for the detected hand.
if started and bbox is not None:
x,y,w,h = bbox
# print("BBOX: ",bbox)
crop = bgr[y:y+h,x:x+w]
img, pad = mva19.preprocess(crop, boxsize, stride)
t = time.time()
op.detectHands(bgr, np.array([[0,0,0,0]+bbox],dtype=np.int32))
op_ms = (time.time() - t) * 1000.0
# opviz = op.render(np.copy(bgr))
# cv2.imshow("OPVIZ", opviz)
leftHands, rightHands = op.getKeypoints(op.KeypointType.HAND)
if rightHands is not None:
keypoints = rightHands[0][:,:3]
mask = keypoints[:, 2] < peaks_thre
keypoints[mask] = [0, 0, 1.0]
if track:
keypoints[mask, :2] = oldKp[mask]
keypoints[:, 2] = keypoints[:, 2]**3
rgbKp = IK.Observations(IK.ObservationType.COLOR, clb, keypoints)
obsVec = IK.ObservationsVector([rgbKp, ])
t = time.time()
score, res = pose_estimator.estimate(obsVec)
ik_ms = (time.time() - t)
# print(count,)
pose_estimator.print_report()
if track:
result_pose = list(smoothing * np.array(pose_estimator.model.init_pose) + (1.0 - smoothing) * np.array(res))
else:
result_pose = list(res)
msg = np.array(res, dtype=np.float)
rospy.loginfo(msg[3:7])
pub.publish(msg)
rate.sleep()
# score is the residual, the lower the better, 0 is best
# -1 is failed optimization.
if track:
if -1 < score:# < 20000:
pose_estimator.model.init_pose = Core.ParamVector(result_pose)
else:
print("\n===>Reseting init position for IK<===\n")
pose_estimator.model.reset_pose()
if score > -1: # compute result points
p2d = np.array(pose_estimator.ba.decodeAndProject(Core.ParamVector(result_pose), clb)).reshape(-1, 2)
# scale = w/config.boxsize
bbox = mva19.update_bbox(p2d,bgr.shape[1::-1])
viz = np.copy(bgr)
viz2d = np.zeros_like(bgr)
if started and result_pose is not None:
# viz2d = mva19.visualize_2dhand_skeleton(viz2d, keypoints, thre=peaks_thre)
# cv2.imshow("2D CNN estimation",viz2d)
header = "FPS OPT+VIZ %03d, OPT %03fms (CNN %03fms, 3D %03fms)"%(1/(time.time()-st),(est_ms+ik_ms),est_ms, ik_ms)
if with_renderer:
hand_visualizer.render(pose_estimator.model, Core.ParamVector(result_pose), clb)
mbv_viz = hand_visualizer.getDepth()
# cv2.imshow("MBV VIZ", mbv_viz)
mask = mbv_viz != [0, 0, 0]
viz[mask] = mbv_viz[mask]
else:
viz = mva19.visualize_3dhand_skeleton(viz, p2d)
pipeline.draw_rect(viz, "Hand", bbox, box_color=(0, 255, 0), text_color=(200, 200, 0))
else:
header = "Press 's' to start, 'r' to reset pose, 'p' to pause frame."
cv2.putText(viz, header, (20, 20), 0, 0.7, (50, 20, 20), 1, cv2.LINE_AA)
cv2.imshow("3D Hand Model reprojection", viz)
key = cv2.waitKey(delay[paused])
if key & 255 == ord('p'):
paused = not paused
if key & 255 == ord('q'):
break
if key & 255 == ord('r'):
print("\n===>Reseting init position for IK<===\n")
pose_estimator.model.reset_pose()
bbox = config['default_bbox']
print("RESETING BBOX",bbox)
if key & 255 == ord('s'):
started = not started
count += 1
except rospy.ROSInterruptException:
pass
def setReward(self, state, done, action):
yaw_reward = []
obstacle_min_range = state[-2]
current_distance = state[-3]
heading = state[-4]
for i in range(5):
angle = -pi / 4 + heading + (pi / 8 * i) + pi / 2
tr = 1 - 4 * math.fabs(0.5 - math.modf(0.25 + 0.5 * angle %
(2 * math.pi) / math.pi)[0])
yaw_reward.append(tr)
distance_rate = 2**(current_distance / self.goal_distance)
if obstacle_min_range < 0.5:
ob_reward = -5
else:
ob_reward = 0
reward = (
(round(yaw_reward[action] * 5, 2)) * distance_rate) + ob_reward
if done:
rospy.loginfo("Near Collision!!")
reward = -200
# driving backwards last 25 actions ~5 seconds
t = 0
l = len(self.action_memory)
vel_cmd = Twist()
# while t <= 10:
# if len(self.action_memory) > 20:
# max_angular_vel = -1.5
# action = self.action_memory[l-t-1]
# ang_vel = ((-self.action_size + 1)/2 - action) * max_angular_vel * 0.5
# vel_cmd.linear.x = -0.15
# # vel_cmd.angular.z = ang_vel
# vel_cmd.angular.z = 0
# time_start = time.time()
# a=0
# self.pub_cmd_vel.publish(vel_cmd)
# t += 1
# else:
# t = 10
# stand still after collision
vel_cmd.linear.x = 0
vel_cmd.angular.z = 0
time_start = time.time()
a = 0
while a < 1:
self.pub_cmd_vel.publish(vel_cmd)
a = time.time() - time_start
if self.get_goalbox:
rospy.loginfo("Goal!!")
print "start_position: ", self.start_x, "/ ", self.start_y
print "odom_position:", self.position.x, "/ ", self.position.y
print "goal_position: ", self.goal_x, "/ ", self.goal_y
print "action: ", action
print "_______________________________________________________________"
reward = 500
self.get_goalbox = False
done = True
vel_cmd = Twist()
vel_cmd.linear.x = 0
vel_cmd.angular.z = 0
start = 0
start_1 = time.time()
while start - 5 < 0:
self.pub_cmd_vel.publish(vel_cmd)
start = time.time() - start_1
# self.pub_cmd_vel.publish(vel_cmd)
# self.goal_x, self.goal_y = self.respawn_goal.getPosition()
# self.goal_distance = self.getGoalDistace()
return reward, done
class PersonDetector():
def __init__(self):
# cv_bridge handles
self.cv_bridge = CvBridge()
# outdoor object
self.person_bbox = BoundingBox()
self.bicycle_bbox = BoundingBox()
self.car_bbox = BoundingBox()
self.motorcycle_bbox = BoundingBox()
self.bus_bbox = BoundingBox()
self.truck_bbox = BoundingBox()
self.traffic_light_bbox = BoundingBox()
self.stop_sign_bbox = BoundingBox()
# ROS PARAM
self.m_pub_threshold = rospy.get_param('~pub_threshold', 0.70)
# detect width height
self.WIDTH = 50
self.HEIGHT = 50
# Subscribe
sub_camera_rgb = rospy.Subscriber('/camera/color/image_raw', Image, self.CamRgbImageCallback)
sub_camera_depth = rospy.Subscriber('/camera/aligned_depth_to_color/image_raw', Image, self.CamDepthImageCallback)
sub_darknet_bbox = rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, self.DarknetBboxCallback)
return
def CamRgbImageCallback(self, rgb_image_data):
try:
rgb_image = self.cv_bridge.imgmsg_to_cv2(rgb_image_data, 'passthrough')
except CvBridgeError, e:
rospy.logerr(e)
rgb_image.flags.writeable = True
rgb_image = cv2.cvtColor(rgb_image, cv2.COLOR_BGR2RGB)
h, w, c = rgb_image.shape
# person exist
if self.person_bbox.probability > 0.0:
x1 = (w / 2) - self.WIDTH
x2 = (w / 2) + self.WIDTH
y1 = (h / 2) - self.HEIGHT
y2 = (h / 2) + self.HEIGHT
sum = 0.0
for i in range(y1, y2):
for j in range(x1, x2):
rgb_image.itemset((i, j, 0), 0)
rgb_image.itemset((i, j, 1), 0)
if self.m_depth_image.item(i,j) == self.m_depth_image.item(i,j):
sum += self.m_depth_image.item(i,j)
ave = sum / ((self.WIDTH * 2) * (self.HEIGHT * 2))
rospy.loginfo('Class : person, Score: %.2f, Dist: %dmm ' %(self.person_bbox.probability, ave))
# bicycle exist
elif self.bicycle_bbox.probability > 0.0:
x1 = (w / 2) - self.WIDTH
x2 = (w / 2) + self.WIDTH
y1 = (h / 2) - self.HEIGHT
y2 = (h / 2) + self.HEIGHT
sum = 0.0
for i in range(y1, y2):
for j in range(x1, x2):
rgb_image.itemset((i, j, 0), 0)
rgb_image.itemset((i, j, 1), 0)
if self.m_depth_image.item(i,j) == self.m_depth_image.item(i,j):
sum += self.m_depth_image.item(i,j)
ave = sum / ((self.WIDTH * 2) * (self.HEIGHT * 2))
rospy.loginfo('Class : bicycle, Score: %.2f, Dist: %dmm ' %(self.bicycle_bbox.probability, ave))
# car exist
elif self.car_bbox.probability > 0.0:
x1 = (w / 2) - self.WIDTH
x2 = (w / 2) + self.WIDTH
y1 = (h / 2) - self.HEIGHT
y2 = (h / 2) + self.HEIGHT
sum = 0.0
for i in range(y1, y2_bbox):
for j in range(x1, x2):
rgb_image.itemset((i, j, 0), 0)
rgb_image.itemset((i, j, 1), 0)
if self.m_depth_image.item(i,j) == self.m_depth_image.item(i,j):
sum += self.m_depth_image.item(i,j)
ave = sum / ((self.WIDTH * 2) * (self.HEIGHT * 2))
rospy.loginfo('Class : car, Score: %.2f, Dist: %dmm ' %(self.car_bbox.probability, ave))
# motorcycle exist
elif self.motorcycle_bbox.probability > 0.0:
x1 = (w / 2) - self.WIDTH
x2 = (w / 2) + self.WIDTH
y1 = (h / 2) - self.HEIGHT
y2 = (h / 2) + self.HEIGHT
sum = 0.0
for i in range(y1, y2):
for j in range(x1, x2):
rgb_image.itemset((i, j, 0), 0)
rgb_image.itemset((i, j, 1), 0)
if self.m_depth_image.item(i,j) == self.m_depth_image.item(i,j):
sum += self.m_depth_image.item(i,j)
ave = sum / ((self.WIDTH * 2) * (self.HEIGHT * 2))
rospy.loginfo('Class : motorcycle, Score: %.2f, Dist: %dmm ' %(self.motorcycle_bbox.probability, ave))
# bus exist
elif self.bus_bbox.probability > 0.0:
x1 = (w / 2) - self.WIDTH
x2 = (w / 2) + self.WIDTH
y1 = (h / 2) - self.HEIGHT
y2 = (h / 2) + self.HEIGHT
sum = 0.0
for i in range(y1, y2):
for j in range(x1, x2):
rgb_image.itemset((i, j, 0), 0)
rgb_image.itemset((i, j, 1), 0)
if self.m_depth_image.item(i,j) == self.m_depth_image.item(i,j):
sum += self.m_depth_image.item(i,j)
ave = sum / ((self.WIDTH * 2) * (self.HEIGHT * 2))
rospy.loginfo('Class : bus, Score: %.2f, Dist: %dmm ' %(self.bus_bbox.probability, ave))
# truck exist
elif self.truck_bbox.probability > 0.0:
x1 = (w / 2) - self.WIDTH
x2 = (w / 2) + self.WIDTH
y1 = (h / 2) - self.HEIGHT
y2 = (h / 2) + self.HEIGHT
sum = 0.0
for i in range(y1, y2):
for j in range(x1, x2):
rgb_image.itemset((i, j, 0), 0)
rgb_image.itemset((i, j, 1), 0)
if self.m_depth_image.item(i,j) == self.m_depth_image.item(i,j):
sum += self.m_depth_image.item(i,j)
ave = sum / ((self.WIDTH * 2) * (self.HEIGHT * 2))
rospy.loginfo('Class : truck, Score: %.2f, Dist: %dmm ' %(self.truck_bbox.probability, ave))
# traffic light exist
elif self.traffic_light_bbox.probability > 0.0:
x1 = (w / 2) - self.WIDTH
x2 = (w / 2) + self.WIDTH
y1 = (h / 2) - self.HEIGHT
y2 = (h / 2) + self.HEIGHT
sum = 0.0
for i in range(y1, y2):
for j in range(x1, x2):
rgb_image.itemset((i, j, 0), 0)
rgb_image.itemset((i, j, 1), 0)
if self.m_depth_image.item(i,j) == self.m_depth_image.item(i,j):
sum += self.m_depth_image.item(i,j)
ave = sum / ((self.WIDTH * 2) * (self.HEIGHT * 2))
rospy.loginfo('Class : traffic light, Score: %.2f, Dist: %dmm ' %(self.traffic_light_bbox.probability, ave))
# stop sign exist
elif self.stop_sign_bbox.probability > 0.0:
x1 = (w / 2) - self.WIDTH
x2 = (w / 2) + self.WIDTH
y1 = (h / 2) - self.HEIGHT
y2 = (h / 2) + self.HEIGHT
sum = 0.0
for i in range(y1, y2):
for j in range(x1, x2):
rgb_image.itemset((i, j, 0), 0)
rgb_image.itemset((i, j, 1), 0)
if self.m_depth_image.item(i,j) == self.m_depth_image.item(i,j):
sum += self.m_depth_image.item(i,j)
ave = sum / ((self.WIDTH * 2) * (self.HEIGHT * 2))
rospy.loginfo('Class : stop sign, Score: %.2f, Dist: %dmm ' %(self.stop_sign_bbox.probability, ave))
"""
cv2.normalize(self.m_depth_image, self.m_depth_image, 0, 1, cv2.NORM_MINMAX)
cv2.namedWindow("color_image")
cv2.namedWindow("depth_image")
cv2.imshow("color_image", rgb_image)
cv2.imshow("depth_image", self.m_depth_image)
cv2.waitKey(10)
"""
"""
cv2.rectangle(rgb_image, (self.person_bbox.xmin, self.person_bbox.ymin), (self.person_bbox.xmax, self.person_bbox.ymax),(0,0,255), 2)
#rospy.loginfo('Class : person, Score: %.2f, Dist: %dmm ' %(self.person_bbox.probability, m_person_depth))
text = "person " +('%dmm' % ave)
text_top = (self.person_bbox.xmin, self.person_bbox.ymin - 10)
text_bot = (self.person_bbox.xmin + 80, self.person_bbox.ymin + 5)
text_pos = (self.person_bbox.xmin + 5, self.person_bbox.ymin)
cv2.rectangle(rgb_image, text_top, text_bot, (0,0,0),-1)
cv2.putText(rgb_image, text, text_pos, cv2.FONT_HERSHEY_SIMPLEX, 0.35, (255, 0, 255), 1)
cv2.namedWindow("rgb_image")
cv2.imshow("rgb_image", rgb_image)
cv2.waitKey(10)
cv2.normalize(self.m_depth_image, self.m_depth_image, 0, 32768, cv2.NORM_MINMAX)
cv2.namedWindow("depth_image")
cv2.imshow("depth_image", self.m_depth_image)
cv2.waitKey(10)
"""
return
elif bboxs[i].Class == 'car' and bboxs[i].probability >= self.m_pub_threshold:
car_bbox = bboxs[i]
elif bboxs[i].Class == 'motorcycle' and bboxs[i].probability >= self.m_pub_threshold:
motorcycle_bbox = bboxs[i]
elif bboxs[i].Class == 'bus' and bboxs[i].probability >= self.m_pub_threshold:
bus_bbox = bboxs[i]
elif bboxs[i].Class == 'truck' and bboxs[i].probability >= self.m_pub_threshold:
truck_bbox = bboxs[i]
elif bboxs[i].Class == "traffic light" and bboxs[i].probability >= self.m_pub_threshold:
traffic_light_bbox = bboxs[i]
elif bboxs[i].Class == "stop sign" and bboxs[i].probability >= self.m_pub_threshold:
stop_sign_bbox = bboxs[i]
self.person_bbox = person_bbox
self.bicycle_bbox = bicycle_bbox
self.car_bbox = car_bbox
self.motorcycle_bbox = motorcycle_bbox
self.bus_bbox = bus_bbox
self.truck_bbox = truck_bbox
self.traffic_light_bbox = traffic_light_bbox
self.stop_sign_bbox = stop_sign_bbox
if __name__ == '__main__':
try:
rospy.init_node('person_detector', anonymous=True)
idc = PersonDetector()
rospy.loginfo('idc Initialized')
rospy.spin()
except rospy.ROSInterruptException:
pass
#!/usr/bin/env python
import rospy, time
from sensor_msgs.msg import LaserScan
ranges_list = []
def callback(data):
global ranges_list
ranges_list = data.ranges
rospy.init_node('Node_Name')
rospy.Subscriber("/scan", LaserScan, callback, queue_size=1)
time.sleep(3)
while not rospy.is_shutdown():
if ranges_list[89] <= 1:
rospy.loginfo("WARNING !!! distance : {}".format(ranges_list[89]))
time.sleep(0.01)
def initialise(self):
"""Inicializa o subscriber para leitura do sensor
"""
rospy.Subscriber(self.topic_name, Range, self._callback)
rospy.loginfo(f"Inicializando sensor de distância {rospy.get_time()}")
def shutdown():
rospy.loginfo("Node has been terminated. Closing gracefully.")
rospy.sleep(5)
if __name__ == '__main__':
try:
# If you want to debug, uncomment next line and import pdb
#pdb.set_trace()
# Init the ROS node
rospy.init_node("paHomePosition")
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(shutdown)
# Enable the robot's arms
print("Getting robot state...")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
rs.state().enabled
print("Enabling robot...")
rs.enable()
if not rospy.is_shutdown():
pa.paHomePosition()
except:
rospy.loginfo("Exception thrown...")
import qlearn
from gym import wrappers
# ROS packages required
import rospy
import rospkg
# import our training environment
import my_turtlebot2_maze
if __name__ == '__main__':
rospy.init_node('example_turtlebot2_maze_qlearn', anonymous=True, log_level=rospy.WARN)
# Create the Gym environment
env = gym.make('MyTurtleBot2Maze-v0')
rospy.loginfo("Gym environment done")
# Set the logging system
# rospack = rospkg.RosPack()
# pkg_path = rospack.get_path('my_turtlebot2_maze')
# outdir = pkg_path + '/training_results'
# env = wrappers.Monitor(env, outdir, force=True)
# rospy.loginfo("Monitor Wrapper started")
last_time_steps = numpy.ndarray(0)
# Loads parameters from the ROS param server
# Parameters are stored in a yaml file inside the config directory
# They are loaded at runtime by the launch file
Alpha = rospy.get_param("/turtlebot2/alpha")
Epsilon = rospy.get_param("/turtlebot2/epsilon")
def shutdown():
rospy.loginfo("Node has been terminated. Closing gracefully.")
rospy.sleep(5)
def close_gripper(pub):
empty_msg = Empty()
rospy.loginfo('Closing gripper')
pub.publish(empty_msg)
amcl_pose = rospy.wait_for_message('{}/amcl_pose'.format(robot_name),
PoseWithCovarianceStamped,
timeout=None)
waypoint_pub.publish(amcl_pose)
r = rospy.Rate(1)
for wp in waypoints_path:
waypoint_pub.publish(wp)
rospy.loginfo('contador do publisher: {}'.format(j))
rospy.loginfo(wp)
j += 1
r.sleep()
#Publicando a mensagem em branco que inicia o movimento
#Futuramente, eh melhor dividir em dois servicos. Um publica os waypoints no Rviz, pra ser visualizado
#em tempo real, e outro servico executa.
execute_path_pub.publish()
return MissionPathResponse('The robot {} is moving!'.format(robot_name))
if __name__ == "__main__":
rospy.init_node('mission_path_server')
s = rospy.Service('mission_path', MissionPath, transform_msg)
rospy.loginfo('The mission_path service is ready to be used!')
rospy.spin()
#Nao esta publicando a primeira mensagem. 'mission_path_script.py' funciona corretamente.
def home_arm(pub):
rospy.loginfo('Going to arm home pose')
set_arm_joint(pub, np.zeros(5))
rospy.sleep(5)
def speak_text(text):
#therapist = aiml.Kernel()
#print "I AM SAYING : " + text
rospy.loginfo(text.data)
#print ">>>> CALLING FESTIVAL >>>>>"
speak_text_service(text.data)
def cleanup(self):
rospy.loginfo("Shutting down color tracking...")
def open_gripper(pub):
empty_msg = Empty()
rospy.loginfo('Opening gripper')
pub.publish(empty_msg)
local_pos_pub = rospy.Publisher('mavros/mocap/pose', PoseStamped, queue_size=1)
time.sleep(1)
# Publishers, subscribers and services
pose_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, Pose_Callback)
state_sub = rospy.Subscriber('/mavros/state', State, State_Callback)
task_sub = rospy.Subscriber('/flyingros/lasers/pose', PoseStamped, laser_callback)
rospy.wait_for_service('mavros/set_mode')
set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
rospy.wait_for_service('mavros/cmd/arming')
arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
# Thread to send setpoints
tSetPoints = Thread(target=sendSetpoint).start()
# Thread to send Lidar height
#tLidarZ = Thread(target=sendLidar).start()
while not rospy.is_shutdown():
InterfaceKeyboard()
if __name__ == '__main__':
rospy.loginfo("We are ready")
try:
init()
except rospy.ROSInterruptException:
rospy.loginfo("init failed")
pass
def callback(data):
if data.data == "yes":
rospy.loginfo("got it!")
else:
rospy.loginfo("Wrong!")
def execute(self, ud):
rospy.loginfo("Current userdata 'msg' : %s" % (ud.msg))
rospy.sleep(2.0)
return "done"
def InterfaceKeyboard():
# Input data
global pose, laser_position_count
# Output data
global setpoint, yawSetPoint, run, position_control
# Publishers
global arming_client, set_mode_client, lasers_yaw
what = getch()
if what == "t":
setpoint.x = setpoint.x - 0.1
if what == "g":
setpoint.x = setpoint.x + 0.1
if what == "f":
setpoint.y = setpoint.y - 0.1
if what == "h":
setpoint.y = setpoint.y + 0.1
if what == "i":
setpoint.z = setpoint.z + 0.1
if what == "k":
setpoint.z = setpoint.z - 0.1
if what == "b":
yawSetPoint = yawSetPoint + 1
if what == "n":
yawSetPoint = yawSetPoint - 1
if what == "c":
setpoint.x = pose.pose.position.x
setpoint.y = pose.pose.position.y
setpoint.z = pose.pose.position.z
if what == "q":
arming_client(False)
if what == "a":
arming_client(True)
if what == "e":
set_mode_client(custom_mode = "OFFBOARD")
if what == "m":
run = False
time.sleep(1)
exit()
Q = (
pose.pose.orientation.x,
pose.pose.orientation.y,
pose.pose.orientation.z,
pose.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(Q)
Q = (
pose.pose.orientation.x,
pose.pose.orientation.y,
pose.pose.orientation.z,
pose.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(Q)
rospy.loginfo("Position x: %s y: %s z: %s", pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)
rospy.loginfo("Setpoint is now x:%s, y:%s, z:%s", setpoint.x, setpoint.y, setpoint.z)
rospy.loginfo("IMU :")
rospy.loginfo("roll : %s", rad2degf(euler[0]))
rospy.loginfo("pitch : %s", rad2degf(euler[1]))
rospy.loginfo("yaw : %s and from lasers %s", rad2degf(euler[2]), rad2degf(lasers_yaw))
rospy.loginfo("wanted yaw : %s", yawSetPoint)
def shutdown(self):
rospy.loginfo("Stop!")
self.cmd_vel.publish(Twist())
rospy.sleep(1)
#!/usr/bin/env python3
import time
import rospy
from std_msgs.msg import String
if __name__ == '__main__':
try:
pub = rospy.Publisher('/move/cmd_str', String, queue_size=10)
rospy.init_node('circle')
rate = rospy.Rate(1) # 1hz
now = time.time()
future = now + 5
while time.time() < future and not rospy.is_shutdown():
rospy.loginfo("Turning Left.")
pub.publish("left")
pub.publish("stop")
except Exception as e:
rospy.logerr("An error occurred")
def callback(data):
rospy.loginfo(rospy.get_caller_id() + 'Executive give me the motion: %s',
data.data)
def execute(self, ud):
rospy.loginfo("Current userdata 'msg' : %s" % (ud.Set_msg_in))
ud.Set_msg_out = self.msg
rospy.sleep(2.0)
return "done"
def capture(self):
rospy.loginfo("Calling capture service")
self.capture_service()
def callback(gps):
distance = math.sqrt(math.pow(gps.x, 2) + math.pow(gps.y, 2))
rospy.loginfo('Listener: GPS: distance=%f, state=%s', distance, gps.state)
rospy.loginfo("Calling capture service")
self.capture_service()
if __name__ == '__main__':
print('[INFO] Starting ros communication system...')
script_path = os.path.dirname(os.path.realpath(__file__))
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
launch_pcd = roslaunch.parent.ROSLaunchParent(uuid, [os.path.join(script_path, '..', 'launch', 'grasping_pcd_processing.launch')])
launch_pcd.start()
rospy.sleep(5)
capture_helper = CaptureHelper()
ROSSMartServo_on = False
while not ROSSMartServo_on:
rospy.loginfo('Please make sure you have started the ROSSMartServo application on the Sunrise Cabinet')
ans = raw_input(
'[USER INPUT] Type [y] and press [enter] if you have started the ROSSMartServo, otherwise exit the program: ')
if ans == 'y':
if '/iiwa/iiwa_subscriber' in rosnode.get_node_names():
ROSSMartServo_on = True
else:
rospy.loginfo('IIWA topics not detected, check network connection if you have started the SmartServo')
else:
rospy.loginfo('Exiting the program...')
launch_pcd.shutdown()
exit()
print('[INFO] Starting kuka controller...')
launch_controller = roslaunch.parent.ROSLaunchParent(uuid, [os.path.join(script_path, '..', 'launch', 'grasping_controller.launch')])
launch_controller.start()
def doPose(data):
rospy.loginfo("乌龟坐标:x=%.2f, y=%.2f,theta=%.2f",data.x,data.y,data.theta)
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) # 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.mode = rospy.get_param('~control_mode', "position")
self.lim1low_topic = rospy.get_param('~lim1low_topic', "odrive1_low_tib")
self.lim1high_topic = rospy.get_param('~lim1high_topic', "odrive1_high_tib")
self.lim2low_topic = rospy.get_param('~lim2low_topic', "odrive1_low_fem")
self.lim2high_topic = rospy.get_param('~lim2high_topic', "odrive1_high_fem")
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('reset_encoder', std_srvs.srv.Trigger, self.reset_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)
print("Subscribed to /cmd_pos")
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)
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
# 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
#set up vars to hold limit switch states
self.lim1low = False
self.lim1high = False
self.lim2low = False
self.lim2high = False
def target_pose_reached_callback(self, data):
rospy.loginfo("Received attempt status data")
self.attempt_finished = data.data
