Can't run /start_traj_mode.

[duytranvan6904]

I want to run a demo python file which move robot from current position into another position. Here is all of my code:

Click to expand

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node

from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from sensor_msgs.msg import JointState
from std_srvs.srv import Trigger

class SimpleTrajectoryClient(Node):
    def __init__(self):
        super().__init__('simple_trajectory_client')
        self._action_client = ActionClient(self, FollowJointTrajectory, '/follow_joint_trajectory')
        self.current_joint_state = None
        self.create_subscription(JointState, '/joint_states', self.joint_state_callback, 10)

        # MotoROS2 trajectory mode services
        self._start_traj_client = self.create_client(Trigger, '/start_traj_mode')
        self._stop_traj_client = self.create_client(Trigger, '/stop_traj_mode')

    def joint_state_callback(self, msg: JointState):
        self.current_joint_state = msg

    def send_simple_trajectory(self):
        self.get_logger().info('Starting trajectory execution...')
        # Wait for joint state
        while self.current_joint_state is None:
            self.get_logger().info('Waiting for /joint_states data...')
            rclpy.spin_once(self, timeout_sec=1.0)

        self.get_logger().info(f'Received joint states: {self.current_joint_state.name}')

        # Wait for start_traj_mode service
        self.get_logger().info('Waiting for /start_traj_mode service to be ready...')
        while not self._start_traj_client.wait_for_service(timeout_sec=5.0):
            self.get_logger().info('Service /start_traj_mode not ready, waiting...')

        # Call start_traj_mode
        request_start = Trigger.Request()
        self.get_logger().info('Sending start_traj_mode request...')
        future_start = self._start_traj_client.call_async(request_start)
        rclpy.spin_until_future_complete(self, future_start)

        if future_start.result() is None or not future_start.result().success:
            msg = '' if future_start.result() is None else future_start.result().message
            self.get_logger().error(f'start_traj_mode service failed: {msg}')
            rclpy.shutdown()
            return
        self.get_logger().info('start_traj_mode service succeeded.')

        # Build trajectory
        start_positions = list(self.current_joint_state.position)
        joint_names = list(self.current_joint_state.name)

        if len(start_positions) == 0 or len(joint_names) == 0 or len(start_positions) != len(joint_names):
            self.get_logger().error('Invalid JointState (empty or size mismatch).')
            self.call_stop_traj_mode()
            rclpy.shutdown()
            return

        # Small move near current pose: +0.1 rad on joint 1, -0.1 rad on joint 3 (if exist)
        target_positions = start_positions.copy()
        delta = 0.1  # ~5.7 deg
        try:
            target_positions[0] = start_positions[0] + delta
            if len(target_positions) >= 3:
                target_positions[2] = start_positions[2] - delta
        except IndexError:
            self.get_logger().error('Joint list unexpectedly short.')
            self.call_stop_traj_mode()
            rclpy.shutdown()
            return

        # Compute motion duration from max joint delta with a conservative speed (rad/s)
        # Ensures smooth movement and avoids exceeding controller limits.
        desired_speed = 0.2  # rad/s
        max_delta = max(abs(tp - sp) for tp, sp in zip(target_positions, start_positions))
        duration = max(2.0, max_delta / desired_speed)  # at least 2s

        goal_msg = FollowJointTrajectory.Goal()
        goal_msg.trajectory.joint_names = joint_names
        goal_msg.trajectory.header.stamp = self.get_clock().now().to_msg()

        # Point A (t = 0)
        point_a = JointTrajectoryPoint()
        point_a.positions = start_positions
        point_a.velocities = [0.0] * len(joint_names)  # REQUIRED by MotoROS2
        point_a.time_from_start.sec = 0
        point_a.time_from_start.nanosec = 0
        goal_msg.trajectory.points.append(point_a)

        # Point B (t = duration)
        point_b = JointTrajectoryPoint()
        point_b.positions = target_positions
        point_b.velocities = [0.0] * len(joint_names)  # REQUIRED by MotoROS2
        point_b.time_from_start.sec = int(duration)
        point_b.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
        goal_msg.trajectory.points.append(point_b)

        # Send goal
        self.get_logger().info('Waiting for action server /follow_joint_trajectory...')
        self._action_client.wait_for_server()
        self.get_logger().info('Action server ready, sending goal...')
        send_goal_future = self._action_client.send_goal_async(goal_msg)
        send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle or not goal_handle.accepted:
            self.get_logger().error('Goal rejected!')
            self.call_stop_traj_mode()
            rclpy.shutdown()
            return

        self.get_logger().info('Goal accepted, waiting for result...')
        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(self.get_result_callback)

    def get_result_callback(self, future):
        self.get_logger().info('Motion complete.')
        self.call_stop_traj_mode()
        rclpy.shutdown()

    def call_stop_traj_mode(self):
        self.get_logger().info('Waiting for /stop_traj_mode service to be ready...')
        while not self._stop_traj_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Service /stop_traj_mode not ready, waiting...')

        request_stop = Trigger.Request()
        self.get_logger().info('Sending stop_traj_mode request...')
        future_stop = self._stop_traj_client.call_async(request_stop)
        rclpy.spin_until_future_complete(self, future_stop)

        if future_stop.result() is not None:
            if future_stop.result().success:
                self.get_logger().info('stop_traj_mode service succeeded.')
            else:
                self.get_logger().error(f'stop_traj_mode service failed: {future_stop.result().message}')
        else:
            self.get_logger().error('stop_traj_mode service call failed (no response).')

def main(args=None):
    rclpy.init(args=args)
    node = SimpleTrajectoryClient()
    node.send_simple_trajectory()
    rclpy.spin(node)  # rclpy.shutdown() is called in callbacks

if __name__ == '__main__':
    main()

I also have turned on Servo. However, it got stucked in 'Service /start_traj_mode not ready, waiting...'

This is my RobotStatus, I saw motion_possible = 0 but don't know how to fix this.

[ted-miller]

Hello @duytranvan6904

Please run the debug log client and post the output here. This should show us what is happening during /start_traj_mode.

Also, make sure to source the motoros2_interfaces messages into your workspace.

[duytranvan6904]

I run the debug_listener.py but it didn't give any respones as in picture:

Click to expand

To make sure if i have connected successfully with robot and source the motoros2_interaces, I have run another simple task which read the joint_status and it have run like as picture bellow:

Click to expand

I don't know the reason why but debug log client also didn't give me respones.

[duytranvan6904]

Hello,

Here are the updates based on your suggestions:

• I have checked and confirmed that the firewall status is inactive. However, when I run the debug_listener.py file, it still hangs and gives no response.

• Regarding the incompatible QoS warning: I changed the QoS setting of joint_state in the .yaml file from sensor_data to default. After that, the warning disappeared. However, I still cannot set start_traj_mode.

• About your third suggestion (checking the network/switch configuration): I’m not sure what specific aspects I should verify. Could you please advise me on what exactly I need to check (e.g., broadcast filtering, switch settings, or something else)?

Thank you for your continued support.

[duytranvan6904]

I tried running the following command: ros2 service call /start_traj_mode motoros2_interfaces/srv/StartTrajMode
When the controller is in REMOTE mode, I received the following response:

requester: making request: motoros2_interfaces.srv.StartTrajMode_Request()

response:
motoros2_interfaces.srv.StartTrajMode_Response(
  result_code=motoros2_interfaces.msg.MotionReadyEnum(value=1),
  message=''
)

After that, I checked /robot_status. I observed that the reported status keeps toggling back and forth between two states, even though the controller stays in REMOTE mode:
State A:

mode:
  val: 2
e_stopped:
  val: 0
drives_powered:
  val: 1
motion_possible:
  val: 1

State B:

mode:
  val: 1
e_stopped:
  val: 0
drives_powered:
  val: 0
motion_possible:
  val: 0

Is this behavior expected? Or does it indicate that something is not configured correctly in my setup (for example, controller settings, MotoROS2 configuration, or network)?

[ted-miller]

About your third suggestion (checking the network/switch configuration): I’m not sure what specific aspects I should verify. Could you please advise me on what exactly I need to check (e.g., broadcast filtering, switch settings, or something else)?

I can't advise you on any specifics. Each switch manufacturer has their own proprietary settings for such things.

For what it's worth, most standard switches don't allow you to adjust these settings. It's only on more advanced "managed" switches.

For this testing, I recommend connecting an ethernet cable directly from your PC into the robot controller's LAN port. This will rule out the possibility of any switch configuration.

I checked /robot_status. I observed that the reported status keeps toggling back and forth between two states, even though the controller stays in REMOTE mode

That is definitely not the expected behavior. Are there multiple robots on your network? If so, you should change the domain id to avoid conflicts.

For this testing, it's best to connect directly from your PC to the robot.

Please also verify that only one instance of MotoROS2 is installed on your robot controller. While in Maintenance Mode, touch MotoPlus APL > File List. There should be only one application listed.

[duytranvan6904]

I tried to connect with another robot and when I switched to Remote mode, I ran additional command: ros2 service call /yaskawa/start_traj_mode motoros2_interfaces/srv/StartTrajMode and Robot_status that was predefined at State A. Now the robot is able to run according to my code. Thank you for your help.