如何将 ROS 消息从一个 python 发布到另一个
How to publish a ROS msg from one python to another
试图在我的 ROS 项目中配置基本的颜色检测，但我陷入了困境：
我有两个 python 脚本，其中“programa.py”是我的主要机器人程序，“camara.py”是我的 openCV 颜色检测脚本。
现在，当 camara.py 为 运行 时，我可以更改发布字符串消息的颜色检测：
rostopic pub -1 /detect_object_color std_msgs/String "purple"
我想以某种方式包括当此脚本为 运行（主函数的第 4 行）时从我的“programa.py”发送此消息的选项。
这些都是程序代码：
programa.py
import sys
import tf
import time
import rospy
import camara
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
import cv2
from math import pi
from std_srvs.srv import Empty
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
class ExampleMoveItTrajectories(object):
"""ExampleMoveItTrajectories"""
def __init__(self):
# Initialize the node
super(ExampleMoveItTrajectories, self).__init__()
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('example_move_it_trajectories')
self.is_gripper_present = rospy.get_param(rospy.get_namespace() + "is_gripper_present", True)
# Create the MoveItInterface necessary objects
arm_group_name = "arm"
self.robot = moveit_commander.RobotCommander("robot_description")
print self.robot.get_group_names()
self.scene = moveit_commander.PlanningSceneInterface(ns=rospy.get_namespace())
self.arm_group = moveit_commander.MoveGroupCommander(arm_group_name, ns=rospy.get_namespace())
self.display_trajectory_publisher = rospy.Publisher(rospy.get_namespace() + 'move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
if self.is_gripper_present:
gripper_group_name = "hand"
self.gripper_group = moveit_commander.MoveGroupCommander(gripper_group_name, ns=rospy.get_namespace())
rospy.loginfo("Initializing node in namespace " + rospy.get_namespace())
def reach_named_position(self, target):
arm_group = self.arm_group
# Going to one of those targets
rospy.loginfo("Going to named target " + target)
# Set the target
arm_group.set_named_target(target)
# Plan the trajectory
planned_path1 = arm_group.plan()
# Execute the trajectory and block while it's not finished
arm_group.execute(planned_path1, wait=True)
def reach_joint_angles(self, tolerance):
arm_group = self.arm_group
# Get the current joint positions
joint_positions = arm_group.get_current_joint_values()
rospy.loginfo("Printing current joint positions before movement :")
for p in joint_positions: rospy.loginfo(p)
# Set the goal joint tolerance
self.arm_group.set_goal_joint_tolerance(tolerance)
# Set the joint target configuration
joint_positions[0] = 0
joint_positions[1] = -pi/4
joint_positions[2] = 0
joint_positions[3] = -pi/2
joint_positions[4] = 0
joint_positions[5] = pi/3
joint_positions[6] = 0
arm_group.set_joint_value_target(joint_positions)
# Plan and execute in one command
arm_group.go(wait=True)
# Show joint positions after movement
new_joint_positions = arm_group.get_current_joint_values()
rospy.loginfo("Printing current joint positions after movement :")
for p in new_joint_positions: rospy.loginfo(p)
def get_cartesian_pose(self):
arm_group = self.arm_group
# Get the current pose and display it
pose = arm_group.get_current_pose()
rospy.loginfo("Actual cartesian pose is : ")
rospy.loginfo(pose.pose)
return pose.pose
def reach_cartesian_pose(self, pose, tolerance, constraints):
arm_group = self.arm_group
# Allow some leeway in position (meters) and orientation (radians)
print("end_effector_link")
print(arm_group.get_end_effector_link())
arm_group.set_pose_reference_frame
gripper_group = self.gripper_group
print(arm_group.get_end_effector_link())
arm_group.set_end_effector_link("panda_TCP")
print(arm_group.get_end_effector_link())
# Set the tolerance
arm_group.set_goal_position_tolerance(tolerance)
# Set the trajectory constraint if one is specified
if constraints is not None:
#import pdb; pdb.set_trace()
arm_group.set_path_constraints(constraints)
# Get the current Cartesian Position
arm_group.set_pose_target(pose)
# Plan and execute
rospy.loginfo("Planning and going to the Cartesian Pose")
arm_group.go(wait=True)
def reach_gripper_position(self, relative_position):
gripper_group = self.gripper_group
# We only have to move this joint because all others are mimic!
gripper_joint1 = self.robot.get_joint("panda_finger_joint1")
gripper_joint2 = self.robot.get_joint("panda_finger_joint2")
gripper_max_absolute_pos = gripper_joint1.max_bound()
print gripper_max_absolute_pos
gripper_joint1.move(relative_position * gripper_max_absolute_pos, wait=True)
gripper_joint2.move(relative_position * gripper_max_absolute_pos, wait=True)
# crear comando para posiciones de la pizan
def send_gripper_closed_command_moveit(self):
gripper_joints_closed = [0.0, 0.0]
gripper_group = self.gripper_group
gripper_group.set_joint_value_target(gripper_joints_closed)
plan = gripper_group.plan()
gripper_group.execute(plan, wait=True)
def send_gripper_open_command_moveit(self):
gripper_joints_open = [0.04, 0.04]
gripper_group = self.gripper_group
gripper_group.set_joint_value_target(gripper_joints_open)
plan = gripper_group.plan()
gripper_group.execute(plan, wait=True)
def send_gripper_command(self):
pub = rospy.Publisher('/panda_hand_controller/command',
Float64MultiArray, queue_size=1)
msg = Float64MultiArray()
msg.layout.dim = [MultiArrayDimension('', 2, 1)]
for i in range(100):
msg.data = [0.04, 0.04]
pub.publish(msg)
rospy.sleep(1)
msg.data = [0.00, 0.00]
pub.publish(msg)
rospy.sleep(1)
def set_max_velocity_scaling_factor(self, value):
""" Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0,1]. """
if value > 0 and value <= 1:
self.arm_group.set_max_velocity_scaling_factor(0.5)
else:
raise MoveItCommanderException("Expected value in the range from 0 to 1 for scaling factor" )
def main():
#camara.listener()
example = ExampleMoveItTrajectories()
# HOME
example.reach_named_position("home")
#INTRODUCE HERE THE COLOR CHANGE MESSAGE
rospy.loginfo("PULSAR PARA COMENZAR PROGRAMA")
rospy.loginfo("DETECCION AZUL")
raw_input()
#PINZA ABIERTA
example.send_gripper_open_command_moveit()
#POSICIONAMIENTO CAMARA pieza1
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = 0.30
actual_pose.position.y = 0.6
actual_pose.position.z = 1.2
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#POSICIONAMIENTO GRASPING pieza 1
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = 0.30
actual_pose.position.y = 0.6
actual_pose.position.z = 1.05
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#PINZA CERRADA
example.send_gripper_closed_command_moveit()
#time.sleep(5)
#POSICIONAMIENTO PIEZA 1 ARRIBA
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = 0.3
actual_pose.position.y = 0.6
actual_pose.position.z = 1.2
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#POSICIONAMIENTO BASURA1
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = 0.5
actual_pose.position.y = 0.0
actual_pose.position.z = 1.4
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#PINZA ABIERTA
example.send_gripper_open_command_moveit()
# HOME
example.reach_named_position("home")
rospy.loginfo("CAMBIO DETECCION COLOR ROJO")
raw_input()
#POSICIONAMIENTO CAMARA pieza2
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = -0.30
actual_pose.position.y = 0.6
actual_pose.position.z = 1.2
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#POSICIONAMIENTO GRASPING pieza 2
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = -0.30
actual_pose.position.y = 0.6
actual_pose.position.z = 1.05
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#PINZA CERRADA
example.send_gripper_closed_command_moveit()
#time.sleep(5)
#POSICIONAMIENTO PIEZA 2 ARRIBA
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = -0.3
actual_pose.position.y = 0.6
actual_pose.position.z = 1.2
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#HOME
example.reach_named_position("home")
#POSICIONAMIENTO BASURA pieza 2
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = 0.5
actual_pose.position.y = 0.0
actual_pose.position.z = 1.4
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#SOLTAR PIEZA 2
example.send_gripper_open_command_moveit()
# HOME
example.reach_named_position("home")
rospy.loginfo("DETECCION MORADO")
raw_input()
#POSICIONAMIENTO CAMARA pieza3
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = -0.30
actual_pose.position.y = 0.7
actual_pose.position.z = 1.2
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#POSICIONAMIENTO GRASPING pieza 3
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = -0.30
actual_pose.position.y = 0.7
actual_pose.position.z = 1.05
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#PINZA CERRADA
example.send_gripper_closed_command_moveit()
#time.sleep(5)
#POSICIONAMIENTO PIEZA 3 ARRIBA
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = -0.3
actual_pose.position.y = 0.7
actual_pose.position.z = 1.2
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#HOME
example.reach_named_position("home")
#POSICIONAMIENTO BASURA pieza 3
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = -0.5
actual_pose.position.y = 0.0
actual_pose.position.z = 1.4
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#SOLTAR PIEZA 3
example.send_gripper_open_command_moveit()
# HOME
example.reach_named_position("home")
rospy.loginfo("DETECCION VERDE")
raw_input()
#POSICIONAMIENTO CAMARA pieza4
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = 0.30
actual_pose.position.y = 0.7
actual_pose.position.z = 1.2
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#POSICIONAMIENTO GRASPING pieza 4
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = 0.30
actual_pose.position.y = 0.7
actual_pose.position.z = 1.05
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#PINZA CERRADA
example.send_gripper_closed_command_moveit()
#time.sleep(5)
#POSICIONAMIENTO PIEZA 4 ARRIBA
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = 0.3
actual_pose.position.y = 0.7
actual_pose.position.z = 1.2
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#HOME
example.reach_named_position("home")
#POSICIONAMIENTO BASURA pieza 4
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = -0.5
actual_pose.position.y = 0.0
actual_pose.position.z = 1.4
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#SOLTAR PIEZA 4
example.send_gripper_open_command_moveit()
# HOME
example.reach_named_position("home")
#example.send_gripper_command()
#rospy.loginfo("Press any key to open the gripper sub example")
#raw_input()
#example.reach_gripper_position(0)
#rospy.loginfo("Press any key to bring the gripper to half-closed sub example")
#raw_input()
#example.reach_gripper_position(1.0)
#rospy.loginfo("Press any key to start Named Target Vertical sub example")
# raw_input()
# example.reach_named_position("up")
# rospy.loginfo("Press any key to start Reach Joint Angles sub example")
# raw_input()
# example.reach_joint_angles(tolerance=0.01) #rad
#rospy.loginfo("Press any key to start Reach Cartesian Pose sub example")
#raw_input()
#actual_pose = example.get_cartesian_pose()
#actual_pose.position.x = 0.0
#actual_pose.position.y = -1
#actual_pose.position.z = 1.02
#example.reach_cartesian_pose(pose=actual_pose, tolerance=0.01, constraints=None)
if example.is_gripper_present:
rospy.loginfo("Press any key to start Reach Cartesian Pose With Constraints sub example")
raw_input()
# Get actual pose
actual_pose = example.get_cartesian_pose()
actual_pose.position.z -= 0.005
# Orientation constraint (we want the end effector to stay the same orientation)
constraints = moveit_msgs.msg.Constraints()
orientation_constraint = moveit_msgs.msg.OrientationConstraint()
orientation_constraint.orientation = actual_pose.orientation
constraints.orientation_constraints.append(orientation_constraint)
# Send the goal
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.01, constraints=constraints)
if __name__ == '__main__':
main()
Camara.py:
from __future__ import print_function
import rospy
import cv2
import numpy as np
from std_msgs.msg import String
from sensor_msgs.msg import Image
from geometry_msgs.msg import Point
from cv_bridge import CvBridge
bridge = CvBridge()
pub=None
lowerb = np.array ([0 , 100 , 100])
upperb = np.array ([0 , 255 , 255])
def callback_color(data):
global lowerb, upperb
if data.data == "red":
lowerb[0]=0
upperb[0]=10
pass
elif data.data == "green":
lowerb[0]=40
upperb[0]=80
pass
elif data.data == "blue":
lowerb[0]=120
upperb[0]=130
pass
elif data.data == "purple":
lowerb[0]=140
upperb[0]=180
pass
def callback(data):
cv_image = bridge.imgmsg_to_cv2(data, desired_encoding='passthrough')
cv_image_hsv = cv2.cvtColor (cv_image , cv2.COLOR_BGR2HSV)
mask = cv2.inRange (cv_image_hsv , lowerb , upperb)
rows, cols =np.where (mask!=0)
point=Point()
if rows.shape[0]:
point.x=np.mean(rows)
point.y=np.mean(cols)
pub.publish(point)
print(point)
cv2.imshow("bgr", cv_image)
cv2.imshow("mask", mask)
cv2.waitKey(1)
def listener():
global pub
rospy.init_node('listener', anonymous=True)
pub = rospy.Publisher('posicion_objeto', Point, queue_size=10)
rospy.Subscriber("/panda/camera1/image_raw", Image, callback)
rospy.Subscriber("/detect_object_color", String, callback_color)
rospy.spin()
if __name__ == '__main__':
listener()
任何提示或建议将不胜感激。提前致谢
你可以为此写一个发布者。
在您的单元中定义发布者
color_pub = rospy.Publisher("/detect_object_color", String, queue_size=10)
并使用您要发布的颜色名称编写发布命令，whenever/wherever。
color_pub.publish("purple")
试图在我的 ROS 项目中配置基本的颜色检测，但我陷入了困境： 我有两个 python 脚本，其中“programa.py”是我的主要机器人程序，“camara.py”是我的 openCV 颜色检测脚本。
现在，当 camara.py 为 运行 时，我可以更改发布字符串消息的颜色检测：
rostopic pub -1 /detect_object_color std_msgs/String "purple"
我想以某种方式包括当此脚本为 运行（主函数的第 4 行）时从我的“programa.py”发送此消息的选项。
这些都是程序代码： programa.py
import sys
import tf
import time
import rospy
import camara
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
import cv2
from math import pi
from std_srvs.srv import Empty
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
class ExampleMoveItTrajectories(object):
"""ExampleMoveItTrajectories"""
def __init__(self):
# Initialize the node
super(ExampleMoveItTrajectories, self).__init__()
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('example_move_it_trajectories')
self.is_gripper_present = rospy.get_param(rospy.get_namespace() + "is_gripper_present", True)
# Create the MoveItInterface necessary objects
arm_group_name = "arm"
self.robot = moveit_commander.RobotCommander("robot_description")
print self.robot.get_group_names()
self.scene = moveit_commander.PlanningSceneInterface(ns=rospy.get_namespace())
self.arm_group = moveit_commander.MoveGroupCommander(arm_group_name, ns=rospy.get_namespace())
self.display_trajectory_publisher = rospy.Publisher(rospy.get_namespace() + 'move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
if self.is_gripper_present:
gripper_group_name = "hand"
self.gripper_group = moveit_commander.MoveGroupCommander(gripper_group_name, ns=rospy.get_namespace())
rospy.loginfo("Initializing node in namespace " + rospy.get_namespace())
def reach_named_position(self, target):
arm_group = self.arm_group
# Going to one of those targets
rospy.loginfo("Going to named target " + target)
# Set the target
arm_group.set_named_target(target)
# Plan the trajectory
planned_path1 = arm_group.plan()
# Execute the trajectory and block while it's not finished
arm_group.execute(planned_path1, wait=True)
def reach_joint_angles(self, tolerance):
arm_group = self.arm_group
# Get the current joint positions
joint_positions = arm_group.get_current_joint_values()
rospy.loginfo("Printing current joint positions before movement :")
for p in joint_positions: rospy.loginfo(p)
# Set the goal joint tolerance
self.arm_group.set_goal_joint_tolerance(tolerance)
# Set the joint target configuration
joint_positions[0] = 0
joint_positions[1] = -pi/4
joint_positions[2] = 0
joint_positions[3] = -pi/2
joint_positions[4] = 0
joint_positions[5] = pi/3
joint_positions[6] = 0
arm_group.set_joint_value_target(joint_positions)
# Plan and execute in one command
arm_group.go(wait=True)
# Show joint positions after movement
new_joint_positions = arm_group.get_current_joint_values()
rospy.loginfo("Printing current joint positions after movement :")
for p in new_joint_positions: rospy.loginfo(p)
def get_cartesian_pose(self):
arm_group = self.arm_group
# Get the current pose and display it
pose = arm_group.get_current_pose()
rospy.loginfo("Actual cartesian pose is : ")
rospy.loginfo(pose.pose)
return pose.pose
def reach_cartesian_pose(self, pose, tolerance, constraints):
arm_group = self.arm_group
# Allow some leeway in position (meters) and orientation (radians)
print("end_effector_link")
print(arm_group.get_end_effector_link())
arm_group.set_pose_reference_frame
gripper_group = self.gripper_group
print(arm_group.get_end_effector_link())
arm_group.set_end_effector_link("panda_TCP")
print(arm_group.get_end_effector_link())
# Set the tolerance
arm_group.set_goal_position_tolerance(tolerance)
# Set the trajectory constraint if one is specified
if constraints is not None:
#import pdb; pdb.set_trace()
arm_group.set_path_constraints(constraints)
# Get the current Cartesian Position
arm_group.set_pose_target(pose)
# Plan and execute
rospy.loginfo("Planning and going to the Cartesian Pose")
arm_group.go(wait=True)
def reach_gripper_position(self, relative_position):
gripper_group = self.gripper_group
# We only have to move this joint because all others are mimic!
gripper_joint1 = self.robot.get_joint("panda_finger_joint1")
gripper_joint2 = self.robot.get_joint("panda_finger_joint2")
gripper_max_absolute_pos = gripper_joint1.max_bound()
print gripper_max_absolute_pos
gripper_joint1.move(relative_position * gripper_max_absolute_pos, wait=True)
gripper_joint2.move(relative_position * gripper_max_absolute_pos, wait=True)
# crear comando para posiciones de la pizan
def send_gripper_closed_command_moveit(self):
gripper_joints_closed = [0.0, 0.0]
gripper_group = self.gripper_group
gripper_group.set_joint_value_target(gripper_joints_closed)
plan = gripper_group.plan()
gripper_group.execute(plan, wait=True)
def send_gripper_open_command_moveit(self):
gripper_joints_open = [0.04, 0.04]
gripper_group = self.gripper_group
gripper_group.set_joint_value_target(gripper_joints_open)
plan = gripper_group.plan()
gripper_group.execute(plan, wait=True)
def send_gripper_command(self):
pub = rospy.Publisher('/panda_hand_controller/command',
Float64MultiArray, queue_size=1)
msg = Float64MultiArray()
msg.layout.dim = [MultiArrayDimension('', 2, 1)]
for i in range(100):
msg.data = [0.04, 0.04]
pub.publish(msg)
rospy.sleep(1)
msg.data = [0.00, 0.00]
pub.publish(msg)
rospy.sleep(1)
def set_max_velocity_scaling_factor(self, value):
""" Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0,1]. """
if value > 0 and value <= 1:
self.arm_group.set_max_velocity_scaling_factor(0.5)
else:
raise MoveItCommanderException("Expected value in the range from 0 to 1 for scaling factor" )
def main():
#camara.listener()
example = ExampleMoveItTrajectories()
# HOME
example.reach_named_position("home")
#INTRODUCE HERE THE COLOR CHANGE MESSAGE
rospy.loginfo("PULSAR PARA COMENZAR PROGRAMA")
rospy.loginfo("DETECCION AZUL")
raw_input()
#PINZA ABIERTA
example.send_gripper_open_command_moveit()
#POSICIONAMIENTO CAMARA pieza1
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = 0.30
actual_pose.position.y = 0.6
actual_pose.position.z = 1.2
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#POSICIONAMIENTO GRASPING pieza 1
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = 0.30
actual_pose.position.y = 0.6
actual_pose.position.z = 1.05
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#PINZA CERRADA
example.send_gripper_closed_command_moveit()
#time.sleep(5)
#POSICIONAMIENTO PIEZA 1 ARRIBA
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = 0.3
actual_pose.position.y = 0.6
actual_pose.position.z = 1.2
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#POSICIONAMIENTO BASURA1
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = 0.5
actual_pose.position.y = 0.0
actual_pose.position.z = 1.4
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#PINZA ABIERTA
example.send_gripper_open_command_moveit()
# HOME
example.reach_named_position("home")
rospy.loginfo("CAMBIO DETECCION COLOR ROJO")
raw_input()
#POSICIONAMIENTO CAMARA pieza2
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = -0.30
actual_pose.position.y = 0.6
actual_pose.position.z = 1.2
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#POSICIONAMIENTO GRASPING pieza 2
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = -0.30
actual_pose.position.y = 0.6
actual_pose.position.z = 1.05
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#PINZA CERRADA
example.send_gripper_closed_command_moveit()
#time.sleep(5)
#POSICIONAMIENTO PIEZA 2 ARRIBA
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = -0.3
actual_pose.position.y = 0.6
actual_pose.position.z = 1.2
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#HOME
example.reach_named_position("home")
#POSICIONAMIENTO BASURA pieza 2
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = 0.5
actual_pose.position.y = 0.0
actual_pose.position.z = 1.4
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#SOLTAR PIEZA 2
example.send_gripper_open_command_moveit()
# HOME
example.reach_named_position("home")
rospy.loginfo("DETECCION MORADO")
raw_input()
#POSICIONAMIENTO CAMARA pieza3
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = -0.30
actual_pose.position.y = 0.7
actual_pose.position.z = 1.2
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#POSICIONAMIENTO GRASPING pieza 3
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = -0.30
actual_pose.position.y = 0.7
actual_pose.position.z = 1.05
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#PINZA CERRADA
example.send_gripper_closed_command_moveit()
#time.sleep(5)
#POSICIONAMIENTO PIEZA 3 ARRIBA
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = -0.3
actual_pose.position.y = 0.7
actual_pose.position.z = 1.2
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#HOME
example.reach_named_position("home")
#POSICIONAMIENTO BASURA pieza 3
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = -0.5
actual_pose.position.y = 0.0
actual_pose.position.z = 1.4
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#SOLTAR PIEZA 3
example.send_gripper_open_command_moveit()
# HOME
example.reach_named_position("home")
rospy.loginfo("DETECCION VERDE")
raw_input()
#POSICIONAMIENTO CAMARA pieza4
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = 0.30
actual_pose.position.y = 0.7
actual_pose.position.z = 1.2
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#POSICIONAMIENTO GRASPING pieza 4
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = 0.30
actual_pose.position.y = 0.7
actual_pose.position.z = 1.05
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#PINZA CERRADA
example.send_gripper_closed_command_moveit()
#time.sleep(5)
#POSICIONAMIENTO PIEZA 4 ARRIBA
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = 0.3
actual_pose.position.y = 0.7
actual_pose.position.z = 1.2
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#HOME
example.reach_named_position("home")
#POSICIONAMIENTO BASURA pieza 4
actual_pose = example.get_cartesian_pose()
roll= 0.0
pitch = pi
yaw = -pi/2
quaternion= tf.transformations.quaternion_from_euler(roll, pitch, yaw)
actual_pose.position.x = -0.5
actual_pose.position.y = 0.0
actual_pose.position.z = 1.4
actual_pose.orientation.x = quaternion[0]
actual_pose.orientation.y = quaternion[1]
actual_pose.orientation.z = quaternion[2]
actual_pose.orientation.w = quaternion[3]
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.001, constraints=None)
#SOLTAR PIEZA 4
example.send_gripper_open_command_moveit()
# HOME
example.reach_named_position("home")
#example.send_gripper_command()
#rospy.loginfo("Press any key to open the gripper sub example")
#raw_input()
#example.reach_gripper_position(0)
#rospy.loginfo("Press any key to bring the gripper to half-closed sub example")
#raw_input()
#example.reach_gripper_position(1.0)
#rospy.loginfo("Press any key to start Named Target Vertical sub example")
# raw_input()
# example.reach_named_position("up")
# rospy.loginfo("Press any key to start Reach Joint Angles sub example")
# raw_input()
# example.reach_joint_angles(tolerance=0.01) #rad
#rospy.loginfo("Press any key to start Reach Cartesian Pose sub example")
#raw_input()
#actual_pose = example.get_cartesian_pose()
#actual_pose.position.x = 0.0
#actual_pose.position.y = -1
#actual_pose.position.z = 1.02
#example.reach_cartesian_pose(pose=actual_pose, tolerance=0.01, constraints=None)
if example.is_gripper_present:
rospy.loginfo("Press any key to start Reach Cartesian Pose With Constraints sub example")
raw_input()
# Get actual pose
actual_pose = example.get_cartesian_pose()
actual_pose.position.z -= 0.005
# Orientation constraint (we want the end effector to stay the same orientation)
constraints = moveit_msgs.msg.Constraints()
orientation_constraint = moveit_msgs.msg.OrientationConstraint()
orientation_constraint.orientation = actual_pose.orientation
constraints.orientation_constraints.append(orientation_constraint)
# Send the goal
example.reach_cartesian_pose(pose=actual_pose, tolerance=0.01, constraints=constraints)
if __name__ == '__main__':
main()
Camara.py:
from __future__ import print_function
import rospy
import cv2
import numpy as np
from std_msgs.msg import String
from sensor_msgs.msg import Image
from geometry_msgs.msg import Point
from cv_bridge import CvBridge
bridge = CvBridge()
pub=None
lowerb = np.array ([0 , 100 , 100])
upperb = np.array ([0 , 255 , 255])
def callback_color(data):
global lowerb, upperb
if data.data == "red":
lowerb[0]=0
upperb[0]=10
pass
elif data.data == "green":
lowerb[0]=40
upperb[0]=80
pass
elif data.data == "blue":
lowerb[0]=120
upperb[0]=130
pass
elif data.data == "purple":
lowerb[0]=140
upperb[0]=180
pass
def callback(data):
cv_image = bridge.imgmsg_to_cv2(data, desired_encoding='passthrough')
cv_image_hsv = cv2.cvtColor (cv_image , cv2.COLOR_BGR2HSV)
mask = cv2.inRange (cv_image_hsv , lowerb , upperb)
rows, cols =np.where (mask!=0)
point=Point()
if rows.shape[0]:
point.x=np.mean(rows)
point.y=np.mean(cols)
pub.publish(point)
print(point)
cv2.imshow("bgr", cv_image)
cv2.imshow("mask", mask)
cv2.waitKey(1)
def listener():
global pub
rospy.init_node('listener', anonymous=True)
pub = rospy.Publisher('posicion_objeto', Point, queue_size=10)
rospy.Subscriber("/panda/camera1/image_raw", Image, callback)
rospy.Subscriber("/detect_object_color", String, callback_color)
rospy.spin()
if __name__ == '__main__':
listener()
任何提示或建议将不胜感激。提前致谢
你可以为此写一个发布者。
在您的单元中定义发布者
color_pub = rospy.Publisher("/detect_object_color", String, queue_size=10)
并使用您要发布的颜色名称编写发布命令，whenever/wherever。
color_pub.publish("purple")