ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
Hi @pmuthu2s,
I've tested your code and it worked after I select the right frame_id
.
In my case I had to change the frame_id
to hokuyo_laser_link
but just because my robot doesn't have a link named arm_link_5
.
You have to check if your robot have a link named arm_link_5
. You can see the "tf tree" with rosrun tf view_frames
. That command will generate a frames.pdf
file that you can use to see the link names of your robot.
Once you open RViz with rosrun rviz rviz
, you also have to select the right "Fixed Frame". In my case the frame was world
. You could select "any" link found on frames.pdf
.
Since I didn't have a topic named /arm_1/arm_controller/cartesian_velocity_command
that publishes message of type TwistStamped
, I just created a topic named /tutorial
that publishes std_msgs/String
and subscribed to that topic (just for simplicity) on the TrajectoryInteractiveMarkers
class.
I've created that topic with: rostopic pub /tutorial std_msgs/String "data: 'Hi'" -r 2
I've also created a video that shows the steps mentioned here.
Since the marker is only published when a message from /arm_1/arm_controller/cartesian_velocity_command
is received, you have to make sure that topic is publishing.
You can do that with any of the commands below:
rostopic echo /arm_1/arm_controller/cartesian_velocity_command
rostopic hz /arm_1/arm_controller/cartesian_velocity_command
The modified code I've used is shown below:
#! /usr/bin/env python
import rospy
from visualization_msgs.msg import Marker
from geometry_msgs.msg import TwistStamped, Pose, Point, Vector3, Quaternion
from std_msgs.msg import Header, ColorRGBA, String
class TrajectoryInteractiveMarkers:
def __init__(self):
self.count = 0
# rospy.Subscriber("/arm_1/arm_controller/cartesian_velocity_command",TwistStamped, self.event_in_cb)
self.marker_publisher = rospy.Publisher('visualization_marker', Marker, queue_size=5)
rospy.Subscriber("/tutorial", String, self.event_in_cb)
rospy.sleep(0.5)
def event_in_cb(self,msg):
self.waypoints = msg
self.a = [1, 1, 1]
#self.a = list()
#self.a.append(self.waypoints.twist.linear.x)
#self.a.append(self.waypoints.twist.linear.y)
#self.a.append(self.waypoints.twist.linear.z)
self.show_text_in_rviz()
def show_text_in_rviz(self):
self.marker = Marker()
self.marker = Marker(
type=Marker.SPHERE,
id=0,
lifetime=rospy.Duration(1000),
pose=Pose(Point(self.a[0]/10**5,self.a[1]/10**5,self.a[2]/10**5), Quaternion(0, 0, 0, 1)),
scale=Vector3(0.05, 0.05, 0.05),
header=Header(frame_id='hokuyo_laser_link'),
color=ColorRGBA(0.0, 2.0, 0.0, 0.8))
self.count+=1
self.marker.id = self.count
self.marker_publisher.publish(self.marker)
rospy.loginfo('msg published')
if __name__ == '__main__':
rospy.init_node("trajectory_interactive_markers_node", anonymous=True)
trajectory_interactive_markers = TrajectoryInteractiveMarkers()
rospy.sleep(0.5)
rospy.spin()
If you still have any question, just take a look at the video.
Cheers.