def addEllipsoid(self, ellipsoid_type='collision'): if ellipsoid_type == 'collision': id = 999 color = ColorRGBA(r=0, g=1, b=0, a=0.5) cube = Marker(header=Header(stamp=rospy.Time.now(), frame_id=self.frame_id), pose=self.collision_ellipsoid_origin, type=Marker.SPHERE, color=color, id=id, scale=Vector3(self.collision_ellipsoid_size_x, self.collision_ellipsoid_size_y, self.collision_ellipsoid_size_z), action=Marker.ADD) self.models[id] = cube # self.deleteEllipsoid(ellipsoid_type='reaching') elif ellipsoid_type == 'reaching': id = 1000 color = ColorRGBA(r=1, g=0, b=1, a=1) cube = Marker(header=Header(stamp=rospy.Time.now(), frame_id=self.frame_id), pose=self.reaching_ellipsoid_origin, type=Marker.SPHERE, color=color, id=id, scale=Vector3(self.reaching_ellipsoid_size_x, self.reaching_ellipsoid_size_y, self.reaching_ellipsoid_size_z), action=Marker.ADD) self.models[id] = cube # self.deleteEllipsoid(ellipsoid_type='collision') elif ellipsoid_type == 'all': id = 999 color = ColorRGBA(r=0, g=1, b=0, a=0.5) cube = Marker(header=Header(stamp=rospy.Time.now(), frame_id=self.frame_id), pose=self.collision_ellipsoid_origin, type=Marker.SPHERE, color=color, id=id, scale=Vector3(self.collision_ellipsoid_size_x, self.collision_ellipsoid_size_y, self.collision_ellipsoid_size_z), action=Marker.ADD) self.models[id] = cube id = 1000 color = ColorRGBA(r=1, g=0, b=1, a=1) cube = Marker(header=Header(stamp=rospy.Time.now(), frame_id=self.frame_id), pose=self.reaching_ellipsoid_origin, type=Marker.SPHERE, color=color, id=id, scale=Vector3(self.reaching_ellipsoid_size_x, self.reaching_ellipsoid_size_y, self.reaching_ellipsoid_size_z), action=Marker.ADD) self.models[id] = cube
def update_dimensions(self, depth, width, height): self.__scale = Vector3(width, depth, height)
def dropped_it(data): if data.data == 1: print "Hello we are in DROPPED_IT" setmodel = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) setmodel(ModelState('object',Pose(Point(1.0, 0.0, 0.0005),Quaternion(0.0,0.0,0.0,1.0)),Twist(Vector3(0.0,0.0,0.0),Vector3(0.0,0.0,0.0)),'world'))
if __name__=="__main__": rospy.init_node("q4") topico_imagem = "/camera/rgb/image_raw/compressed" velocidade_saida = rospy.Publisher("/cmd_vel", Twist, queue_size = 3 ) recebe_scan = rospy.Subscriber("/scan", LaserScan, scaneou) recebedor = rospy.Subscriber(topico_imagem, CompressedImage, roda_todo_frame, queue_size=4, buff_size = 2**24) delta_tolerance = 5 dist_tolerance = 0.1 metro = 1.0 rot = Twist(Vector3(0,0,0), Vector3(0,0,0.1)) frente = Twist(Vector3(0.2,0,0), Vector3(0,0,0)) zero = Twist(Vector3(0,0,0), Vector3(0,0,0)) PROCURANDO, CENTRALIZANDO, AVANCANDO, PARADO = 1,2,3,4 state = PROCURANDO # Evitando bugs em algun setups velocidade_saida.publish(zero) rospy.sleep(1.0) dt = 0.05 while not rospy.is_shutdown():
def run(self): rospy.loginfo("ROS thread started.") self.running = True tf_pub = tf.TransformBroadcaster() #rospy.Publisher('topic_name', std_msgs.msg.String, queue_size=10) map_pub = rospy.Publisher('map', OccupancyGrid, queue_size=10) odom_pub = rospy.Publisher("odom", Odometry, queue_size=10) nav_goal_pub = rospy.Publisher("move_base_simple/goal", PoseStamped, queue_size=10) rate = rospy.Rate(20) # 10hz old_x = 0 old_y = 0 old_th = 0 last_time = rospy.Time.now() global_frame = "map" # odom child_frame = "base_link" self.map_msg = OccupancyGrid() self.map_msg.info.width = 640 # Map width self.map_msg.info.height = 480 # Map height self.map_msg.info.resolution = 0.005 # Map resolution self.map_msg.header.frame_id = global_frame # New odom_msg = Odometry() odom_msg.header.frame_id = global_frame # odom odom_msg.child_frame_id = child_frame nav_goal_msg = PoseStamped() nav_goal_msg.header.frame_id = global_frame self.set_nav_goal = {"state": False} while self.running and not rospy.is_shutdown(): #rospy.loginfo(msg) '''*************************************** ** Needs to be passed in the class **transform_data = raw_map.get_transform() ***************************************''' current_time = rospy.Time.now() self.map_msg.header.stamp = current_time # New self.map_msg.info.map_load_time = current_time # New '''*************************************** **Needs to be passed in the class **map_msg.data = raw_map.get_new_frame() #pass Frame here ***************************************''' # Publishing the map map_pub.publish(self.map_msg) try: if self.transform_data["state"] == 1: #rospy.loginfo("New tf sent!") # Getting the robot's position x = self.transform_data["x"] * 0.005 y = self.transform_data["y"] * 0.005 th = self.transform_data["angle"] odom_quat = tf.transformations.quaternion_from_euler( 0, 0, th) # Publshing the transforms tf_pub.sendTransform( (x, y, 0), odom_quat, current_time, child_frame, # child frame global_frame) # Parent frame odom odom_msg.header.stamp = current_time # Setting the pose of the robot odom_msg.pose.pose.position.x = x odom_msg.pose.pose.position.y = y odom_msg.pose.pose.position.z = 0 odom_msg.pose.pose.orientation = Quaternion(*odom_quat) # calculating velocities dt = (current_time - last_time).to_sec() vx = (x - old_x) / dt vy = (y - old_y) / dt vth = (th - old_th) / dt # Setting the velocities odom_msg.twist.twist.linear = Vector3(vx, vy, 0) odom_msg.twist.twist.angular = Vector3(0, 0, vth) # Publishing the odometery message odom_pub.publish(odom_msg) # Recording the values for the next loop Iteration old_x = x old_y = y old_th = th last_time = current_time else: #rospy.loginfo("lol tf failed") ''' # Getting the robot's position x = 1 y = 1 th = 0 odom_quat = tf.transformations.quaternion_from_euler(0, 0, th) # Publshing the transforms tf_pub.sendTransform((x, y, 0), odom_quat, current_time, child_frame, # child frame global_frame) # Parent frame odom odom_msg.header.stamp = current_time # Setting the pose of the robot odom_msg.pose.pose.position.x = x odom_msg.pose.pose.position.y = y odom_msg.pose.pose.position.z = 0 odom_msg.pose.pose.orientation = Quaternion(*odom_quat) # calculating velocities dt = (current_time - last_time).to_sec() vx = (x - old_x)/dt vy = (y - old_y)/ dt vth = (th - old_th)/ dt # Setting the velocities odom_msg.twist.twist.linear = Vector3(vx, vy, 0) odom_msg.twist.twist.angular = Vector3(0, 0, vth) # Publishing the odometery message odom_pub.publish(odom_msg) ''' except Exception as e: rospy.loginfo("Was about to die, check error below:") rospy.loginfo(e) if self.set_nav_goal["state"]: nav_goal_msg.header.stamp = rospy.Time.now() nav_goal_msg.pose.pose.position.x = self.set_nav_goal["x"] nav_goal_msg.pose.pose.position.y = self.set_nav_goal["y"] nav_goal_odom_quat = tf.transformations.quaternion_from_euler( 0, 0, self.set_nav_goal["th"]) nav_goal_msg.pose.pose.orientation = Quaternion( *nav_goal_odom_quat) nav_goal_pub.publish(nav_goal_msg) #rospy.loginfo("In the thread!!") rate.sleep() rospy.loginfo("ROS thread stopped")
def visualizer(): rospy.Subscriber("baro/pressure", FluidPressure, update_pressure) rospy.Subscriber("gps", GPS, update_gps) rate = rospy.Rate(1) trace_pub = rospy.Publisher("viz/gps_trace", Marker) primary_geofence_pub = rospy.Publisher("viz/primary_geofence", Marker) secondary_geofence_pub = rospy.Publisher("viz/secondary_geofence", Marker) trace_points = [] # initialize parameters secondary_geofence_width = rospy.get_param("secondary_geofence/width", 0.00002) min_lon = 0 max_lon = 0 min_lat = 0 max_lat = 0 try: min_lon = rospy.get_param("geofence/lon/min") max_lon = rospy.get_param("geofence/lon/max") min_lat = rospy.get_param("geofence/lat/max") max_lat = rospy.get_param("geofence/lat/min") except KeyError: rospy.logfatal( "Geofence was not configured on the parameter server! Shutting down" ) sys.exit(1) # get parameters with defaults that require the aforementioned non-default parameters secondary_min_lat = rospy.get_param("secondary_geofence/lat/min", min_lat + secondary_geofence_width) secondary_max_lat = rospy.get_param("secondary_geofence/lat/max", max_lat - secondary_geofence_width) secondary_min_lon = rospy.get_param("secondary_geofence/lon/min", min_lon - secondary_geofence_width) secondary_max_lon = rospy.get_param("secondary_geofence/lon/max", max_lon + secondary_geofence_width) while not rospy.is_shutdown(): if fix and pressure_offset != -1: gps_divisor = rospy.get_param("~gps_divisor", 0.00001) try: primary_geofence_pts = (Point( 0, 0, 0), Point(0, (max_lat - min_lat) / gps_divisor, 0), Point( (max_lon - min_lon) / gps_divisor, (max_lat - min_lat) / gps_divisor, 0), Point( (max_lon - min_lon) / gps_divisor, 0, 0), Point(0, 0, 0)) secondary_geofence_pts = ( Point((secondary_min_lon - min_lon) / gps_divisor, (secondary_min_lat - min_lat) / gps_divisor, 0), Point((secondary_min_lon - min_lon) / gps_divisor, (secondary_max_lat - min_lat) / gps_divisor, 0), Point((secondary_max_lon - min_lon) / gps_divisor, (secondary_max_lat - min_lat) / gps_divisor, 0), Point((secondary_max_lon - min_lon) / gps_divisor, (secondary_min_lat - min_lat) / gps_divisor, 0), Point((secondary_min_lon - min_lon) / gps_divisor, (secondary_min_lat - min_lat) / gps_divisor, 0)) trace_points.append( Point((lon - min_lon) / gps_divisor, (lat - min_lat) / gps_divisor, relative_alt / rospy.get_param("~baro_divisor", 10))) trace_color = rospy.get_param("~trace_color", { "r": 0, "g": 1, "b": 0, "a": 1 }) viz_trace_color = ColorRGBA(trace_color['r'], trace_color['g'], trace_color['b'], trace_color['a']) primary_geofence_color = rospy.get_param( "~primary_geofence_color", { "r": 1, "g": 1, "b": 0, "a": 1 }) viz_primary_geofence_color = ColorRGBA( primary_geofence_color['r'], primary_geofence_color['g'], primary_geofence_color['b'], primary_geofence_color['a']) secondary_geofence_color = rospy.get_param( "~secondary_geofence_color", { "r": 1, "g": 0, "b": 0, "a": 1 }) viz_secondary_geofence_color = ColorRGBA( secondary_geofence_color['r'], secondary_geofence_color['g'], secondary_geofence_color['b'], secondary_geofence_color['a']) scale = rospy.get_param("~scale", 0.25) scale_vec = Vector3(scale, 0, 0) trace_ns = rospy.get_param("~trace_ns", "moby_trace") primary_geofence_ns = rospy.get_param("~primary_geofence_ns", "moby_primary_geofence") secondary_geofence_ns = rospy.get_param( "~secondary_geofence_ns", "moby_secondary_geofence") header = Header() header.stamp = rospy.Time.now() header.frame_id = "map" trace_msg = Marker(header=header, ns=trace_ns, id=0, type=4, action=0, scale=scale_vec, color=viz_trace_color, lifetime=rospy.Duration(), frame_locked=False, points=trace_points) trace_msg.pose.orientation.w = 1.0 trace_pub.publish(trace_msg) primary_geofence_msg = Marker(header=header, ns=primary_geofence_ns, id=0, type=4, action=0, scale=scale_vec, color=viz_primary_geofence_color, lifetime=rospy.Duration(), frame_locked=False, points=primary_geofence_pts) primary_geofence_pub.publish(primary_geofence_msg) secondary_geofence_msg = Marker( header=header, ns=secondary_geofence_ns, id=0, type=4, action=0, scale=scale_vec, color=viz_secondary_geofence_color, lifetime=rospy.Duration(), frame_locked=False, points=secondary_geofence_pts) secondary_geofence_pub.publish(secondary_geofence_msg) except KeyError: rospy.logerr( "Visualization is enabled but zero point(s) are not set!") rate.sleep()
def calibrate_tool(): rospy.init_node('calibrate_tool') tool_name = rospy.get_param('~tool_name') robot_name = rospy.get_param('~robot_name') store_to_file = rospy.get_param('~store_to_file') robot = rospy.get_param('~robot') if robot == "kuka": joint_names = rospy.get_param('/controller_joint_names') controller_topic = '/position_trajectory_controller/command' calcOffset_service = '/CalculateAverageMasurement' else: joint_names = rospy.get_param('/arm/joint_names') controller_topic = '/arm/joint_trajectory_controller/command' calcOffset_service = '/arm/CalculateAverageMasurement' trajectory_pub = rospy.Publisher(controller_topic, JointTrajectory, latch=True, queue_size=1) average_measurements_srv = rospy.ServiceProxy(calcOffset_service, CalculateAverageMasurement) # Posees poses = [[0.0, 0.0, 1.5707963, 0.0, -1.5707963, 0.0], [0.0, 0.0, 1.5707963, 0.0, 1.5707963, 0.0], [0.0, 0.0, 0.0, 0.0, 1.5707963, 0.0]] poses_kuka = [[0.0, -1.5707963, 1.5707963, 0.0, -1.5707963, 0.0], [0.0, -1.5707963, 1.5707963, 0.0, 1.5707963, 0.0], [0.0, -1.5707963, 1.5707963, 0.0, 0.0, 0.0]] measurement = [] for i in range(0,len(poses)): trajectory = JointTrajectory() point = JointTrajectoryPoint() trajectory.header.stamp = rospy.Time.now() trajectory.joint_names = joint_names point.time_from_start = rospy.Duration(5.0) if robot == "kuka": point.positions = poses_kuka[i] else: point.positions = poses[i] trajectory.points.append(point) trajectory_pub.publish(trajectory) rospy.loginfo("Going to position: " + str(point.positions)) rospy.sleep(10.0) rospy.loginfo("Calculating tool force.") #ret = average_measurements_srv(500, 0.01) ret = average_measurements_srv(500, 0.01, "fts_base_link") measurement.append(ret.measurement) CoG = Vector3() Fg = (abs(measurement[0].force.z) + abs(measurement[1].force.z))/2.0; CoG.z = (sqrt(measurement[2].torque.x*measurement[2].torque.x + measurement[2].torque.y*measurement[2].torque.y)) / Fg; #CoG.z = (measurement[2].torque.x) / Fg; rospy.loginfo("Setting parametes for tool: " + tool_name) rospy.set_param('/temp/tool/CoG_x', CoG.x) rospy.set_param('/temp/tool/CoG_y', CoG.y) rospy.set_param('/temp/tool/CoG_z', CoG.z) rospy.set_param('/temp/tool/force', Fg) if store_to_file: call("rosparam dump -v `rospack find iirob_description`/tools/urdf/" + tool_name + "/" + robot_name + "_gravity.yaml /temp/tool", shell=True)
def publish_desired_acc(self): pub_vec3 = Vector3Stamped() pub_vec3.header.stamp = rospy.Time.now() pub_vec3.vector = Vector3(self.desired_acceleratons[0, 0], self.desired_acceleratons[1, 0], \ self.desired_acceleratons[2, 0]) self.pub_desired_acc.publish(pub_vec3)
def sim_odom_callback(odom_msg): odom_msg.pose.pose.orientation = Quaternion() odom_msg.pose.pose.orientation.w = 1.0 odom_msg.twist.twist.angular = Vector3() odom_msg.child_frame_id = 'level_quad' odom_pub.publish(odom_msg)
def main(): rospy.init_node('simple_marker') wait_for_time() global pub_init_pose pub_init_pose = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=1) pub = rospy.Publisher('odometry_goal', Point, queue_size=5) hostname = socket.gethostname() marker_publisher = rospy.Publisher('visualization_marker', Marker, queue_size=5) marker_publisher_host = rospy.Publisher(hostname + '/visualization_marker', Marker, queue_size=5) waypoints_publisher = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=5) waypoints_publisher_text = rospy.Publisher( 'visualization_marker_array_text', MarkerArray, queue_size=5) rospy.sleep(0.5) server = InteractiveMarkerServer('simple_marker') marker1 = DestinationMarker(server, 0, 0, 'dest1', pub) global pose_marker pose_marker = DestinationMarker(server, 1, 0, 'pose estimate', pub) global waypoint_marker waypoint_marker = DestinationMarker(server, 2, -1, 'waypoint', pub) marker1.start() marker1.markerOff() pose_marker.start() waypoint_marker.start() global waypoint_list #marker1.markerOff() waypoint_marker.markerOff() pose_marker.markerOff() rospy.Subscriber("robot_pose", PoseStamped, robot_marker_callback) service_waypoint = rospy.Service('waypoint_markers_service', waypoint, turn_waypoint_on_off) service_pose_estimate = rospy.Service('poseestimate_markers_service', poseestimate, turn_poseestimate_on_off) service_start_slam = rospy.Service('start_slam', slamsrv, turn_slam_on_off) service_start_nav = rospy.Service('start_navigation', navigatesrv, turn_nav_on_off) set_pose = rospy.Service('set_pose', poseestimate, set_post) get_map = rospy.Service('get_map', maps, get_maps) getpose = rospy.Service('get_pose', getpost, get_pose) get_map = rospy.Service('save_map', savemaps, save_maps) get_waypoint = rospy.Service('get_waypoint', waypointsarray, getwaypoints) get_a_waypoint = rospy.Service('get_a_waypoint', awaypoint, getwayAwaypoint) get_waypoint_name = rospy.Service('get_waypoint_name', waypointname, getWaypointname) delete_map = rospy.Service('delete_map', deletemap, deleteMap) delete_waypoint = rospy.Service('delete_waypoint', deletewaypoint, deleteWaypoint) tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) rate = rospy.Rate(20.0) is_slam_running = False is_nav_running = False wpts = [] p_wpts = [] markerArray2 = MarkerArray() markerArray3 = MarkerArray() p_waypoint_length = 0 print("Start loop") while not rospy.is_shutdown(): if not q_slam.empty(): status = q_slam.get() if status == 1: if is_slam_running == False: uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch_slam = roslaunch.parent.ROSLaunchParent( uuid, [ "/home/pi/linorobot_ws/src/linorobot/launch/slam.launch" ]) launch_slam.start() is_slam_running = True print("start") else: if is_slam_running == True: launch_slam.shutdown() is_slam_running = False print("stop") if not q_nav.empty(): nav_status = q_nav.get() if nav_status["sw"] == 1: if is_slam_running == False: uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) hostname = socket.gethostname() cli_args = [ "/home/pi/linorobot_ws/src/linorobot/launch/navigate.launch", "map_file:=/home/pi/linorobot_ws/src/linorobot/maps/" + nav_status["mapname"] + ".yaml" ] roslaunch_args = cli_args[1:] roslaunch_file = [( roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args)] launch_nav = roslaunch.parent.ROSLaunchParent( uuid, roslaunch_file) #map_file launch_nav.start() print(hostname) #uuid2 = roslaunch.rlutil.get_or_generate_uuid(None, False) #roslaunch.configure_logging(uuid2) #hostname = socket.gethostname() #cli_args2 = ["/home/pi/linorobot_ws/src/mqtt_bridge/launch/demo.launch"] #roslaunch_args2 = cli_args2[1:] #roslaunch_file2 = [(roslaunch.rlutil.resolve_launch_arguments(cli_args2)[0], roslaunch_args2)] #launch_nav2 = roslaunch.parent.ROSLaunchParent(uuid2, roslaunch_file2) #map_file is_nav_running = True print("start") else: if is_nav_running == True: launch_nav.shutdown() #launch_nav2.shutdown() is_nav_running = False print("stop") if not q_map_name.empty(): mapname = q_map_name.get() bin_cmd = 'rosrun map_server map_saver -f /home/pi/linorobot_ws/src/linorobot/maps/' + mapname print bin_cmd os.system(bin_cmd) data = {} with open(os.path.join(MAP_PATH, mapname + '.json'), 'w') as fp: json.dump(data, fp) #subprocess.run(["rosrun", bin_cmd]) if not q_wayponts.empty(): wpts = q_wayponts.get() wp_names = q_wayponts_names.get() print "========>" print len(wpts) #print wpts mk_length = len(markerArray2.markers) id = 0 id_t = len(markerArray2.markers) + 1 i = 0 del markerArray2.markers[:] del markerArray3.markers[:] for x in range(0, mk_length): #marker = markerArray2.markers.append( Marker(type=Marker.ARROW, id=id, action=2, scale=Vector3(0.2, 0.2, 0.2), header=Header(frame_id='map'), color=ColorRGBA(1.0, 1.0, 0.0, 1.0), text="AGV")) markerArray3.markers.append( Marker(type=Marker.TEXT_VIEW_FACING, id=id_t, action=2, scale=Vector3(0.3, 0.3, 0.3), header=Header(frame_id='map'), color=ColorRGBA(0.0, 0.1, 1.0, 1.0), text="null")) i = i + 1 id += 1 id_t += 1 waypoints_publisher.publish(markerArray2) waypoints_publisher_text.publish(markerArray3) print "clear========>" del markerArray2.markers[:] del markerArray3.markers[:] #markerArray2.markers = [] #markerArray3.markers = [] id = 0 id_t = len(wpts) + 1 i = 0 for x in wpts: #marker = markerArray2.markers.append( Marker(type=Marker.ARROW, id=id, pose=x, lifetime=rospy.Duration.from_sec(1), scale=Vector3(0.2, 0.2, 0.2), header=Header(frame_id='map'), color=ColorRGBA(1.0, 1.0, 0.0, 1.0), text="AGV")) ik_pose = Pose() ik_pose.position.x = x.position.x ik_pose.position.y = x.position.y ik_pose.position.z = x.position.z + 0.1 ik_pose.orientation.x = x.orientation.x ik_pose.orientation.y = x.orientation.y ik_pose.orientation.z = x.orientation.z ik_pose.orientation.w = x.orientation.w markerArray3.markers.append( Marker(type=Marker.TEXT_VIEW_FACING, id=id_t, pose=ik_pose, scale=Vector3(0.3, 0.3, 0.3), header=Header(frame_id='map'), color=ColorRGBA(0.0, 0.1, 1.0, 1.0), text=wp_names[i])) i = i + 1 id += 1 id_t += 1 p_waypoint_length = len(wpts) p_wpts = wpts #print markerArray3 waypoints_publisher.publish(markerArray2) waypoints_publisher_text.publish(markerArray3) try: point_odom = tfBuffer.lookup_transform('map', 'base_footprint', rospy.Time(0)) hhname = socket.gethostname() marker = Marker(type=Marker.ARROW, id=0, lifetime=rospy.Duration(1.5), pose=Pose(point_odom.transform.translation, point_odom.transform.rotation), scale=Vector3(0.2, 0.2, 0.2), header=Header(frame_id='map'), color=ColorRGBA(0.0, 1.0, 0.0, 1.0), text=hhname) marker_publisher.publish(marker) marker_publisher_host.publish(marker) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rate.sleep() #continue #print("loop") rate.sleep()
def update(self, timestamp, pose, angular_velocity, linear_acceleration): # Prepare state vector (pose only; ignore angular_velocity, linear_acceleration) state = np.array([ pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ]).reshape(1, -1) # Compute reward / penalty and check if this episode is complete done = False if (timestamp < 2 and pose.position.z < self.target_z): reward = -min( abs(self.target_z - pose.position.z), 20.0 ) # reward = zero for matching target z, -ve as you go farther, upto -20 if pose.position.z >= self.target_z: # agent has crossed the target height reward += 10.0 # bonus reward # takeoff_done = True # Hover reward elif (timestamp < 4): reward = -min( abs(self.target_z - pose.position.z), 20.0 ) # reward = zero for matching target z, -ve as you go farther, upto -20 if abs(pose.position.z - self.target_z ) >= 2: # agent has gone deviated from the target height reward -= 10.0 # bonus reward # Landing reward elif (timestamp < 6): reward = ( abs(pose.position.z) * 3 + abs(angular_velocity.x) + abs(angular_velocity.y) + abs(angular_velocity.z) + abs(linear_acceleration.x) + abs(linear_acceleration.y) + abs(linear_acceleration.z) ) # reward = zero for matching target z, -ve as you go farther, upto -20 # Normalizing the reward reward = -min(reward / 3, 20.0) if pose.position.z <= 0.8: # agent has crossed the target height reward += 10.0 # bonus reward elif timestamp > self.max_duration: # or pose.position.x > 5 or pose.position.x < -5 or pose.position.y > 5 or pose.position.y < -5: # agent has run out of time reward -= 10.0 # extra penalty done = True else: done = True reward = -min( abs(self.target_z - pose.position.z), 20.0 ) # reward = zero for matching target z, -ve as you go farther, upto -20 if pose.position.z >= self.target_z: # agent has crossed the target height reward += 10.0 # bonus reward done = True elif timestamp > self.max_duration: # agent has run out of time reward -= 10.0 # extra penalty done = True # Take one RL step, passing in current state and reward, and obtain action # Note: The reward passed in here is the result of past action(s) action = self.agent.step(state, reward, done) # note: action = <force; torque> vector # Convert to proper force command (a Wrench object) and return it if action is not None: action = np.clip(action.flatten(), self.action_space.low, self.action_space.high ) # flatten, clamp to action space limits return Wrench( force=Vector3(action[0], action[1], action[2]), torque=Vector3(action[3], action[4], action[5]) # force=Vector3(0,0, action[2]), # torque=Vector3(0,0,0) ), done else: return Wrench(), done
rospy.Subscriber("ardu_send_imu", Imu, sub_imu_nf) rospy.Subscriber("ardu_send_mag", MagneticField, sub_mag_nf) if calibration == 'Previous': offset = get_previous_imu() elif calibration == 'New': offset = calibrate_imu() else: offset = np.zeros((9, 1)) rospy.loginfo("\nCalibration done") raw_imu = vect_imu + offset pub_send_euler_angles = rospy.Publisher('filter_send_euler_angles', Vector3, queue_size=10) euler_angles_msg = Vector3() P0 = 10 * np.eye(3) Q = 0.028**2 * np.eye(3) #0.028 R = 0.01 * np.eye(3) EKF_yaw = Extended_kalman_filter(np.zeros((3, 1)), P0, f, F, h, H, Q, R) EKF_pitch = Extended_kalman_filter(np.zeros((3, 1)), P0, f, F, h, H, Q, R) EKF_roll = Extended_kalman_filter(np.zeros((3, 1)), P0, f, F, h, H, Q, R) q = Quaternion(1, 0, 0, 0) alpha = 0.2 low_pass_imu = raw_imu.copy() rospy.loginfo("\nStart main loop") while not rospy.is_shutdown():
m.pose.orientation.w = 1 m.scale = scale m.color.r = 0.2 m.color.g = 0.5 m.color.b = 1.0 m.color.a = 0.3 m.points = [tail, tip] return m while not rospy.is_shutdown(): rospy.loginfo("Publishing arrow marker") # marker_pub.publish(make_arrow_points_marker(Point(0,0,0), Point(2,2,0), 0)) # marker_pub.publish(make_arrow_points_marker(Point(0,0,0), Point(1,-1,1), 1)) # marker_pub.publish(make_arrow_points_marker(Point(-1,-1,-1), Point(1,-1,-1), 2)) # this arrow should look exactly like the other one, except that is # is twice a wide in the z direction. scale = Vector3(2, 4, 0.69) marker_pub.publish( make_arrow_points_marker(scale, Point(0, 0, 0), Point(3, 0, 0), 3)) scale = Vector3(3, 2, 1) marker_pub.publish(make_marker(Marker.SPHERE, scale, 1, 0.5, 0.2, 0.3)) marker_pub.publish(make_marker(Marker.CYLINDER, scale, 0.5, 0.2, 1, 0.3)) marker_pub.publish(make_marker(Marker.CUBE, scale, 0.2, 1, 0.5, 0.3)) marker_pub.publish(make_marker(Marker.ARROW, scale, 1, 1, 1, 0.5)) rospy.sleep(1.0)
def listerner_callback(self, msg): #instiate needed varibles and constants cv_bridge = CvBridge() img = cv_bridge.imgmsg_to_cv2(msg) img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) angle = 0.365 max_height = 350 min_height = 200 #cut off top and bottom of image img = img[len(img) - max_height:len(img) - min_height] #change the resolution of the image for easier computing img = cv2.resize(img, (100, int((max_height - min_height) / 2)), interpolation=cv2.INTER_AREA) #determine the width of the image img_width = len(img[0]) #create a second image to copy the first image into img2 = np.zeros((len(img), len(img[0]))) #cuts off portions of the image so teh resutling image is of the projection of a rectangle on the ground projected onto the camera for i in range(len(img)): #determine left and right cutoff for that row left_cutoff = int(i * angle) right_cutoff = len(img[i]) - (left_cutoff) temp = np.zeros((1, right_cutoff - left_cutoff)) #cutoff the sides and set hte img row to the resulting row with cutoffs temp[0] = img[len(img) - 1 - i][left_cutoff:right_cutoff] temp = cv2.resize(temp, (img_width, 1), interpolation=cv2.INTER_AREA) img[len(img) - 1 - i] = temp[0] #if first image, set the scanline as the reference scanline if (self.first): self.reference_scanline = self.scanLine(img) self.first = False #set some constants min_radius = 200 searches = 50 curr = -1 / min_radius max_added_width = int( math.ceil(math.sqrt(min_radius**2 - ((len(img) - 1)**2)))) max_width = img_width + 2 * max_added_width values = np.zeros(searches) #preforms the shifts on the image based on guessed turn radius for i in range(searches): temp_image = np.zeros((len(img), max_width)) if (curr == 0): shift = 0 values[i] = self.score(img) continue rad = 1 / curr #prefroms shift for j in range(len(img)): shift = int(abs(rad) - math.sqrt(rad**2 - j**2)) if (rad < 0): shift *= -1 temp_image[len(img) - 1 - j][max_added_width + shift:max_added_width + shift + img_width] = img[len(img) - 1 - j] values[i] = self.score(temp_image) curr += (1 / min_radius) / (searches / 2) turn_radius = 0 best = 0 #finds best fitting raidus best on values array for i in range(searches): if (values[i] > best): best = values[i] turn_radius = (-1 / min_radius) + ((1 / min_radius) / (searches / 2)) * i #recreates the best shifted image for debugging purposes shifted_best = np.zeros((len(img), max_width)) if (turn_radius == 0): shifted_best = img else: rad = 1 / turn_radius for j in range(len(img)): shift = int(abs(rad) - math.sqrt(rad**2 - j**2)) if (rad < 0): shift *= -1 shifted_best[len(img) - 1 - j][max_added_width + shift:max_added_width + shift + img_width] = img[len(img) - 1 - j] #calculates the offset offset = self.offset_calc(self.scanLine(shifted_best)) #print the calculated offset and turn radius self.get_logger().info('offset: "%f"' % offset) self.get_logger().info('turn_raidus: "%f"' % turn_radius) #calculates the robot's speed and rotation twist_msg = Twist() twist_msg.angular = Vector3() x_speed = 0.5 offset_r = offset * .03 turn_radius_r = turn_radius / 0.03 #applies maximium value to the rotation caused by offset if (offset_r > 0): offset_r = min(offset_r, 0.3) else: offset_r = max(offset_r, -0.3) #applies maximium value to the rotation cuase by turn_radius if (turn_radius_r > 0): turn_radius_r = min(turn_radius_r, 0.8) else: turn_radius_r = max(turn_radius_r, -0.8) #slows down the robot and lowers offset rotation influence if the turn is too tight if (abs(turn_radius) > 0.004): x_speed -= abs(turn_radius - 0.004) * 5 if (offset_r * turn_radius_r < 0): if (offset_r > 0): offset_r = min(offset_r, 0.15) else: offset_r = max(offset_r, -0.15) #set the twist object to the robots speed and rotation twist_msg.angular = Vector3() twist_msg.angular.z = turn_radius_r + offset_r self.get_logger().info('turning: "%f"' % (turn_radius_r + offset_r)) self.get_logger().info('turning: "%f"' % twist_msg.angular.z) twist_msg.linear = Vector3() twist_msg.linear.x = x_speed #publish the twist object with teh robots speed and rotation self.twist_pub.publish(twist_msg)
def publish(self): euler_angles_msg = Vector3() euler_angles_msg.x = self.yaw euler_angles_msg.y = self.pitch euler_angles_msg.z = self.roll self.pub_send_euler_angles.publish(euler_angles_msg)
def publish_desired_vel(self): pub_vec3 = Vector3Stamped() pub_vec3.header.stamp = rospy.Time.now() pub_vec3.vector = Vector3(self.desired_velocities[0, 0], self.desired_velocities[1, 0], \ self.desired_velocities[2, 0]) self.pub_desired_vel.publish(pub_vec3)
HEIGHT = 500 pygame.display.set_caption('Drone Control') windowSurface = pygame.display.set_mode((WIDTH, HEIGHT), 0, 32) windowSurface.fill(BLACK) font = pygame.font.Font(pygame.font.get_default_font(), 10) myText = 't: Takeoff, l: Land, w: Forward, a: Left, s: Backward, d: Right, q: Up, e: Down, z: RotateLeft, x: RotateRight' myTextSurface = font.render(myText, True, WHITE) textRect = myTextSurface.get_rect() textRect.center = (WIDTH // 2, HEIGHT // 2) windowSurface.blit(myTextSurface, textRect) pygame.display.update() while True: setVelocity.twist.linear = Vector3(x=0, y=0, z=0) setVelocity.twist.angular = Vector3(x=0, y=0, z=0) for event in pygame.event.get(): if event.type == pygame.KEYDOWN: if event.key == pygame.K_t: setMode() armCopter() takeOff() time.sleep(5) if event.key == pygame.K_l: land() disarmCopter() time.sleep(5) if event.key == pygame.K_w: # move forward
def publish_desired_snap(self): pub_vec3 = Vector3Stamped() pub_vec3.header.stamp = rospy.Time.now() pub_vec3.vector = Vector3(self.desired_snap[0, 0], self.desired_snap[1, 0], \ self.desired_snap[2, 0]) self.pub_desired_snap.publish(pub_vec3)
def stopMoving(self): velocity = Twist() velocity.linear = Vector3(0., 0., 0.) velocity.angular = Vector3(0., 0., 0.) self.cmdVelPublisher.publish(velocity)
########################################## # Joint trayectory control UR5e Geomagic # ########################################## ############ # Var init # ############ ################# # Main function # ################# # Inicializo nodo - haptic_jointpose - rospy.init_node('testing', anonymous=False) r = rospy.Rate(10) pub = rospy.Publisher('/Geomagic/force_feedback', DeviceFeedback, queue_size=1) df = DeviceFeedback() df.force = Vector3() df.lock = [False] while not rospy.is_shutdown(): df.force.x = 0 df.force.y = 0 df.force.z = 0 pub.publish(df) r.sleep()
def callback(self, data): drone01 = data.pose[idx[0]] drone01_vel = data.twist[idx[0]] d01_posX = drone01.position.x d01_posY = drone01.position.y d01_posZ = drone01.position.z d01_velX = drone01_vel.linear.x d01_velY = drone01_vel.linear.y drone02 = data.pose[idx[1]] drone02_vel = data.twist[idx[1]] d02_posX = drone02.position.x d02_posY = drone02.position.y d02_posZ = drone02.position.z d02_velX = drone02_vel.linear.x d02_velY = drone02_vel.linear.y drone04 = data.pose[idx[3]] drone04_vel = data.twist[idx[3]] d04_posX = drone04.position.x d04_posY = drone04.position.y d04_posZ = drone04.position.z d04_velX = drone04_vel.linear.x d04_velY = drone04_vel.linear.y drone05 = data.pose[idx[4]] drone05_vel = data.twist[idx[4]] d05_posX = drone05.position.x d05_posY = drone05.position.y d05_posZ = drone05.position.z d05_velX = drone05_vel.linear.x d05_velY = drone05_vel.linear.y drone06 = data.pose[idx[5]] drone06_vel = data.twist[idx[5]] d06_posX = drone06.position.x d06_posY = drone06.position.y d06_posZ = drone06.position.z d06_velX = drone06_vel.linear.x d06_velY = drone06_vel.linear.y drone07 = data.pose[idx[6]] drone07_vel = data.twist[idx[6]] d07_posX = drone07.position.x d07_posY = drone07.position.y d07_posZ = drone07.position.z d07_velX = drone07_vel.linear.x d07_velY = drone07_vel.linear.y drone08 = data.pose[idx[7]] drone08_vel = data.twist[idx[7]] d08_posX = drone08.position.x d08_posY = drone08.position.y d08_posZ = drone08.position.z d08_velX = drone08_vel.linear.x d08_velY = drone08_vel.linear.y #print(control,d04_posZ) #################### ## POSITION BASED ## #################### velX, velY, velZ = control #print("ini velx: ", velX) repul = 0.0 pub = self.cmd_vel_pub curr_time = time.time() - start dist_87 = float( math.sqrt((d08_posX - d07_posX)**2 + (d08_posY - d07_posY)**2)) dist_76 = float( math.sqrt((d06_posX - d07_posX)**2 + (d06_posY - d07_posY)**2)) if d07_posZ < 2 and dist_87 > 15: calc_odom = (Vector3(-0.1 * (d07_velX - d08_posX), -0.1 * (d07_posY - d08_posY), -0.05 * (d07_posZ - 3))) print("ATR APPEAR!!") else: if d07_posZ >= 2 and curr_time > 10: if dist_87 <= MAX_DIST and d08_posZ >= 2: #print("d87 REPUL APPEAR!!", dist_87) b87 = 1.8 * float((20.0 / (MAX_DIST**7)) * (dist_87**7) - (70.0 / (MAX_DIST**6)) * (dist_87**6) + (84.0 / (MAX_DIST**5)) * (dist_87**5) - (35.0 / (MAX_DIST**4)) * (dist_87**4) + 1) else: b87 = 0.0 if dist_76 <= MAX_DIST and d06_posZ >= 2: #print("d76 REPUL APPEAR!!", dist_76) b76 = -0.4 * float((20.0 / (MAX_DIST**7)) * (dist_76**7) - (70.0 / (MAX_DIST**6)) * (dist_76**6) + (84.0 / (MAX_DIST**5)) * (dist_76**5) - (35.0 / (MAX_DIST**4)) * (dist_76**4) + 1) else: b76 = 0.0 repul = round((b76 + b87), 2) else: repul = 0.0 new_velX = velX - repul calc_odom = (Vector3(new_velX, velY, -0.8 * (d07_posZ - 3))) pub.publish(Twist(calc_odom, Vector3(0, 0, 0))) #print('position control now') rospy.Rate(1).sleep
def main(): rospy.init_node('object', anonymous=True) #Initial location (reset) setmodel = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) setmodel(ModelState('object',Pose(Point(0.0,0.0,0.0005),Quaternion(0.0,0.0,0.0,1.0)),Twist(Vector3(0.0,0.0,0.0),Vector3(0.0,0.0,0.0)),'world')) #Run loop for all the simulation time length while not rospy.is_shutdown(): rospy.sleep(0.01) rospy.Subscriber('robot_has_piece', Int8,correct_gazebo) if correction == 0: getmodel = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) data = getmodel('object','') piece=rospy.Publisher('piece_location', Point,queue_size=1,latch=True) piece.publish(data.pose.position.x+0.3,data.pose.position.y-0.3,data.pose.position.z+0.555) print str(data.pose.position.x+0.3)+','+str(data.pose.position.y-0.3)+','+str(data.pose.position.z+0.555) else: setmodel = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) #setmodel(ModelState('object',Pose(Point(0.0,0.0,0.0005),Quaternion(0.0,0.0,0.0,1.0)),Twist(Vector3(0.0,0.0,0.0),Vector3(0.0,0.0,0.0)),'world')) #setmodel(ModelState('object',Pose(Point(0.18,0.47,0.15),Quaternion(0.0,0.0,0.0,1.0)),Twist(Vector3(0.0,0.0,0.0),Vector3(0.0,0.0,0.0)),'world')) piece=rospy.Publisher('piece_location', Point,queue_size=1,latch=True) piece.publish(0.48,0.17,0.705) print str(0.48)+','+str(0.17)+','+str(0.705) rospy.sleep(0.01) rospy.Subscriber('resetpiece', Int8,reset) rospy.Subscriber('dropped_piece', Int8, dropped_it)
def two_arms_generate_pose(self): rospy.logdebug("Generating pose") data_0 = deepcopy(self._last_data_0[0]) data_1 = deepcopy(self._last_data_1[0]) right_pose = deepcopy(self._right_calib_pose) left_pose = deepcopy(self._left_calib_pose) if self.arm_mode == "first": if self._is_vector_valid(data_0): change_0 = Vector3() change_0.x = data_0.x - self._calib_data_0[0].x change_0.y = data_0.y - self._calib_data_0[0].y change_0.z = data_0.z - self._calib_data_0[0].z print "MYO_0 (Right forearm)" print "PITCH: ", change_0.y if (self._is_over_step(change_0.y)): right_pose["right_w1"] += radians(-1 * change_0.y) print "YAW: ", change_0.z if (self._is_over_step(change_0.z)): right_pose["right_w0"] += radians(-1 * change_0.z) print "ROLL: ", change_0.x if (self._is_over_step(change_0.x)): right_pose["right_w2"] += radians(-1 * change_0.x) if self._is_vector_valid(data_1): change_1 = Vector3() change_1.x = data_1.x - self._calib_data_1[0].x change_1.y = data_1.y - self._calib_data_1[0].y change_1.z = data_1.z - self._calib_data_1[0].z print "MYO_1 (Left forearm)" print "PITCH: ", change_1.y if (self._is_over_step(change_1.y)): left_pose["left_w1"] += radians(-1 * change_1.y) print "YAW: ", change_1.z if (self._is_over_step(change_1.z)): left_pose["left_w0"] += radians(-1 * change_1.z) print "ROLL: ", change_1.x if (self._is_over_step(change_1.x)): left_pose["left_w2"] += radians(-1 * change_1.x) elif self.arm_mode == "second": if self._is_vector_valid(data_0): change_0 = Vector3() change_0.x = data_0.x - self._calib_data_0[0].x change_0.y = data_0.y - self._calib_data_0[0].y change_0.z = data_0.z - self._calib_data_0[0].z print "MYO_0 (Right forearm)" print "PITCH: ", change_0.y if (self._is_over_step(change_0.y)): right_pose["right_e1"] += radians(change_0.y) print "YAW: ", change_0.z if (self._is_over_step(change_0.z)): right_pose["right_w1"] += radians(change_0.z) print "ROLL: ", change_0.x if (self._is_over_step(change_0.x)): right_pose["right_w2"] += radians(-1 * change_0.x) if self._is_vector_valid(data_1): change_1 = Vector3() change_1.x = data_1.x - self._calib_data_1[0].x change_1.y = data_1.y - self._calib_data_1[0].y change_1.z = data_1.z - self._calib_data_1[0].z print "MYO_1 (Left forearm)" print "PITCH: ", change_1.y if (self._is_over_step(change_1.y)): left_pose["left_e1"] += radians(-1 * change_1.y) print "YAW: ", change_1.z if (self._is_over_step(change_1.z)): left_pose["left_w1"] += radians(-1 * change_1.z) print "ROLL: ", change_1.x if (self._is_over_step(change_1.x)): left_pose["left_w2"] += radians(change_1.x) return (right_pose, left_pose)
def send_att(self): rate = rospy.Rate(100) # Hz self.att.body_rate = Vector3(0, 0, 2) self.att.header = Header() self.att.header.frame_id = "base_footprint" self.att.orientation = Quaternion(*quaternion_from_euler(0, 0, 0)) self.att.thrust = 0.3 self.att.type_mask = 7 # ignore body rate xPID = PID(kp=0.1, kd=0.05, ki=0.05) zPID = PID(kp=0.03, kd=0.01, ki=0.01) yPID = PID(kp=0.03, kd=0.01, ki=0.01) time = np.arange(0, 2 * np.pi, 0.001) amplitude = 2 * np.sin(time) length = amplitude.shape[0] i = 0 while not rospy.is_shutdown(): try: trans = (self.udrone_state.x, self.udrone_state.y, self.udrone_state.z) print("Estimate: ", trans) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as Ex: print(Ex) self.att.header.stamp = rospy.Time.now() self.att.orientation = Quaternion( *quaternion_from_euler(0, 0, 0)) self.att.thrust = 0.3 self.att_pub.publish(self.att) rate.sleep() continue x_error = xPID.run(trans[0], 0) y_error = yPID.run(trans[1], amplitude[i]) i += 1 i = i % length z_error = zPID.run(trans[2], 5) # if z_error is positive, udrone is far from boat # so increase pitch to increase the altitude if z_error < -1.5709: z_error = -1.5709 elif z_error > 1.5709: z_error = 1.5709 # if y_error is positive, udrone is far from boat # so increase pitch to increase the altitude if y_error < -1.5709: y_error = -1.5709 elif y_error > 1.5709: y_error = 1.5709 if x_error < 0: x_error = 0.0 elif x_error > 1: x_error = 1.0 print(x_error, y_error, z_error) self.att.header.stamp = rospy.Time.now() self.att.orientation = Quaternion( *quaternion_from_euler(0, -1 * z_error, y_error)) self.att.thrust = x_error self.att_pub.publish(self.att) rate.sleep()
def one_arm_generate_pose(self): rospy.logdebug("Generating pose") data_0 = deepcopy(self._last_data_0[0]) data_1 = deepcopy(self._last_data_1[0]) this_pose = deepcopy(self._right_calib_pose) if self.arm_mode == "first": if self._is_vector_valid(data_1): change_1 = Vector3() change_1.x = data_1.x - self._calib_data_1[0].x change_1.y = data_1.y - self._calib_data_1[0].y change_1.z = data_1.z - self._calib_data_1[0].z if (change_1.x < -179): change_1.x += 360 if (change_1.y < -179): change_1.y += 360 if (change_1.z < -179): change_1.z += 360 if (change_1.x > 179): change_1.x -= 360 if (change_1.y > 179): change_1.y -= 360 if (change_1.z > 179): change_1.z -= 360 # print "MYO_1 (Upper arm)" rospy.logdebug("Data_1.x %f" % data_1.x) rospy.logdebug("CalibData_1.x %f" % self._calib_data_1[0].x) rospy.logdebug("Data_1.y %f" % data_1.y) rospy.logdebug("CalibData_1.y %f" % self._calib_data_1[0].y) rospy.logdebug("Data_1.z %f" % data_1.z) rospy.logdebug("CalibData_1.z %f" % self._calib_data_1[0].z) # print "ROLL: ", change_1.z if (self._is_over_step(change_1.z)): this_pose["right_e0"] += radians(-1 * change_1.z) # print "PITCH: ", change_1.y if (self._is_over_step(change_1.y)): # this_pose["right_e1"] += radians(-1 * change_1.y) this_pose["right_s1"] += radians(1 * change_1.y) # print "YAW: ", change_1.x if (self._is_over_step(change_1.x)): this_pose["right_s0"] += radians(-1 * change_1.x) if self._is_vector_valid(data_0): change_0 = Vector3() change_0.x = data_0.x - self._calib_data_0[0].x - change_1.x change_0.y = data_0.y - self._calib_data_0[0].y - change_1.y change_0.z = data_0.z - self._calib_data_0[0].z - change_1.z if (change_0.x < -179): change_0.x += 360 if (change_0.y < -179): change_0.y += 360 if (change_0.z < -179): change_0.z += 360 if (change_0.x > 179): change_0.x -= 360 if (change_0.y > 179): change_0.y -= 360 if (change_0.z > 179): change_0.z -= 360 # print "MYO_0 (Forearm)" rospy.logdebug("Data_0.x %f" % data_0.x) rospy.logdebug("CalibData_0.x %f" % self._calib_data_0[0].x) rospy.logdebug("Data_0.y %f" % data_0.y) rospy.logdebug("CalibData_0.y %f" % self._calib_data_0[0].y) rospy.logdebug("Data_0.z %f" % data_0.z) rospy.logdebug("CalibData_0.z %f" % self._calib_data_0[0].z) # print "ROLL: ", change_0.z if (self._is_over_step(change_0.z)): this_pose["right_w0"] += radians(-1 * change_0.z) # this_pose["right_w0"] += radians(1 * change_0.z) # print "PITCH: ", change_0.y if (self._is_over_step(change_0.y)): # this_pose["right_w1"] += radians(-1 * change_0.y) # this_pose["right_w1"] += radians(-1 * change_0.y) this_pose["right_w1"] += radians(1 * change_0.y) # print "YAW: ", change_0.x if (self._is_over_step(change_0.x)): # this_pose["right_w2"] += radians(-1 * change_0.x) # this_pose["right_w2"] += radians(1 * change_0.x) this_pose["right_e1"] += radians(-1 * change_0.x) elif self.arm_mode == "second": print "SECOND!" # Alex you need to change below if self._is_vector_valid(data_1): change_1 = Vector3() change_1.x = data_1.x - self._calib_data_1[0].x change_1.y = data_1.y - self._calib_data_1[0].y change_1.z = data_1.z - self._calib_data_1[0].z print "MYO_1 (Upper arm)" print "ROLL: ", change_1.x if (self._is_over_step(change_1.x)): this_pose["right_e0"] += radians(change_1.x) print "PITCH: ", data_1.y if (self._is_over_step(change_1.y)): this_pose["right_s1"] += radians(-1 * change_1.y) print "YAW: ", change_1.z if (self._is_over_step(change_1.z)): this_pose["right_s0"] += radians(change_1.z) if self._is_vector_valid(data_0): change_0 = Vector3() change_0.x = data_0.x - self._calib_data_0[0].x change_0.y = data_0.y - self._calib_data_0[0].y change_0.z = data_0.z - self._calib_data_0[0].z print "MYO_0 (Forearm)" print "PITCH: ", data_0.y print "ROLL: ", data_0.x if (self._is_over_step(change_0.x)): this_pose["right_w2"] += radians(-1 * change_0.x) if (self._is_over_step(change_0.y)): change_0.y += change_1.y this_pose["right_e1"] += radians(-1 * change_0.y) print "YAW: ", data_0.z if (self._is_over_step(change_0.z)): this_pose["right_w1"] += radians(change_0.z) return this_pose
def __init__(self): # Give the simulation enough time to start time.sleep(10) self.cage_is_on = True # Create the publisher and subscriber self.position_pub = rospy.Publisher('/uav/input/position', Vector3, queue_size=1) self.keyboard_sub = rospy.Subscriber('/keyboardmanager/position', Vector3, self.getKeyboardCommand, queue_size=1) # TO BE COMPLETED AFTER CHECKPOINT 1 # TODO: Add a position_sub that subscribes to the drones pose self.position_sub = rospy.Subscriber('/uav/sensors/gps', PoseStamped, self.getPoint, queue_size=1) self.service = rospy.Service('toggle_cage', toggle_cage, self.toggleCage) # Get the acceptance range self.acceptance_range = rospy.get_param( "/state_safety_node/acceptance_range", 0.5) # Getting the virtual cage parameters cage_params = rospy.get_param('/state_safety_node/virtual_cage', { 'x': 5, 'y': 5, 'z': 5 }) cx, cy, cz = cage_params['x'], cage_params['y'], cage_params['z'] # Create the virtual cage self.cage_x = [-1 * cx, cx] self.cage_y = [-1 * cy, cy] self.cage_z = [0, cz] # Display incoming parameters rospy.loginfo( str(rospy.get_name()) + ": Launching with the following parameters:") rospy.loginfo( str(rospy.get_name()) + ": Param: cage x - " + str(self.cage_x)) rospy.loginfo( str(rospy.get_name()) + ": Param: cage y - " + str(self.cage_y)) rospy.loginfo( str(rospy.get_name()) + ": Param: cage z - " + str(self.cage_z)) rospy.loginfo( str(rospy.get_name()) + ": Param: acceptance range - " + str(self.acceptance_range)) # Create the drones state as hovering self.state = DroneState.HOVERING rospy.loginfo(str(rospy.get_name()) + ": Current State: HOVERING") # Create the goal messages we are going to be sending self.goal_cmd = Vector3() # Create a point message that saves the drones current position self.drone_position = Point() # Start the drone a little bit off the ground self.goal_cmd.z = 0.5 # Keeps track of whether the position was changed or not self.goal_changed = False # Call the mainloop of our class self.mainloop()
#! /usr/bin/env python3 # -*- coding:utf-8 -*- import rospy from geometry_msgs.msg import Twist, Vector3 v = 10 # Velocidade linear w = 5 # Velocidade angular if __name__ == "__main__": rospy.init_node("roda_exemplo") pub = rospy.Publisher("cmd_vel", Twist, queue_size=3) recebe_scan = rospy.Subscriber("/scan", LaserScan, scaneou) try: while not rospy.is_shutdown(): vel = Twist(Vector3(v,0,0), Vector3(0,0,0)) pub.publish(vel) rospy.sleep(2.0) except rospy.ROSInterruptException: print("Ocorreu uma exceção com o rospy")
def arm_sim(bot, interploation_rate): rospy.init_node('talker', anonymous=True) joint_vel_pub = rospy.Publisher('/robot/limb/right/joint_command', baxter_core_msgs.msg.JointCommand, queue_size=10) jacobian_pub = rospy.Publisher('/hw2/jacobian', hw2.msg.Jacobian, queue_size=10) rate = rospy.Rate(1000) rospy.Subscriber("trajectory_command", hw2.msg.TrajectoryCommand, TrajectoryCommandCallback, callback_args=bot) rospy.Subscriber("trajectory_multi_command", hw2.msg.TrajectoryMultiCommand, TrajectoryMultiCommandCallback, callback_args=bot) num_joints = len(BAXTER_RIGHT_JOINT_NAMES) #Time reference: http://wiki.ros.org/rospy/Overview/Time #Give ROS time to connect and get the time rate.sleep() t_initial = rospy.get_rostime().to_sec() - 0.001 #sleep a bit so that our first time step isn't at t=0 because that will make the motion equation singluar rate.sleep() print "Ready" while not rospy.is_shutdown(): joint_cmd = baxter_core_msgs.msg.JointCommand() joint_cmd.mode = joint_cmd.VELOCITY_MODE joint_still_moving = False for joint_index,joint_name in enumerate(BAXTER_RIGHT_JOINT_NAMES): joint_cmd.names.append(joint_name) t_now = rospy.get_rostime().to_sec() if (abs(bot.dhParams[joint_index][3] - bot.desiredDhParams[joint_index][3]) > ENDING_POSITION_TOLERANCE) and (t_now < (bot.tFinal[joint_index]+END_TIME_TOLERANCE)): #We can use these equations to compute the required velocity of the system at any point in time t = (t_now - bot.tInitial[joint_index]) #check if we need to blend this velocity blended_a3 = bot.a3[joint_index] blended_a2 = bot.a2[joint_index] blended_a1 = bot.a1[joint_index] blended_a0 = bot.a0[joint_index] if bot.nextDesiredDhParams != []: #There is another trajectory coming up, so blend to it t_blend = (bot.tFinal[joint_index] - t_now) # this gives seconds until changeover if t_blend > bot.nextTKPrime[0]: t_blend = bot.nextTKPrime[0] alpha = t_blend / bot.nextTKPrime[0] blended_a3 = bot.a3[joint_index]*alpha + bot.nexta3[0][joint_index]*(1.0-alpha) blended_a2 = bot.a2[joint_index]*alpha + bot.nexta2[0][joint_index]*(1.0-alpha) blended_a1 = bot.a1[joint_index]*alpha + bot.nexta1[0][joint_index]*(1.0-alpha) blended_a0 = bot.a0[joint_index]*alpha + bot.nexta0[0][joint_index]*(1.0-alpha) velocity_this_step = 3*blended_a3*pow(t,2) + 2*blended_a2*t + blended_a1 #Store the new position of the joint by solving equation 1 for qf at the current time bot.dhParams[joint_index][3] = blended_a3*math.pow(t,3) + blended_a2*math.pow(t,2) + blended_a1*t + blended_a0 joint_still_moving = True else: velocity_this_step = 0 #Store the new velocity of the joint by similarly solving equation 1 bot.currentVelocity[joint_index] = velocity_this_step joint_cmd.command.append(velocity_this_step) #print("Commanded joint "+joint_cmd.names[-1]+ " to velocity "+str(joint_cmd.command[-1])) if joint_still_moving == True: joint_vel_pub.publish(joint_cmd) #print("Commanding"+str(joint_cmd.command)) elif bot.supressCommand != 0: #When working with the real robot you'll want the line below so that the arm doesn't keep moving when an action is compelte: #joint_cmd.command = [0 for x in range(0,len(BAXTER_RIGHT_JOINT_NAMES))] joint_vel_pub.publish(joint_cmd) bot.supressCommand -= 1 #print("Commanding"+str(joint_cmd.command)) elif bot.nextDesiredDhParams != []: #apply these new params bot.desiredDhParams = bot.nextDesiredDhParams[0] bot.a0 = bot.nexta0[0] bot.a1 = bot.nexta1[0] bot.a2 = bot.nexta2[0] bot.a3 = bot.nexta3[0] bot.tFinal = bot.nextTFinal[0] bot.tKPrime = bot.nextTKPrime[0] bot.tInitial = bot.nextTInitial[0] bot.nextDesiredDhParams = bot.nextDesiredDhParams[1:] bot.nexta0 = bot.nexta0[1:] bot.nexta1 = bot.nexta1[1:] bot.nexta2 = bot.nexta2[1:] bot.nexta3 = bot.nexta3[1:] bot.nextTFinal = bot.nextTFinal[1:] bot.nextTKPrime = bot.nextTKPrime[1:] bot.nextTInitial = bot.nextTInitial[1:] print "Executing next trajectory, "+str(len(bot.nextDesiredDhParams))+" remain" bot.supressCommand = ZERO_VEL_RETRANSMISSIONS states = "State:" #compute the position of the end efector #This is easy given the Project 1 code: just compute the homogeneous transform of the end effector and take the rightmost column h_transform_end_effector = bot.getT(0,num_joints-1) jacobian_msg = hw2.msg.Jacobian() jp = np.zeros((3,num_joints)) jo = np.zeros((3,num_joints)) for joint_index,name in enumerate(BAXTER_RIGHT_JOINT_NAMES): states += " q"+str(joint_index)+"="+str(bot.dhParams[joint_index][3]) #compute the jacobian for this joint #compute the position of this joint h_transform_this_joint = bot.getT(0,joint_index-1) #The equation for the first three rows of the i'th column of the jacobian is: # Jp_i=z_{i-1} x (P_e-P_i-1) joint_position = (h_transform_end_effector - h_transform_this_joint)[0:3,-1] #The z axis of the previous joint is just the third column (column 2) of the homogeneous matrix z_vector_previous_joint = h_transform_this_joint[0:3,2] jp[0:3,joint_index] = np.cross(z_vector_previous_joint,joint_position) jo[0:3,joint_index] = z_vector_previous_joint #Format this as ROS wants it jp_col = np.round(jp[0:3,joint_index],15) jp_vector = Vector3(jp_col[0],jp_col[1],jp_col[2]) jacobian_msg.JP.append(jp_vector) jo_col = np.round(jo[0:3,joint_index],15) jo_vector = Vector3(jo_col[0],jo_col[1],jo_col[2]) jacobian_msg.JO.append(jo_vector) jacobian_msg.header.stamp = rospy.Time.now() jacobian_msg.header.frame_id = "0" jacobian_msg.names = BAXTER_RIGHT_JOINT_NAMES jacobian_pub.publish(jacobian_msg) print(states) rate.sleep()
def publish_desired_payload_pos(self): pub_vec3 = Vector3Stamped() pub_vec3.header.stamp = rospy.Time.now() pub_vec3.vector = Vector3(self.des_payload_position[0, 0], self.des_payload_position[1, 0], \ self.des_payload_position[2, 0]) self.pub_payload_desired_pos.publish(pub_vec3)
def toTranslationMsg(tq): t = TranslationMSG() t.x, t.y, t.z = toXYZ(tq) return t
from geometry_msgs.msg import Quaternion serial_port = '/dev/ttyUSB0' baudrate = 115200 serial_timeout_seconds = 0.05 sensors_list = ['A', 'C', 'G'] sensors_dictionary = {'A' : 'acceleration', 'G' : 'gyroscope', 'C' : 'compass'} command_min_length = 5 #Covariance matrixes orientation_covariance = [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] angular_velocity_covariance = [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] linear_acceleration_covariance = [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #Sensors data ROS buffers orientation_msg = Vector3(0, 0, 0) angular_vel_msg = Vector3(0, 0, 0) lin_accel_msg = Vector3(0, 0, 0) #Sensors data dictionary sensor_data_dict = {'A' : lin_accel_msg, 'G' : angular_vel_msg, 'C' : orientation_msg} #ROS IMU message imuMsg = Imu() imuMsg.header.frame_id = 'imu_pub' imuMsg.orientation_covariance = orientation_covariance imuMsg.angular_velocity_covariance = angular_velocity_covariance imuMsg.linear_acceleration_covariance = linear_acceleration_covariance imuMsg.orientation = orientation_msg imuMsg.angular_velocity = angular_vel_msg