def setLeft(pub, val):
buff = ApplyJointEffort()
buff.effort = val
start_time = rospy.Time(0, 0)
end_time = rospy.Time(0.01, 0)
buff.joint_name = "dd_robot::left_wheel_hinge"
pub(buff.joint_name, buff.effort, start_time, end_time)
def setRotation(pub, val):
start_t = rospy.Time(0, 0)
end_t = rospy.Time(0.01, 0)
buff = ApplyJointEffort()
buff.effort = val
buff.joint_name = 'dd_robot::left_wheel_hinge'
pub(buff.joint_name, -buff.effort, start_t, end_t)
buff.joint_name = 'dd_robot::right_wheel_hinge'
pub(buff.joint_name, buff.effort, start_t, end_t)
def talker():
# pub = rospy.Publisher('/gazebo/apply_joint_effort', ApplyJointEffort, queue_size=10)
rospy.init_node('dd_ctrl', anonymous=True)
pub = rospy.ServiceProxy('/gazebo/apply_joint_effort',ApplyJointEffort)
rate = rospy.Rate(10) # 10hz
i = 0
buff = ApplyJointEffort()
buff.joint_name = "dd_robot::left_wheel_hinge"
buff.effort = 1.0
buff.start_time = 0.0
buff.duration = 0.0
start_time = rospy.Time(0,0)
end_time = rospy.Time(0.01,0)
while not rospy.is_shutdown():
if i > 30:
buff.effort = -buff.effort
i = 0
i = i + 1
# ['joint_name', 'effort', 'start_time', 'duration']
#pub( buff.joint_name, buff.effort, buff.start_time, buff.duration )
#pub( buff.joint_name, buff.effort, 0.0, 0.1 )
buff.joint_name = "dd_robot::left_wheel_hinge"
pub(buff.joint_name, buff.effort, start_time, end_time)
buff.joint_name = "dd_robot::right_wheel_hinge"
pub(buff.joint_name, buff.effort, start_time, end_time)
rospy.loginfo(buff)
rate.sleep()
def setRot(pub, val):
buff = ApplyJointEffort()
buff.effort = val * 10
start_time = rospy.Time(0, 0)
end_time = rospy.Time(0.01, 0)
buff.joint_name = "cc_robot::left_wheel_hinge"
pub(buff.joint_name, buff.effort, start_time, end_time)
buff.joint_name = "cc_robot::right_wheel_hinge"
pub(buff.joint_name, -buff.effort, start_time, end_time)
def setRot(pub, val, direction):
buff = ApplyJointEffort()
buff.effort = val
start_time = rospy.Time(0, 0)
end_time = rospy.Time(0.01, 0)
if direction == "l":
buff.joint_name = "dd_robot::left_wheel_hinge"
pub(buff.joint_name, buff.effort, start_time, end_time)
if direction == "r":
buff.joint_name = "dd_robot::right_wheel_hinge"
pub(buff.joint_name, -buff.effort, start_time, end_time)
def setRot(pub, msg):
global speed
d = msg
buff = ApplyJointEffort()
start_time = rospy.Time(0, 0)
end_time = rospy.Time(0.01, 0)
if (d == "s"):
pub("dd_robot::left_wheel_hinge", 1 * speed, start_time, end_time)
pub("dd_robot::right_wheel_hinge", 1 * speed, start_time, end_time)
#pub("dd_robot::left_wheel_hinge", -1 * speed, start_time, end_time)
#pub("dd_robot::right_wheel_hinge", -1 * speed, start_time, end_time)
elif (d == "w"):
pub("dd_robot::left_wheel_hinge", -1 * speed, start_time, end_time)
pub("dd_robot::right_wheel_hinge", -1 * speed, start_time, end_time)
#pub("dd_robot::left_wheel_hinge", 1 * speed, start_time, end_time)
#pub("dd_robot::right_wheel_hinge", 1 * speed, start_time, end_time)
elif (d == "d"):
pub("dd_robot::left_wheel_hinge", 1 * turn, start_time, end_time)
pub("dd_robot::right_wheel_hinge", -1 * turn, start_time, end_time)
#pub("dd_robot::left_wheel_hinge", -1 * speed, start_time, end_time)
#pub("dd_robot::right_wheel_hinge", 1 * speed, start_time, end_time)
elif (d == "a"):
pub("dd_robot::left_wheel_hinge", -1 * turn, start_time, end_time)
pub("dd_robot::right_wheel_hinge", 1 * turn, start_time, end_time)
#pub("dd_robot::left_wheel_hinge", 1 * speed, start_time, end_time)
#pub("dd_robot::right_wheel_hinge", -1 * speed, start_time, end_time)
else:
pass
def move():
rospy.init_node('swing')
value = ApplyJointEffort()
count = 0
while not rospy.is_shutdown():
value.joint_name = 'joint1'
if count % 2 == 0:
value.effort = 10
value.start_time = rospy.Time(0)
value.duration = rospy.Time(1)
else:
value.effort = 0
value.start_time = rospy.Time(0)
value.duration = rospy.Time(1)
rospy.wait_for_service('/gazebo/apply_joint_effort')
JointEffort = rospy.ServiceProxy('/gazebo/apply_joint_effort',
ApplyJointEffort)
resp1 = JointEffort('joint1', value.effort, value.start_time,
value.duration)
rospy.sleep(2.5)
#!/usr/bin/env python3
import rospy
from gazebo_msgs.srv import ApplyJointEffort
msg_topic = '/gazebo/apply_joint_effort'
joint_left = 'dd_roobot::left_wheel_hinge'
joint_right = 'dd_roobot::right_wheel_hinge'
msg_topic_feedabck = '/gazebo/get_joint_properties'
rospy.init_node('dd_ctrl', anonymous=True)
pub = rospy.ServiceProxy(msg_topic, ApplyJointEffort)
pub_feedback = ApplyJointEffort()
effort = 1.0
start_time = rospy.Time(0,0)
f = 0.5
T = 1/f
end_time = rospy.Time(1,0)
rate = rospy.Rate(f)
while True:
effort = -effort
pub(joint_left, effort, start_time, end_time)
val = pub_feedback(joint_left)
print(val.rate)
rate.sleep()
